Initial commit with upstream sources

Change-Id: Ie9460111f21fc955102fd8732a0173b2d0499a4a

729 files changed, 117216 insertions, 0 deletions

diff --git a/core/arch/arm/arm.mk b/core/arch/arm/arm.mk
new file mode 100644
index 0000000..4d8c6f0
--- /dev/null
+++ b/core/arch/arm/arm.mk
@@ -0,0 +1,157 @@
+CFG_LTC_OPTEE_THREAD ?= y
+# Size of emulated TrustZone protected SRAM, 360 kB.
+# Only applicable when paging is enabled.
+CFG_CORE_TZSRAM_EMUL_SIZE ?= 368640
+CFG_LPAE_ADDR_SPACE_SIZE ?= (1ull << 32)
+
+ifeq ($(CFG_ARM64_core),y)
+CFG_KERN_LINKER_FORMAT ?= elf64-littleaarch64
+CFG_KERN_LINKER_ARCH ?= aarch64
+endif
+ifeq ($(CFG_ARM32_core),y)
+CFG_KERN_LINKER_FORMAT ?= elf32-littlearm
+CFG_KERN_LINKER_ARCH ?= arm
+endif
+
+ifeq ($(CFG_TA_FLOAT_SUPPORT),y)
+# Use hard-float for floating point support in user TAs instead of
+# soft-float
+CFG_WITH_VFP ?= y
+ifeq ($(CFG_ARM64_core),y)
+# AArch64 has no fallback to soft-float
+$(call force,CFG_WITH_VFP,y)
+endif
+ifeq ($(CFG_WITH_VFP),y)
+platform-hard-float-enabled := y
+endif
+endif
+
+ifeq ($(CFG_WITH_PAGER),y)
+ifeq ($(CFG_CORE_SANITIZE_KADDRESS),y)
+$(error Error: CFG_CORE_SANITIZE_KADDRESS not compatible with CFG_WITH_PAGER)
+endif
+endif
+
+ifeq ($(CFG_ARM32_core),y)
+# Configration directive related to ARMv7 optee boot arguments.
+# CFG_PAGEABLE_ADDR: if defined, forces pageable data physical address.
+# CFG_NS_ENTRY_ADDR: if defined, forces NS World physical entry address.
+# CFG_DT_ADDR:       if defined, forces Device Tree data physical address.
+endif
+
+core-platform-cppflags	+= -I$(arch-dir)/include
+core-platform-subdirs += \
+	$(addprefix $(arch-dir)/, kernel mm tee pta) $(platform-dir)
+
+ifneq ($(CFG_WITH_ARM_TRUSTED_FW),y)
+core-platform-subdirs += $(arch-dir)/sm
+endif
+
+arm64-platform-cppflags += -DARM64=1 -D__LP64__=1
+arm32-platform-cppflags += -DARM32=1 -D__ILP32__=1
+
+platform-cflags-generic ?= -g -ffunction-sections -fdata-sections -pipe
+platform-aflags-generic ?= -g -pipe
+
+arm32-platform-cflags-no-hard-float ?= -mno-apcs-float -mfloat-abi=soft
+arm32-platform-cflags-hard-float ?= -mfloat-abi=hard -funsafe-math-optimizations
+arm32-platform-cflags-generic ?= -mthumb -mthumb-interwork \
+			-fno-short-enums -fno-common -mno-unaligned-access
+arm32-platform-aflags-no-hard-float ?=
+
+arm64-platform-cflags-no-hard-float ?= -mgeneral-regs-only
+arm64-platform-cflags-hard-float ?=
+arm64-platform-cflags-generic ?= -mstrict-align
+
+ifeq ($(DEBUG),1)
+platform-cflags-optimization ?=  -O0
+else
+platform-cflags-optimization ?=  -Os
+endif
+
+platform-cflags-debug-info ?= -g3
+platform-aflags-debug-info ?=
+
+core-platform-cflags += $(platform-cflags-optimization)
+core-platform-cflags += $(platform-cflags-generic)
+core-platform-cflags += $(platform-cflags-debug-info)
+
+core-platform-aflags += $(platform-aflags-generic)
+core-platform-aflags += $(platform-aflags-debug-info)
+
+ifeq ($(CFG_ARM64_core),y)
+arch-bits-core := 64
+core-platform-cppflags += $(arm64-platform-cppflags)
+core-platform-cflags += $(arm64-platform-cflags)
+core-platform-cflags += $(arm64-platform-cflags-generic)
+core-platform-cflags += $(arm64-platform-cflags-no-hard-float)
+core-platform-aflags += $(arm64-platform-aflags)
+else
+arch-bits-core := 32
+core-platform-cppflags += $(arm32-platform-cppflags)
+core-platform-cflags += $(arm32-platform-cflags)
+core-platform-cflags += $(arm32-platform-cflags-no-hard-float)
+ifeq ($(CFG_CORE_UNWIND),y)
+core-platform-cflags += -funwind-tables
+endif
+core-platform-cflags += $(arm32-platform-cflags-generic)
+core-platform-aflags += $(core_arm32-platform-aflags)
+core-platform-aflags += $(arm32-platform-aflags)
+endif
+
+ifneq ($(filter ta_arm32,$(ta-targets)),)
+# Variables for ta-target/sm "ta_arm32"
+CFG_ARM32_ta_arm32 := y
+arch-bits-ta_arm32 := 32
+ta_arm32-platform-cppflags += $(arm32-platform-cppflags)
+ta_arm32-platform-cflags += $(arm32-platform-cflags)
+ta_arm32-platform-cflags += $(platform-cflags-optimization)
+ta_arm32-platform-cflags += $(platform-cflags-debug-info)
+ta_arm32-platform-cflags += -fpie
+ta_arm32-platform-cflags += $(arm32-platform-cflags-generic)
+ifeq ($(platform-hard-float-enabled),y)
+ta_arm32-platform-cflags += $(arm32-platform-cflags-hard-float)
+else
+ta_arm32-platform-cflags += $(arm32-platform-cflags-no-hard-float)
+endif
+ta_arm32-platform-aflags += $(platform-aflags-debug-info)
+ta_arm32-platform-aflags += $(arm32-platform-aflags)
+
+ta-mk-file-export-vars-ta_arm32 += CFG_ARM32_ta_arm32
+ta-mk-file-export-vars-ta_arm32 += ta_arm32-platform-cppflags
+ta-mk-file-export-vars-ta_arm32 += ta_arm32-platform-cflags
+ta-mk-file-export-vars-ta_arm32 += ta_arm32-platform-aflags
+
+ta-mk-file-export-add-ta_arm32 += CROSS_COMPILE32 ?= $$(CROSS_COMPILE)_nl_
+ta-mk-file-export-add-ta_arm32 += CROSS_COMPILE_ta_arm32 ?= $$(CROSS_COMPILE32)_nl_
+endif
+
+ifneq ($(filter ta_arm64,$(ta-targets)),)
+# Variables for ta-target/sm "ta_arm64"
+CFG_ARM64_ta_arm64 := y
+arch-bits-ta_arm64 := 64
+ta_arm64-platform-cppflags += $(arm64-platform-cppflags)
+ta_arm64-platform-cflags += $(arm64-platform-cflags)
+ta_arm64-platform-cflags += $(platform-cflags-optimization)
+ta_arm64-platform-cflags += $(platform-cflags-debug-info)
+ta_arm64-platform-cflags += -fpie
+ta_arm64-platform-cflags += $(arm64-platform-cflags-generic)
+ifeq ($(platform-hard-float-enabled),y)
+ta_arm64-platform-cflags += $(arm64-platform-cflags-hard-float)
+else
+ta_arm64-platform-cflags += $(arm64-platform-cflags-no-hard-float)
+endif
+ta_arm64-platform-aflags += $(platform-aflags-debug-info)
+ta_arm64-platform-aflags += $(arm64-platform-aflags)
+
+ta-mk-file-export-vars-ta_arm64 += CFG_ARM64_ta_arm64
+ta-mk-file-export-vars-ta_arm64 += ta_arm64-platform-cppflags
+ta-mk-file-export-vars-ta_arm64 += ta_arm64-platform-cflags
+ta-mk-file-export-vars-ta_arm64 += ta_arm64-platform-aflags
+
+ta-mk-file-export-add-ta_arm64 += CROSS_COMPILE64 ?= $$(CROSS_COMPILE)_nl_
+ta-mk-file-export-add-ta_arm64 += CROSS_COMPILE_ta_arm64 ?= $$(CROSS_COMPILE64)_nl_
+endif
+
+# Set cross compiler prefix for each submodule
+$(foreach sm, core $(ta-targets), $(eval CROSS_COMPILE_$(sm) ?= $(CROSS_COMPILE$(arch-bits-$(sm)))))
diff --git a/core/arch/arm/include/arm.h b/core/arch/arm/include/arm.h
new file mode 100644
index 0000000..a644dd4
--- /dev/null
+++ b/core/arch/arm/include/arm.h
@@ -0,0 +1,65 @@
+/*
+ * 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.
+ */
+#ifndef ARM_H
+#define ARM_H
+
+
+#define MPIDR_CPU_MASK		0xff
+#define MPIDR_CLUSTER_SHIFT	8
+#define MPIDR_CLUSTER_MASK	(0xff << MPIDR_CLUSTER_SHIFT)
+
+#define ARM32_CPSR_MODE_MASK	0x1f
+#define ARM32_CPSR_MODE_USR	0x10
+#define ARM32_CPSR_MODE_FIQ	0x11
+#define ARM32_CPSR_MODE_IRQ	0x12
+#define ARM32_CPSR_MODE_SVC	0x13
+#define ARM32_CPSR_MODE_MON	0x16
+#define ARM32_CPSR_MODE_ABT	0x17
+#define ARM32_CPSR_MODE_UND	0x1b
+#define ARM32_CPSR_MODE_SYS	0x1f
+
+#define ARM32_CPSR_T		(1 << 5)
+#define ARM32_CPSR_F_SHIFT	6
+#define ARM32_CPSR_F		(1 << 6)
+#define ARM32_CPSR_I		(1 << 7)
+#define ARM32_CPSR_A		(1 << 8)
+#define ARM32_CPSR_E		(1 << 9)
+#define ARM32_CPSR_FIA		(ARM32_CPSR_F | ARM32_CPSR_I | ARM32_CPSR_A)
+#define ARM32_CPSR_IT_MASK	(ARM32_CPSR_IT_MASK1 | ARM32_CPSR_IT_MASK2)
+#define ARM32_CPSR_IT_MASK1	0x06000000
+#define ARM32_CPSR_IT_MASK2	0x0000fc00
+
+
+#ifdef ARM32
+#include <arm32.h>
+#endif
+
+#ifdef ARM64
+#include <arm64.h>
+#endif
+
+#endif /*ARM_H*/
diff --git a/core/arch/arm/include/arm32.h b/core/arch/arm/include/arm32.h
new file mode 100644
index 0000000..822ff95
--- /dev/null
+++ b/core/arch/arm/include/arm32.h
@@ -0,0 +1,606 @@
+/*
+ * Copyright (c) 2016, Linaro Limited
+ * 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.
+ */
+
+#ifndef ARM32_H
+#define ARM32_H
+
+#include <sys/cdefs.h>
+#include <stdint.h>
+#include <util.h>
+
+#define CPSR_MODE_MASK	ARM32_CPSR_MODE_MASK
+#define CPSR_MODE_USR	ARM32_CPSR_MODE_USR
+#define CPSR_MODE_FIQ	ARM32_CPSR_MODE_FIQ
+#define CPSR_MODE_IRQ	ARM32_CPSR_MODE_IRQ
+#define CPSR_MODE_SVC	ARM32_CPSR_MODE_SVC
+#define CPSR_MODE_MON	ARM32_CPSR_MODE_MON
+#define CPSR_MODE_ABT	ARM32_CPSR_MODE_ABT
+#define CPSR_MODE_UND	ARM32_CPSR_MODE_UND
+#define CPSR_MODE_SYS	ARM32_CPSR_MODE_SYS
+
+#define CPSR_T		ARM32_CPSR_T
+#define CPSR_F_SHIFT	ARM32_CPSR_F_SHIFT
+#define CPSR_F		ARM32_CPSR_F
+#define CPSR_I		ARM32_CPSR_I
+#define CPSR_A		ARM32_CPSR_A
+#define CPSR_FIA	ARM32_CPSR_FIA
+#define CPSR_IT_MASK	ARM32_CPSR_IT_MASK
+#define CPSR_IT_MASK1	ARM32_CPSR_IT_MASK1
+#define CPSR_IT_MASK2	ARM32_CPSR_IT_MASK2
+
+#define SCR_NS		BIT32(0)
+#define SCR_IRQ		BIT32(1)
+#define SCR_FIQ		BIT32(2)
+#define SCR_EA		BIT32(3)
+#define SCR_FW		BIT32(4)
+#define SCR_AW		BIT32(5)
+#define SCR_NET		BIT32(6)
+#define SCR_SCD		BIT32(7)
+#define SCR_HCE		BIT32(8)
+#define SCR_SIF		BIT32(9)
+
+#define SCTLR_M		BIT32(0)
+#define SCTLR_A		BIT32(1)
+#define SCTLR_C		BIT32(2)
+#define SCTLR_CP15BEN	BIT32(5)
+#define SCTLR_SW	BIT32(10)
+#define SCTLR_Z		BIT32(11)
+#define SCTLR_I		BIT32(12)
+#define SCTLR_V		BIT32(13)
+#define SCTLR_RR	BIT32(14)
+#define SCTLR_HA	BIT32(17)
+#define SCTLR_WXN	BIT32(19)
+#define SCTLR_UWXN	BIT32(20)
+#define SCTLR_FI	BIT32(21)
+#define SCTLR_VE	BIT32(24)
+#define SCTLR_EE	BIT32(25)
+#define SCTLR_NMFI	BIT32(26)
+#define SCTLR_TRE	BIT32(28)
+#define SCTLR_AFE	BIT32(29)
+#define SCTLR_TE	BIT32(30)
+
+#define ACTLR_SMP	BIT32(6)
+#define ACTLR_DODMBS	BIT32(10)
+#define ACTLR_L2RADIS	BIT32(11)
+#define ACTLR_L1RADIS	BIT32(12)
+#define ACTLR_L1PCTL	BIT32(13)
+#define ACTLR_DDVM	BIT32(15)
+#define ACTLR_DDI	BIT32(28)
+
+#define NSACR_CP10	BIT32(10)
+#define NSACR_CP11	BIT32(11)
+#define NSACR_NSD32DIS	BIT32(14)
+#define NSACR_NSASEDIS	BIT32(15)
+#define NSACR_NS_L2ERR	BIT32(17)
+#define NSACR_NS_SMP	BIT32(18)
+
+#define CPACR_ASEDIS	BIT32(31)
+#define CPACR_D32DIS	BIT32(30)
+#define CPACR_CP(co_proc, access)	SHIFT_U32((access), ((co_proc) * 2))
+#define CPACR_CP_ACCESS_DENIED		0x0
+#define CPACR_CP_ACCESS_PL1_ONLY	0x1
+#define CPACR_CP_ACCESS_FULL		0x3
+
+
+#define DACR_DOMAIN(num, perm)		SHIFT_U32((perm), ((num) * 2))
+#define DACR_DOMAIN_PERM_NO_ACCESS	0x0
+#define DACR_DOMAIN_PERM_CLIENT		0x1
+#define DACR_DOMAIN_PERM_MANAGER	0x3
+
+#define PAR_F			BIT32(0)
+#define PAR_SS			BIT32(1)
+#define PAR_LPAE		BIT32(11)
+#define PAR_PA_SHIFT		12
+#define PAR32_PA_MASK		(BIT32(20) - 1)
+#define PAR64_PA_MASK		(BIT64(28) - 1)
+
+/*
+ * TTBCR has different register layout if LPAE is enabled or not.
+ * TTBCR.EAE == 0 => LPAE is not enabled
+ * TTBCR.EAE == 1 => LPAE is enabled
+ */
+#define TTBCR_EAE	BIT32(31)
+
+/* When TTBCR.EAE == 0 */
+#define TTBCR_PD0	BIT32(4)
+#define TTBCR_PD1	BIT32(5)
+
+/* When TTBCR.EAE == 1 */
+#define TTBCR_T0SZ_SHIFT	0
+#define TTBCR_EPD0		BIT32(7)
+#define TTBCR_IRGN0_SHIFT	8
+#define TTBCR_ORGN0_SHIFT	10
+#define TTBCR_SH0_SHIFT		12
+#define TTBCR_T1SZ_SHIFT	16
+#define TTBCR_A1		BIT32(22)
+#define TTBCR_EPD1		BIT32(23)
+#define TTBCR_IRGN1_SHIFT	24
+#define TTBCR_ORGN1_SHIFT	26
+#define TTBCR_SH1_SHIFT		28
+
+/* Normal memory, Inner/Outer Non-cacheable */
+#define TTBCR_XRGNX_NC		0x0
+/* Normal memory, Inner/Outer Write-Back Write-Allocate Cacheable */
+#define TTBCR_XRGNX_WB		0x1
+/* Normal memory, Inner/Outer Write-Through Cacheable */
+#define TTBCR_XRGNX_WT		0x2
+/* Normal memory, Inner/Outer Write-Back no Write-Allocate Cacheable */
+#define TTBCR_XRGNX_WBWA	0x3
+
+/* Non-shareable */
+#define TTBCR_SHX_NSH		0x0
+/* Outer Shareable */
+#define TTBCR_SHX_OSH		0x2
+/* Inner Shareable */
+#define TTBCR_SHX_ISH		0x3
+
+#define TTBR_ASID_MASK		0xff
+#define TTBR_ASID_SHIFT		48
+
+
+#define FSR_LPAE		BIT32(9)
+#define FSR_WNR			BIT32(11)
+
+/* Valid if FSR.LPAE is 1 */
+#define FSR_STATUS_MASK		(BIT32(6) - 1)
+
+/* Valid if FSR.LPAE is 0 */
+#define FSR_FS_MASK		(BIT32(10) | (BIT32(4) - 1))
+
+#ifndef ASM
+static inline uint32_t read_mpidr(void)
+{
+	uint32_t mpidr;
+
+	asm volatile ("mrc	p15, 0, %[mpidr], c0, c0, 5"
+			: [mpidr] "=r" (mpidr)
+	);
+
+	return mpidr;
+}
+
+static inline uint32_t read_sctlr(void)
+{
+	uint32_t sctlr;
+
+	asm volatile ("mrc	p15, 0, %[sctlr], c1, c0, 0"
+			: [sctlr] "=r" (sctlr)
+	);
+
+	return sctlr;
+}
+
+static inline void write_sctlr(uint32_t sctlr)
+{
+	asm volatile ("mcr	p15, 0, %[sctlr], c1, c0, 0"
+			: : [sctlr] "r" (sctlr)
+	);
+}
+
+static inline uint32_t read_cpacr(void)
+{
+	uint32_t cpacr;
+
+	asm volatile ("mrc	p15, 0, %[cpacr], c1, c0, 2"
+			: [cpacr] "=r" (cpacr)
+	);
+
+	return cpacr;
+}
+
+static inline void write_cpacr(uint32_t cpacr)
+{
+	asm volatile ("mcr	p15, 0, %[cpacr], c1, c0, 2"
+			: : [cpacr] "r" (cpacr)
+	);
+}
+
+static inline void write_ttbr0(uint32_t ttbr0)
+{
+	asm volatile ("mcr	p15, 0, %[ttbr0], c2, c0, 0"
+			: : [ttbr0] "r" (ttbr0)
+	);
+}
+
+static inline void write_ttbr0_64bit(uint64_t ttbr0)
+{
+	asm volatile ("mcrr	p15, 0, %Q[ttbr0], %R[ttbr0], c2"
+			: : [ttbr0] "r" (ttbr0)
+	);
+}
+
+static inline uint32_t read_ttbr0(void)
+{
+	uint32_t ttbr0;
+
+	asm volatile ("mrc	p15, 0, %[ttbr0], c2, c0, 0"
+			: [ttbr0] "=r" (ttbr0)
+	);
+
+	return ttbr0;
+}
+
+static inline uint64_t read_ttbr0_64bit(void)
+{
+	uint64_t ttbr0;
+
+	asm volatile ("mrrc	p15, 0, %Q[ttbr0], %R[ttbr0], c2"
+			: [ttbr0] "=r" (ttbr0)
+	);
+
+	return ttbr0;
+}
+
+static inline void write_ttbr1(uint32_t ttbr1)
+{
+	asm volatile ("mcr	p15, 0, %[ttbr1], c2, c0, 1"
+			: : [ttbr1] "r" (ttbr1)
+	);
+}
+
+static inline void write_ttbr1_64bit(uint64_t ttbr1)
+{
+	asm volatile ("mcrr	p15, 1, %Q[ttbr1], %R[ttbr1], c2"
+			: : [ttbr1] "r" (ttbr1)
+	);
+}
+
+static inline uint32_t read_ttbr1(void)
+{
+	uint32_t ttbr1;
+
+	asm volatile ("mrc	p15, 0, %[ttbr1], c2, c0, 1"
+			: [ttbr1] "=r" (ttbr1)
+	);
+
+	return ttbr1;
+}
+
+
+static inline void write_ttbcr(uint32_t ttbcr)
+{
+	asm volatile ("mcr	p15, 0, %[ttbcr], c2, c0, 2"
+			: : [ttbcr] "r" (ttbcr)
+	);
+}
+
+static inline uint32_t read_ttbcr(void)
+{
+	uint32_t ttbcr;
+
+	asm volatile ("mrc	p15, 0, %[ttbcr], c2, c0, 2"
+			: [ttbcr] "=r" (ttbcr)
+	);
+
+	return ttbcr;
+}
+
+static inline void write_dacr(uint32_t dacr)
+{
+	asm volatile ("mcr	p15, 0, %[dacr], c3, c0, 0"
+			: : [dacr] "r" (dacr)
+	);
+}
+
+static inline uint32_t read_ifar(void)
+{
+	uint32_t ifar;
+
+	asm volatile ("mrc	p15, 0, %[ifar], c6, c0, 2"
+			: [ifar] "=r" (ifar)
+	);
+
+	return ifar;
+}
+
+static inline uint32_t read_dfar(void)
+{
+	uint32_t dfar;
+
+	asm volatile ("mrc	p15, 0, %[dfar], c6, c0, 0"
+			: [dfar] "=r" (dfar)
+	);
+
+	return dfar;
+}
+
+static inline uint32_t read_dfsr(void)
+{
+	uint32_t dfsr;
+
+	asm volatile ("mrc	p15, 0, %[dfsr], c5, c0, 0"
+			: [dfsr] "=r" (dfsr)
+	);
+
+	return dfsr;
+}
+
+static inline uint32_t read_ifsr(void)
+{
+	uint32_t ifsr;
+
+	asm volatile ("mrc	p15, 0, %[ifsr], c5, c0, 1"
+			: [ifsr] "=r" (ifsr)
+	);
+
+	return ifsr;
+}
+
+static inline void write_scr(uint32_t scr)
+{
+	asm volatile ("mcr	p15, 0, %[scr], c1, c1, 0"
+			: : [scr] "r" (scr)
+	);
+}
+
+static inline void isb(void)
+{
+	asm volatile ("isb");
+}
+
+static inline void dsb(void)
+{
+	asm volatile ("dsb");
+}
+
+static inline void dmb(void)
+{
+	asm volatile ("dmb");
+}
+
+static inline void sev(void)
+{
+	asm volatile ("sev");
+}
+
+static inline void wfe(void)
+{
+	asm volatile ("wfe");
+}
+
+/* Address translate privileged write translation (current state secure PL1) */
+static inline void write_ats1cpw(uint32_t va)
+{
+	asm volatile ("mcr	p15, 0, %0, c7, c8, 1" : : "r" (va));
+}
+
+static inline void write_ats1cpr(uint32_t va)
+{
+	asm volatile ("mcr	p15, 0, %0, c7, c8, 0" : : "r" (va));
+}
+
+static inline void write_ats1cpuw(uint32_t va)
+{
+	asm volatile ("mcr	p15, 0, %0, c7, c8, 3" : : "r" (va));
+}
+
+static inline void write_ats1cpur(uint32_t va)
+{
+	asm volatile ("mcr	p15, 0, %0, c7, c8, 2" : : "r" (va));
+}
+
+static inline uint32_t read_par32(void)
+{
+	uint32_t val;
+
+	asm volatile ("mrc	p15, 0, %0, c7, c4, 0" : "=r" (val));
+	return val;
+}
+
+#ifdef CFG_WITH_LPAE
+static inline uint64_t read_par64(void)
+{
+	uint64_t val;
+
+	asm volatile ("mrrc	p15, 0, %Q0, %R0, c7" : "=r" (val));
+	return val;
+}
+#endif
+
+static inline void write_mair0(uint32_t mair0)
+{
+	asm volatile ("mcr	p15, 0, %[mair0], c10, c2, 0"
+			: : [mair0] "r" (mair0)
+	);
+}
+
+static inline void write_prrr(uint32_t prrr)
+{
+	/*
+	 * Same physical register as MAIR0.
+	 *
+	 * When an implementation includes the Large Physical Address
+	 * Extension, and address translation is using the Long-descriptor
+	 * translation table formats, MAIR0 replaces the PRRR
+	 */
+	write_mair0(prrr);
+}
+
+static inline void write_mair1(uint32_t mair1)
+{
+	asm volatile ("mcr	p15, 0, %[mair1], c10, c2, 1"
+			: : [mair1] "r" (mair1)
+	);
+}
+
+static inline void write_nmrr(uint32_t nmrr)
+{
+	/*
+	 * Same physical register as MAIR1.
+	 *
+	 * When an implementation includes the Large Physical Address
+	 * Extension, and address translation is using the Long-descriptor
+	 * translation table formats, MAIR1 replaces the NMRR
+	 */
+	write_mair1(nmrr);
+}
+
+static inline uint32_t read_contextidr(void)
+{
+	uint32_t contextidr;
+
+	asm volatile ("mrc	p15, 0, %[contextidr], c13, c0, 1"
+			: [contextidr] "=r" (contextidr)
+	);
+
+	return contextidr;
+}
+
+static inline void write_contextidr(uint32_t contextidr)
+{
+	asm volatile ("mcr	p15, 0, %[contextidr], c13, c0, 1"
+			: : [contextidr] "r" (contextidr)
+	);
+}
+
+static inline uint32_t read_cpsr(void)
+{
+	uint32_t cpsr;
+
+	asm volatile ("mrs	%[cpsr], cpsr"
+			: [cpsr] "=r" (cpsr)
+	);
+	return cpsr;
+}
+
+static inline void write_cpsr(uint32_t cpsr)
+{
+	asm volatile ("msr	cpsr_fsxc, %[cpsr]"
+			: : [cpsr] "r" (cpsr)
+	);
+}
+
+static inline uint32_t read_spsr(void)
+{
+	uint32_t spsr;
+
+	asm volatile ("mrs	%[spsr], spsr"
+			: [spsr] "=r" (spsr)
+	);
+	return spsr;
+}
+
+static inline uint32_t read_actlr(void)
+{
+	uint32_t actlr;
+
+	asm volatile ("mrc	p15, 0, %[actlr], c1, c0, 1"
+			: [actlr] "=r" (actlr)
+	);
+
+	return actlr;
+}
+
+static inline void write_actlr(uint32_t actlr)
+{
+	asm volatile ("mcr	p15, 0, %[actlr], c1, c0, 1"
+			: : [actlr] "r" (actlr)
+	);
+}
+
+static inline uint32_t read_nsacr(void)
+{
+	uint32_t nsacr;
+
+	asm volatile ("mrc	p15, 0, %[nsacr], c1, c1, 2"
+			: [nsacr] "=r" (nsacr)
+	);
+
+	return nsacr;
+}
+
+static inline void write_nsacr(uint32_t nsacr)
+{
+	asm volatile ("mcr	p15, 0, %[nsacr], c1, c1, 2"
+			: : [nsacr] "r" (nsacr)
+	);
+}
+
+static inline uint64_t read_cntpct(void)
+{
+	uint64_t val;
+
+	asm volatile("mrrc p15, 0, %Q0, %R0, c14" : "=r" (val));
+	return val;
+}
+
+static inline uint32_t read_cntfrq(void)
+{
+	uint32_t frq;
+
+	asm volatile("mrc p15, 0, %0, c14, c0, 0" : "=r" (frq));
+	return frq;
+}
+
+static inline void write_cntfrq(uint32_t frq)
+{
+	asm volatile("mcr p15, 0, %0, c14, c0, 0" : : "r" (frq));
+}
+
+static __always_inline uint32_t read_pc(void)
+{
+	uint32_t val;
+
+	asm volatile ("adr %0, ." : "=r" (val));
+	return val;
+}
+
+static __always_inline uint32_t read_sp(void)
+{
+	uint32_t val;
+
+	asm volatile ("mov %0, sp" : "=r" (val));
+	return val;
+}
+
+static __always_inline uint32_t read_lr(void)
+{
+	uint32_t val;
+
+	asm volatile ("mov %0, lr" : "=r" (val));
+	return val;
+}
+
+static __always_inline uint32_t read_fp(void)
+{
+	uint32_t val;
+
+	asm volatile ("mov %0, fp" : "=r" (val));
+	return val;
+}
+
+static __always_inline uint32_t read_r7(void)
+{
+	uint32_t val;
+
+	asm volatile ("mov %0, r7" : "=r" (val));
+	return val;
+}
+#endif /*ASM*/
+
+#endif /*ARM32_H*/
diff --git a/core/arch/arm/include/arm32_macros.S b/core/arch/arm/include/arm32_macros.S
new file mode 100644
index 0000000..0a4ca28
--- /dev/null
+++ b/core/arch/arm/include/arm32_macros.S
@@ -0,0 +1,215 @@
+/*
+ * 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.
+ */
+
+	/* Please keep them sorted based on the CRn register */
+	.macro read_mpidr reg
+	mrc	p15, 0, \reg, c0, c0, 5
+	.endm
+
+	.macro read_sctlr reg
+	mrc	p15, 0, \reg, c1, c0, 0
+	.endm
+
+	.macro write_sctlr reg
+	mcr	p15, 0, \reg, c1, c0, 0
+	.endm
+
+	.macro write_actlr reg
+	mcr	p15, 0, \reg, c1, c0, 1
+	.endm
+
+	.macro read_actlr reg
+	mrc	p15, 0, \reg, c1, c0, 1
+	.endm
+
+	.macro write_cpacr reg
+	mcr	p15, 0, \reg, c1, c0, 2
+	.endm
+
+	.macro read_cpacr reg
+	mrc	p15, 0, \reg, c1, c0, 2
+	.endm
+
+	.macro read_scr reg
+	mrc	p15, 0, \reg, c1, c1, 0
+	.endm
+
+	.macro write_scr reg
+	mcr	p15, 0, \reg, c1, c1, 0
+	.endm
+
+	.macro write_nsacr reg
+	mcr	p15, 0, \reg, c1, c1, 2
+	.endm
+
+	.macro read_nsacr reg
+	mrc	p15, 0, \reg, c1, c1, 2
+	.endm
+
+	.macro write_ttbr0 reg
+	mcr	p15, 0, \reg, c2, c0, 0
+	.endm
+
+	.macro read_ttbr0 reg
+	mrc	p15, 0, \reg, c2, c0, 0
+	.endm
+
+	.macro write_ttbr1 reg
+	mcr	p15, 0, \reg, c2, c0, 1
+	.endm
+
+	.macro read_ttbr1 reg
+	mrc	p15, 0, \reg, c2, c0, 1
+	.endm
+
+	.macro write_ttbcr reg
+	mcr	p15, 0, \reg, c2, c0, 2
+	.endm
+
+	.macro read_ttbcr reg
+	mrc	p15, 0, \reg, c2, c0, 2
+	.endm
+
+
+	.macro write_dacr reg
+	mcr	p15, 0, \reg, c3, c0, 0
+	.endm
+
+	.macro read_dacr reg
+	mrc	p15, 0, \reg, c3, c0, 0
+	.endm
+
+	.macro read_dfsr reg
+	mrc	p15, 0, \reg, c5, c0, 0
+	.endm
+
+	.macro write_iciallu
+	/* Invalidate all instruction caches to PoU (register ignored) */
+	mcr	p15, 0, r0, c7, c5, 0
+	.endm
+
+	.macro write_icialluis
+	/*
+	 * Invalidate all instruction caches to PoU, Inner Shareable
+	 * (register ignored)
+	 */
+	mcr	p15, 0, r0, c7, c1, 0
+	.endm
+
+	.macro write_bpiall
+	/* Invalidate entire branch predictor array (register ignored) */
+	mcr	p15, 0, r0, c7, c5, 0
+	.endm
+
+	.macro write_bpiallis
+	/*
+	 * Invalidate entire branch predictor array, Inner Shareable
+	 * (register ignored)
+	 */
+	mcr	p15, 0, r0, c7, c1, 6
+	.endm
+
+	.macro write_tlbiall
+	/* Invalidate entire unified TLB (register ignored) */
+	mcr	p15, 0, r0, c8, c7, 0
+	.endm
+
+	.macro write_tlbiallis
+	/* Invalidate entire unified TLB Inner Sharable (register ignored) */
+	mcr	p15, 0, r0, c8, c3, 0
+	.endm
+
+	.macro write_tlbiasidis reg
+	/* Invalidate unified TLB by ASID Inner Sharable */
+	mcr	p15, 0, \reg, c8, c3, 2
+	.endm
+
+	.macro write_prrr reg
+	mcr	p15, 0, \reg, c10, c2, 0
+	.endm
+
+	.macro read_prrr reg
+	mrc	p15, 0, \reg, c10, c2, 0
+	.endm
+
+	.macro write_nmrr reg
+	mcr	p15, 0, \reg, c10, c2, 1
+	.endm
+
+	.macro read_nmrr reg
+	mrc	p15, 0, \reg, c10, c2, 1
+	.endm
+
+	.macro read_vbar reg
+	mrc	p15, 0, \reg, c12, c0, 0
+	.endm
+
+	.macro write_vbar reg
+	mcr	p15, 0, \reg, c12, c0, 0
+	.endm
+
+	.macro write_mvbar reg
+	mcr	p15, 0, \reg, c12, c0, 1
+	.endm
+
+	.macro read_mvbar reg
+	mrc	p15, 0, \reg, c12, c0, 1
+	.endm
+
+	.macro write_fcseidr reg
+	mcr	p15, 0, \reg, c13, c0, 0
+	.endm
+
+	.macro read_fcseidr reg
+	mrc	p15, 0, \reg, c13, c0, 0
+	.endm
+
+	.macro write_contextidr reg
+	mcr	p15, 0, \reg, c13, c0, 1
+	.endm
+
+	.macro read_contextidr reg
+	mrc	p15, 0, \reg, c13, c0, 1
+	.endm
+
+	.macro write_tpidruro reg
+	mcr	p15, 0, \reg, c13, c0, 3
+	.endm
+
+	.macro read_tpidruro reg
+	mrc	p15, 0, \reg, c13, c0, 3
+	.endm
+
+	.macro mov_imm reg, val
+		.if ((\val) & 0xffff0000) == 0
+			mov	\reg, #(\val)
+		.else
+			movw	\reg, #((\val) & 0xffff)
+			movt	\reg, #((\val) >> 16)
+		.endif
+	.endm
+
diff --git a/core/arch/arm/include/arm32_macros_cortex_a9.S b/core/arch/arm/include/arm32_macros_cortex_a9.S
new file mode 100644
index 0000000..57a2a10
--- /dev/null
+++ b/core/arch/arm/include/arm32_macros_cortex_a9.S
@@ -0,0 +1,44 @@
+/*
+ * Copyright (c) 2016, Freescale Semiconductor, Inc.
+ * All rights reserved.
+ *
+ * Peng Fan <peng.fan@nxp.com>
+ *
+ * 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.
+ */
+
+	.macro write_pcr reg
+	mcr  p15, 0, \reg, c15, c0, 0
+	.endm
+
+	.macro read_pcr reg
+	mrc  p15, 0, \reg, c15, c0, 0
+	.endm
+
+	.macro write_diag reg
+	mcr  p15, 0, \reg, c15, c0, 1
+	.endm
+
+	.macro read_diag reg
+	mrc  p15, 0, \reg, c15, c0, 1
+	.endm
diff --git a/core/arch/arm/include/arm64.h b/core/arch/arm/include/arm64.h
new file mode 100644
index 0000000..148b761
--- /dev/null
+++ b/core/arch/arm/include/arm64.h
@@ -0,0 +1,310 @@
+/*
+ * 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.
+ */
+#ifndef ARM64_H
+#define ARM64_H
+
+#include <sys/cdefs.h>
+#include <stdint.h>
+#include <util.h>
+
+#define SCTLR_M		BIT32(0)
+#define SCTLR_A		BIT32(1)
+#define SCTLR_C		BIT32(2)
+#define SCTLR_SA	BIT32(3)
+#define SCTLR_I		BIT32(12)
+
+#define TTBR_ASID_MASK		0xff
+#define TTBR_ASID_SHIFT		48
+
+#define CLIDR_LOUIS_SHIFT	21
+#define CLIDR_LOC_SHIFT		24
+#define CLIDR_FIELD_WIDTH	3
+
+#define CSSELR_LEVEL_SHIFT	1
+
+#define DAIFBIT_FIQ			BIT32(0)
+#define DAIFBIT_IRQ			BIT32(1)
+#define DAIFBIT_ABT			BIT32(2)
+#define DAIFBIT_DBG			BIT32(3)
+#define DAIFBIT_ALL			(DAIFBIT_FIQ | DAIFBIT_IRQ | \
+					 DAIFBIT_ABT | DAIFBIT_DBG)
+
+#define DAIF_F_SHIFT		6
+#define DAIF_F			BIT32(6)
+#define DAIF_I			BIT32(7)
+#define DAIF_A			BIT32(8)
+#define DAIF_D			BIT32(9)
+#define DAIF_AIF		(DAIF_A | DAIF_I | DAIF_F)
+
+#define SPSR_MODE_RW_SHIFT	4
+#define SPSR_MODE_RW_MASK	0x1
+#define SPSR_MODE_RW_64		0x0
+#define SPSR_MODE_RW_32		0x1
+
+#define SPSR_64_MODE_SP_SHIFT	0
+#define SPSR_64_MODE_SP_MASK	0x1
+#define SPSR_64_MODE_SP_EL0	0x0
+#define SPSR_64_MODE_SP_ELX	0x1
+
+#define SPSR_64_MODE_EL_SHIFT	2
+#define SPSR_64_MODE_EL_MASK	0x3
+#define SPSR_64_MODE_EL1	0x1
+#define SPSR_64_MODE_EL0	0x0
+
+#define SPSR_64_DAIF_SHIFT	6
+#define SPSR_64_DAIF_MASK	0xf
+
+#define SPSR_32_AIF_SHIFT	6
+#define SPSR_32_AIF_MASK	0x7
+
+#define SPSR_32_E_SHIFT		9
+#define SPSR_32_E_MASK		0x1
+#define SPSR_32_E_LITTLE	0x0
+#define SPSR_32_E_BIG		0x1
+
+#define SPSR_32_T_SHIFT		5
+#define SPSR_32_T_MASK		0x1
+#define SPSR_32_T_ARM		0x0
+#define SPSR_32_T_THUMB		0x1
+
+#define SPSR_32_MODE_SHIFT	0
+#define SPSR_32_MODE_MASK	0xf
+#define SPSR_32_MODE_USR	0x0
+
+
+#define SPSR_64(el, sp, daif)						\
+	(SPSR_MODE_RW_64 << SPSR_MODE_RW_SHIFT |			\
+	((el) & SPSR_64_MODE_EL_MASK) << SPSR_64_MODE_EL_SHIFT |	\
+	((sp) & SPSR_64_MODE_SP_MASK) << SPSR_64_MODE_SP_SHIFT |	\
+	((daif) & SPSR_64_DAIF_MASK) << SPSR_64_DAIF_SHIFT)
+
+#define SPSR_32(mode, isa, aif)						\
+	(SPSR_MODE_RW_32 << SPSR_MODE_RW_SHIFT |			\
+	SPSR_32_E_LITTLE << SPSR_32_E_SHIFT |				\
+	((mode) & SPSR_32_MODE_MASK) << SPSR_32_MODE_SHIFT |		\
+	((isa) & SPSR_32_T_MASK) << SPSR_32_T_SHIFT |			\
+	((aif) & SPSR_32_AIF_MASK) << SPSR_32_AIF_SHIFT)
+
+
+#define TCR_T0SZ_SHIFT		0
+#define TCR_EPD0		BIT32(7)
+#define TCR_IRGN0_SHIFT		8
+#define TCR_ORGN0_SHIFT		10
+#define TCR_SH0_SHIFT		12
+#define TCR_T1SZ_SHIFT		16
+#define TCR_A1			BIT32(22)
+#define TCR_EPD1		BIT32(23)
+#define TCR_IRGN1_SHIFT		24
+#define TCR_ORGN1_SHIFT		26
+#define TCR_SH1_SHIFT		28
+#define TCR_EL1_IPS_SHIFT	32
+#define TCR_TG1_4KB		SHIFT_U32(2, 30)
+#define TCR_RES1		BIT32(31)
+
+
+/* Normal memory, Inner/Outer Non-cacheable */
+#define TCR_XRGNX_NC		0x0
+/* Normal memory, Inner/Outer Write-Back Write-Allocate Cacheable */
+#define TCR_XRGNX_WB		0x1
+/* Normal memory, Inner/Outer Write-Through Cacheable */
+#define TCR_XRGNX_WT		0x2
+/* Normal memory, Inner/Outer Write-Back no Write-Allocate Cacheable */
+#define TCR_XRGNX_WBWA	0x3
+
+/* Non-shareable */
+#define TCR_SHX_NSH		0x0
+/* Outer Shareable */
+#define TCR_SHX_OSH		0x2
+/* Inner Shareable */
+#define TCR_SHX_ISH		0x3
+
+#define ESR_EC_SHIFT		26
+#define ESR_EC_MASK		0x3f
+
+#define ESR_EC_UNKNOWN		0x00
+#define ESR_EC_WFI		0x01
+#define ESR_EC_AARCH32_CP15_32	0x03
+#define ESR_EC_AARCH32_CP15_64	0x04
+#define ESR_EC_AARCH32_CP14_MR	0x05
+#define ESR_EC_AARCH32_CP14_LS	0x06
+#define ESR_EC_FP_ASIMD		0x07
+#define ESR_EC_AARCH32_CP10_ID	0x08
+#define ESR_EC_AARCH32_CP14_64	0x0c
+#define ESR_EC_ILLEGAL		0x0e
+#define ESR_EC_AARCH32_SVC	0x11
+#define ESR_EC_AARCH64_SVC	0x15
+#define ESR_EC_AARCH64_SYS	0x18
+#define ESR_EC_IABT_EL0		0x20
+#define ESR_EC_IABT_EL1		0x21
+#define ESR_EC_PC_ALIGN		0x22
+#define ESR_EC_DABT_EL0		0x24
+#define ESR_EC_DABT_EL1		0x25
+#define ESR_EC_SP_ALIGN		0x26
+#define ESR_EC_AARCH32_FP	0x28
+#define ESR_EC_AARCH64_FP	0x2c
+#define ESR_EC_SERROR		0x2f
+#define ESR_EC_BREAKPT_EL0	0x30
+#define ESR_EC_BREAKPT_EL1	0x31
+#define ESR_EC_SOFTSTP_EL0	0x32
+#define ESR_EC_SOFTSTP_EL1	0x33
+#define ESR_EC_WATCHPT_EL0	0x34
+#define ESR_EC_WATCHPT_EL1	0x35
+#define ESR_EC_AARCH32_BKPT	0x38
+#define ESR_EC_AARCH64_BRK	0x3c
+
+/* Combined defines for DFSC and IFSC */
+#define ESR_FSC_MASK		0x3f
+#define ESR_FSC_TRANS_L0	0x04
+#define ESR_FSC_TRANS_L1	0x05
+#define ESR_FSC_TRANS_L2	0x06
+#define ESR_FSC_TRANS_L3	0x07
+#define ESR_FSC_ACCF_L1		0x09
+#define ESR_FSC_ACCF_L2		0x0a
+#define ESR_FSC_ACCF_L3		0x0b
+#define ESR_FSC_PERMF_L1	0x0d
+#define ESR_FSC_PERMF_L2	0x0e
+#define ESR_FSC_PERMF_L3	0x0f
+#define ESR_FSC_ALIGN		0x21
+
+/* WnR for DABT and RES0 for IABT */
+#define ESR_ABT_WNR		BIT32(6)
+
+#define CPACR_EL1_FPEN_SHIFT	20
+#define CPACR_EL1_FPEN_MASK	0x3
+#define CPACR_EL1_FPEN_NONE	0x0
+#define CPACR_EL1_FPEN_EL1	0x1
+#define CPACR_EL1_FPEN_EL0EL1	0x3
+#define CPACR_EL1_FPEN(x)	((x) >> CPACR_EL1_FPEN_SHIFT \
+				      & CPACR_EL1_FPEN_MASK)
+
+
+#define PAR_F			BIT32(0)
+#define PAR_PA_SHIFT		12
+#define PAR_PA_MASK		(BIT64(36) - 1)
+
+#ifndef ASM
+static inline void isb(void)
+{
+	asm volatile ("isb");
+}
+
+static inline void dsb(void)
+{
+	asm volatile ("dsb sy");
+}
+
+static inline void write_at_s1e1r(uint64_t va)
+{
+	asm volatile ("at	S1E1R, %0" : : "r" (va));
+}
+
+static __always_inline uint64_t read_pc(void)
+{
+	uint64_t val;
+
+	asm volatile ("adr %0, ." : "=r" (val));
+	return val;
+}
+
+static __always_inline uint64_t read_fp(void)
+{
+	uint64_t val;
+
+	asm volatile ("mov %0, x29" : "=r" (val));
+	return val;
+}
+
+/*
+ * Templates for register read/write functions based on mrs/msr
+ */
+
+#define DEFINE_REG_READ_FUNC_(reg, type, asmreg)	\
+static inline type read_##reg(void)			\
+{							\
+	type val;					\
+							\
+	asm volatile("mrs %0, " #asmreg : "=r" (val));	\
+	return val;					\
+}
+
+#define DEFINE_REG_WRITE_FUNC_(reg, type, asmreg)		\
+static inline void write_##reg(type val)			\
+{								\
+	asm volatile("msr " #asmreg ", %0" : : "r" (val));	\
+}
+
+#define DEFINE_U32_REG_READ_FUNC(reg) \
+		DEFINE_REG_READ_FUNC_(reg, uint32_t, reg)
+
+#define DEFINE_U32_REG_WRITE_FUNC(reg) \
+		DEFINE_REG_WRITE_FUNC_(reg, uint32_t, reg)
+
+#define DEFINE_U32_REG_READWRITE_FUNCS(reg)	\
+		DEFINE_U32_REG_READ_FUNC(reg)	\
+		DEFINE_U32_REG_WRITE_FUNC(reg)
+
+#define DEFINE_U64_REG_READ_FUNC(reg) \
+		DEFINE_REG_READ_FUNC_(reg, uint64_t, reg)
+
+#define DEFINE_U64_REG_WRITE_FUNC(reg) \
+		DEFINE_REG_WRITE_FUNC_(reg, uint64_t, reg)
+
+#define DEFINE_U64_REG_READWRITE_FUNCS(reg)	\
+		DEFINE_U64_REG_READ_FUNC(reg)	\
+		DEFINE_U64_REG_WRITE_FUNC(reg)
+
+/*
+ * Define register access functions
+ */
+
+DEFINE_U32_REG_READWRITE_FUNCS(cpacr_el1)
+DEFINE_U32_REG_READWRITE_FUNCS(daif)
+DEFINE_U32_REG_READWRITE_FUNCS(fpcr)
+DEFINE_U32_REG_READWRITE_FUNCS(fpsr)
+
+DEFINE_U32_REG_READ_FUNC(contextidr_el1)
+DEFINE_U32_REG_READ_FUNC(sctlr_el1)
+
+DEFINE_REG_READ_FUNC_(cntfrq, uint32_t, cntfrq_el0)
+
+DEFINE_U64_REG_READWRITE_FUNCS(ttbr0_el1)
+DEFINE_U64_REG_READWRITE_FUNCS(ttbr1_el1)
+DEFINE_U64_REG_READWRITE_FUNCS(tcr_el1)
+
+DEFINE_U64_REG_READ_FUNC(esr_el1)
+DEFINE_U64_REG_READ_FUNC(far_el1)
+DEFINE_U64_REG_READ_FUNC(mpidr_el1)
+DEFINE_U64_REG_READ_FUNC(par_el1)
+
+DEFINE_U64_REG_WRITE_FUNC(mair_el1)
+
+DEFINE_REG_READ_FUNC_(cntpct, uint64_t, cntpct_el0)
+
+#endif /*ASM*/
+
+#endif /*ARM64_H*/
+
diff --git a/core/arch/arm/include/arm64_macros.S b/core/arch/arm/include/arm64_macros.S
new file mode 100644
index 0000000..981a150
--- /dev/null
+++ b/core/arch/arm/include/arm64_macros.S
@@ -0,0 +1,125 @@
+/*
+ * 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.
+ */
+
+	.altmacro
+
+	/*
+	 * This helper macro concatenates instr_prefix, instr_suffix, to
+	 * create a ldp/stp instruction. It also selects register name x/w
+	 * based on reg_bytes.
+	 */
+	.macro __do_dregs instr_prefix, instr_suffix, reg_bytes, base_reg, \
+			base_offs, reg0, reg1
+		.if \reg_bytes == 8
+			\instr_prefix\instr_suffix \
+				x\reg0, x\reg1, [\base_reg, #\base_offs]
+		.else
+			\instr_prefix\instr_suffix \
+				w\reg0, w\reg1, [\base_reg, #\base_offs]
+		.endif
+	.endm
+
+	/*
+	 * This helper macro concatenates instr_prefix, instr_suffix, to
+	 * create a ldr/str instruction. It also selects register name x/w
+	 * based on reg_bytes.
+	 */
+	.macro __do_reg instr_prefix, instr_suffix, reg_bytes, base_reg, \
+			base_offs, reg
+		.if \reg_bytes == 8
+			\instr_prefix\instr_suffix \
+				x\reg, [\base_reg, #\base_offs]
+		.else
+			\instr_prefix\instr_suffix \
+				w\reg, [\base_reg, #\base_offs]
+		.endif
+	.endm
+
+	/*
+	 * This helper macro uses recursion to create a loop which will
+	 * start with generating instructions for register pairs and if
+	 * it's an odd number of registers end with a single load/store.
+	 */
+	.macro _do_regs instr_prefix, reg_bytes, base_reg, base_offs, \
+			from_regnum, to_regnum
+		.if (\to_regnum - \from_regnum + 1) >= 2
+			__do_dregs \instr_prefix, p, \reg_bytes, \base_reg, \
+				\base_offs, \from_regnum, %(\from_regnum + 1)
+		.else
+			__do_reg \instr_prefix, r, \reg_bytes, \base_reg, \
+				\base_offs, \from_regnum
+		.endif
+		.if (\to_regnum - \from_regnum + 1) > 2
+			_do_regs \instr_prefix, \reg_bytes, \base_reg, \
+				%(\base_offs + 2 * \reg_bytes), \
+				%(\from_regnum + 2), \to_regnum
+		.endif
+	.endm
+
+	/*
+	 * Stores registers x[from_regnum]..x[to_regnum] at
+	 * [base_reg, #base_offs]
+	 */
+	.macro store_xregs base_reg, base_offs, from_regnum, to_regnum
+		_do_regs st 8 \base_reg, \base_offs, \from_regnum, \to_regnum
+	.endm
+
+	/*
+	 * Stores registers w[from_regnum]..w[to_regnum] at
+	 * [base_reg, #base_offs]
+	 */
+	.macro store_wregs base_reg, base_offs, from_regnum, to_regnum
+		_do_regs st 4 \base_reg, \base_offs, \from_regnum, \to_regnum
+	.endm
+
+	/*
+	 * Loads registers x[from_regnum]..x[to_regnum] at
+	 * [base_reg, #base_offs]
+	 */
+	.macro load_xregs base_reg, base_offs, from_regnum, to_regnum
+		_do_regs ld 8 \base_reg, \base_offs, \from_regnum, \to_regnum
+	.endm
+
+	/*
+	 * Loads registers w[from_regnum]..w[to_regnum] at
+	 * [base_reg, #base_offs]
+	 */
+	.macro load_wregs base_reg, base_offs, from_regnum, to_regnum
+		_do_regs ld 4 \base_reg, \base_offs, \from_regnum, \to_regnum
+	.endm
+
+
+	/* Push register pair on stack */
+	.macro  push, r1, r2
+	stp     \r1, \r2, [sp, #-16]!
+	.endm
+
+	/* Pop register pair from stack */
+	.macro  pop, r1, r2
+	ldp     \r1, \r2, [sp], #16
+	.endm
+
diff --git a/core/arch/arm/include/kernel/abort.h b/core/arch/arm/include/kernel/abort.h
new file mode 100644
index 0000000..0480f43
--- /dev/null
+++ b/core/arch/arm/include/kernel/abort.h
@@ -0,0 +1,57 @@
+/*
+ * 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.
+ */
+
+#ifndef KERNEL_ABORT_H
+#define KERNEL_ABORT_H
+
+#define ABORT_TYPE_UNDEF	0
+#define ABORT_TYPE_PREFETCH	1
+#define ABORT_TYPE_DATA		2
+
+#ifndef ASM
+
+#include <compiler.h>
+#include <types_ext.h>
+
+struct abort_info {
+	uint32_t abort_type;
+	uint32_t fault_descr;	/* only valid for data of prefetch abort */
+	vaddr_t va;
+	uint32_t pc;
+	struct thread_abort_regs *regs;
+};
+
+void abort_print(struct abort_info *ai);
+void abort_print_error(struct abort_info *ai);
+
+void abort_handler(uint32_t abort_type, struct thread_abort_regs *regs);
+
+bool abort_is_user_exception(struct abort_info *ai);
+
+#endif /*ASM*/
+#endif /*KERNEL_ABORT_H*/
+
diff --git a/core/arch/arm/include/kernel/generic_boot.h b/core/arch/arm/include/kernel/generic_boot.h
new file mode 100644
index 0000000..622c6ff
--- /dev/null
+++ b/core/arch/arm/include/kernel/generic_boot.h
@@ -0,0 +1,101 @@
+/*
+ * 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.
+ */
+#ifndef KERNEL_GENERIC_BOOT_H
+#define KERNEL_GENERIC_BOOT_H
+
+#include <initcall.h>
+#include <types_ext.h>
+
+#if defined(CFG_WITH_ARM_TRUSTED_FW)
+unsigned long cpu_on_handler(unsigned long a0, unsigned long a1);
+struct thread_vector_table *
+generic_boot_init_primary(unsigned long pageable_part, unsigned long unused,
+			  unsigned long fdt);
+unsigned long generic_boot_cpu_on_handler(unsigned long a0, unsigned long a1);
+#else
+void generic_boot_init_primary(unsigned long pageable_part,
+			       unsigned long nsec_entry, unsigned long fdt);
+void generic_boot_init_secondary(unsigned long nsec_entry);
+#endif
+
+void main_init_gic(void);
+void main_secondary_init_gic(void);
+
+void init_sec_mon(unsigned long nsec_entry);
+
+const struct thread_handlers *generic_boot_get_handlers(void);
+
+/* weak routines eventually overridden by platform */
+void plat_cpu_reset_early(void);
+void plat_cpu_reset_late(void);
+void arm_cl2_config(vaddr_t pl310);
+void arm_cl2_enable(vaddr_t pl310);
+
+#if defined(CFG_BOOT_SECONDARY_REQUEST)
+extern paddr_t ns_entry_addrs[] __early_bss;
+int generic_boot_core_release(size_t core_idx, paddr_t entry);
+paddr_t generic_boot_core_hpen(void);
+#endif
+
+extern uint8_t __text_init_start[];
+extern uint8_t __text_start[];
+extern initcall_t __initcall_start;
+extern initcall_t __initcall_end;
+extern uint8_t __data_start[];
+extern uint8_t __data_end[];
+extern uint8_t __rodata_start[];
+extern uint8_t __rodata_end[];
+extern uint8_t __early_bss_start[];
+extern uint8_t __early_bss_end[];
+extern uint8_t __bss_start[];
+extern uint8_t __bss_end[];
+extern uint8_t __nozi_start[];
+extern uint8_t __nozi_end[];
+extern uint8_t __nozi_stack_start[];
+extern uint8_t __nozi_stack_end[];
+extern uint8_t __init_start[];
+extern uint8_t __init_size[];
+extern uint8_t __tmp_hashes_start[];
+extern uint8_t __tmp_hashes_size[];
+extern uint8_t __heap1_start[];
+extern uint8_t __heap1_end[];
+extern uint8_t __heap2_start[];
+extern uint8_t __heap2_end[];
+extern uint8_t __pageable_part_start[];
+extern uint8_t __pageable_part_end[];
+extern uint8_t __pageable_start[];
+extern uint8_t __pageable_end[];
+extern uint8_t __asan_shadow_start[];
+extern uint8_t __asan_shadow_end[];
+extern vaddr_t __ctor_list;
+extern vaddr_t __ctor_end;
+extern uint8_t __end[];
+
+/* Generated by core/arch/arm/kernel/link.mk */
+extern const char core_v_str[];
+
+#endif /* KERNEL_GENERIC_BOOT_H */
diff --git a/core/arch/arm/include/kernel/misc.h b/core/arch/arm/include/kernel/misc.h
new file mode 100644
index 0000000..a9174a8
--- /dev/null
+++ b/core/arch/arm/include/kernel/misc.h
@@ -0,0 +1,53 @@
+/*
+ * 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.
+ */
+
+#ifndef KERNEL_MISC_H
+#define KERNEL_MISC_H
+
+#include <types_ext.h>
+#include <arm.h>
+#include <kernel/thread.h>
+
+size_t get_core_pos(void);
+
+uint32_t read_mode_sp(int cpu_mode);
+uint32_t read_mode_lr(int cpu_mode);
+
+static inline uint64_t reg_pair_to_64(uint32_t reg0, uint32_t reg1)
+{
+	return (uint64_t)reg0 << 32 | reg1;
+}
+
+static inline void reg_pair_from_64(uint64_t val, uint32_t *reg0,
+			uint32_t *reg1)
+{
+	*reg0 = val >> 32;
+	*reg1 = val;
+}
+
+#endif /*KERNEL_MISC_H*/
+
diff --git a/core/arch/arm/include/kernel/mutex.h b/core/arch/arm/include/kernel/mutex.h
new file mode 100644
index 0000000..1698b35
--- /dev/null
+++ b/core/arch/arm/include/kernel/mutex.h
@@ -0,0 +1,98 @@
+/*
+ * Copyright (c) 2014, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+#ifndef KERNEL_MUTEX_H
+#define KERNEL_MUTEX_H
+
+#include <types_ext.h>
+#include <sys/queue.h>
+#include <kernel/wait_queue.h>
+
+enum mutex_value {
+	MUTEX_VALUE_UNLOCKED,
+	MUTEX_VALUE_LOCKED,
+};
+
+struct mutex {
+	enum mutex_value value;
+	unsigned spin_lock;	/* used when operating on this struct */
+	struct wait_queue wq;
+	int owner_id;
+	TAILQ_ENTRY(mutex) link;
+};
+#define MUTEX_INITIALIZER \
+	{ .value = MUTEX_VALUE_UNLOCKED, .owner_id = -1, \
+	  .wq = WAIT_QUEUE_INITIALIZER, }
+
+TAILQ_HEAD(mutex_head, mutex);
+
+void mutex_init(struct mutex *m);
+void mutex_destroy(struct mutex *m);
+
+#ifdef CFG_MUTEX_DEBUG
+void mutex_unlock_debug(struct mutex *m, const char *fname, int lineno);
+#define mutex_unlock(m) mutex_unlock_debug((m), __FILE__, __LINE__)
+
+void mutex_lock_debug(struct mutex *m, const char *fname, int lineno);
+#define mutex_lock(m) mutex_lock_debug((m), __FILE__, __LINE__)
+
+bool mutex_trylock_debug(struct mutex *m, const char *fname, int lineno);
+#define mutex_trylock(m) mutex_trylock_debug((m), __FILE__, __LINE__)
+
+#else
+void mutex_unlock(struct mutex *m);
+void mutex_lock(struct mutex *m);
+bool mutex_trylock(struct mutex *m);
+#endif
+
+
+struct condvar {
+	unsigned spin_lock;
+	struct mutex *m;
+};
+#define CONDVAR_INITIALIZER { .m = NULL }
+
+void condvar_init(struct condvar *cv);
+void condvar_destroy(struct condvar *cv);
+
+#ifdef CFG_MUTEX_DEBUG
+void condvar_signal_debug(struct condvar *cv, const char *fname, int lineno);
+#define condvar_signal(cv) condvar_signal_debug((cv), __FILE__, __LINE__)
+
+void condvar_broadcast_debug(struct condvar *cv, const char *fname, int lineno);
+#define condvar_broadcast(cv) condvar_broadcast_debug((cv), __FILE__, __LINE__)
+
+void condvar_wait_debug(struct condvar *cv, struct mutex *m,
+			const char *fname, int lineno);
+#define condvar_wait(cv, m) condvar_wait_debug((cv), (m), __FILE__, __LINE__)
+#else
+void condvar_signal(struct condvar *cv);
+void condvar_broadcast(struct condvar *cv);
+void condvar_wait(struct condvar *cv, struct mutex *m);
+#endif
+
+#endif /*KERNEL_MUTEX_H*/
+
diff --git a/core/arch/arm/include/kernel/pm_stubs.h b/core/arch/arm/include/kernel/pm_stubs.h
new file mode 100644
index 0000000..6cbe897
--- /dev/null
+++ b/core/arch/arm/include/kernel/pm_stubs.h
@@ -0,0 +1,37 @@
+/*
+ * 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.
+ */
+
+#ifndef PM_STUBS_H
+#define PM_STUBS_H
+
+#include <stdint.h>
+#include <compiler.h>
+
+unsigned long pm_panic(unsigned long a0, unsigned long a1) __noreturn;
+unsigned long pm_do_nothing(unsigned long a0, unsigned long a1);
+
+#endif /* PM_STUBS_H */
diff --git a/core/arch/arm/include/kernel/pseudo_ta.h b/core/arch/arm/include/kernel/pseudo_ta.h
new file mode 100644
index 0000000..98316bd
--- /dev/null
+++ b/core/arch/arm/include/kernel/pseudo_ta.h
@@ -0,0 +1,84 @@
+/*
+ * 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.
+ */
+#ifndef KERNEL_PSEUDO_TA_H
+#define KERNEL_PSEUDO_TA_H
+
+#include <assert.h>
+#include <compiler.h>
+#include <kernel/tee_ta_manager.h>
+#include <tee_api_types.h>
+#include <user_ta_header.h>
+#include <util.h>
+
+#define PTA_MANDATORY_FLAGS	(TA_FLAG_SINGLE_INSTANCE | \
+				TA_FLAG_MULTI_SESSION | \
+				TA_FLAG_INSTANCE_KEEP_ALIVE)
+
+#define PTA_ALLOWED_FLAGS	PTA_MANDATORY_FLAGS
+#define PTA_DEFAULT_FLAGS	PTA_MANDATORY_FLAGS
+
+struct pseudo_ta_head {
+	TEE_UUID uuid;
+	const char *name;
+	uint32_t flags;
+
+	TEE_Result (*create_entry_point)(void);
+	void (*destroy_entry_point)(void);
+	TEE_Result (*open_session_entry_point)(uint32_t nParamTypes,
+			TEE_Param pParams[TEE_NUM_PARAMS],
+			void **ppSessionContext);
+	void (*close_session_entry_point)(void *pSessionContext);
+	TEE_Result (*invoke_command_entry_point)(void *pSessionContext,
+			uint32_t nCommandID, uint32_t nParamTypes,
+			TEE_Param pParams[TEE_NUM_PARAMS]);
+};
+
+#define pseudo_ta_register(...) static const struct pseudo_ta_head __head \
+			__used __section("ta_head_section") = { __VA_ARGS__ }
+
+
+struct pseudo_ta_ctx {
+	const struct pseudo_ta_head *pseudo_ta;
+	struct tee_ta_ctx ctx;
+};
+
+static inline bool is_pseudo_ta_ctx(struct tee_ta_ctx *ctx)
+{
+	return !(ctx->flags & TA_FLAG_USER_MODE);
+}
+
+static inline struct pseudo_ta_ctx *to_pseudo_ta_ctx(struct tee_ta_ctx *ctx)
+{
+	assert(is_pseudo_ta_ctx(ctx));
+	return container_of(ctx, struct pseudo_ta_ctx, ctx);
+}
+
+TEE_Result tee_ta_init_pseudo_ta_session(const TEE_UUID *uuid,
+			struct tee_ta_session *s);
+
+#endif /* KERNEL_PSEUDO_TA_H */
+
diff --git a/core/arch/arm/include/kernel/spinlock.h b/core/arch/arm/include/kernel/spinlock.h
new file mode 100644
index 0000000..c248673
--- /dev/null
+++ b/core/arch/arm/include/kernel/spinlock.h
@@ -0,0 +1,86 @@
+/*
+ * Copyright (c) 2014, STMicroelectronics International N.V.
+ * Copyright (c) 2016, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef KERNEL_SPINLOCK_H
+#define KERNEL_SPINLOCK_H
+
+#define SPINLOCK_LOCK       1
+#define SPINLOCK_UNLOCK     0
+
+#ifndef ASM
+#include <assert.h>
+#include <compiler.h>
+#include <stdbool.h>
+#include <kernel/thread.h>
+
+#ifdef CFG_TEE_CORE_DEBUG
+void spinlock_count_incr(void);
+void spinlock_count_decr(void);
+bool have_spinlock(void);
+static inline void assert_have_no_spinlock(void)
+{
+	assert(!have_spinlock());
+}
+#else
+static inline void spinlock_count_incr(void) { }
+static inline void spinlock_count_decr(void) { }
+static inline void assert_have_no_spinlock(void) { }
+#endif
+
+void __cpu_spin_lock(unsigned int *lock);
+void __cpu_spin_unlock(unsigned int *lock);
+/* returns 0 on locking success, non zero on failure */
+unsigned int __cpu_spin_trylock(unsigned int *lock);
+
+static inline void cpu_spin_lock(unsigned int *lock)
+{
+	assert(thread_irq_disabled());
+	__cpu_spin_lock(lock);
+	spinlock_count_incr();
+}
+
+static inline bool cpu_spin_trylock(unsigned int *lock)
+{
+	unsigned int rc;
+
+	assert(thread_irq_disabled());
+	rc = __cpu_spin_trylock(lock);
+	if (!rc)
+		spinlock_count_incr();
+	return !rc;
+}
+
+static inline void cpu_spin_unlock(unsigned int *lock)
+{
+	assert(thread_irq_disabled());
+	__cpu_spin_unlock(lock);
+	spinlock_count_decr();
+}
+#endif /* ASM */
+
+#endif /* KERNEL_SPINLOCK_H */
diff --git a/core/arch/arm/include/kernel/tee_l2cc_mutex.h b/core/arch/arm/include/kernel/tee_l2cc_mutex.h
new file mode 100644
index 0000000..508a510
--- /dev/null
+++ b/core/arch/arm/include/kernel/tee_l2cc_mutex.h
@@ -0,0 +1,72 @@
+/*
+ * 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.
+ */
+#ifndef TEE_L2CC_MUTEX_H
+#define TEE_L2CC_MUTEX_H
+#include <inttypes.h>
+#include <tee_api_types.h>
+#include <tee_api_defines.h>
+#include <compiler.h>
+
+#if defined(CFG_PL310)
+TEE_Result tee_enable_l2cc_mutex(void);
+TEE_Result tee_disable_l2cc_mutex(void);
+TEE_Result tee_get_l2cc_mutex(paddr_t *mutex);
+TEE_Result tee_set_l2cc_mutex(paddr_t *mutex);
+void tee_l2cc_mutex_lock(void);
+void tee_l2cc_mutex_unlock(void);
+
+/*
+ * Store the pa of a mutex used for l2cc
+ * It is allocated from the boot
+ */
+void tee_l2cc_store_mutex_boot_pa(uint32_t pa);
+
+#else
+static TEE_Result tee_enable_l2cc_mutex(void);
+static TEE_Result tee_disable_l2cc_mutex(void);
+static TEE_Result tee_get_l2cc_mutex(paddr_t *mutex);
+static TEE_Result tee_set_l2cc_mutex(paddr_t *mutex);
+
+static inline TEE_Result tee_enable_l2cc_mutex(void)
+{
+	return TEE_ERROR_NOT_SUPPORTED;
+}
+static inline TEE_Result tee_disable_l2cc_mutex(void)
+{
+	return TEE_ERROR_NOT_SUPPORTED;
+}
+static inline TEE_Result tee_get_l2cc_mutex(paddr_t *mutex __unused)
+{
+	return TEE_ERROR_NOT_SUPPORTED;
+}
+static inline TEE_Result tee_set_l2cc_mutex(paddr_t *mutex __unused)
+{
+	return TEE_ERROR_NOT_SUPPORTED;
+}
+#endif
+
+#endif /* TEE_L2CC_MUTEX_H */
diff --git a/core/arch/arm/include/kernel/thread.h b/core/arch/arm/include/kernel/thread.h
new file mode 100644
index 0000000..175ba77
--- /dev/null
+++ b/core/arch/arm/include/kernel/thread.h
@@ -0,0 +1,559 @@
+/*
+ * Copyright (c) 2014, STMicroelectronics International N.V.
+ * Copyright (c) 2016-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 KERNEL_THREAD_H
+#define KERNEL_THREAD_H
+
+#ifndef ASM
+#include <types_ext.h>
+#include <compiler.h>
+#include <optee_msg.h>
+#include <kernel/mutex.h>
+#include <kernel/vfp.h>
+#include <mm/pgt_cache.h>
+#endif
+
+#define THREAD_ID_0		0
+#define THREAD_ID_INVALID	-1
+
+#define THREAD_RPC_MAX_NUM_PARAMS	4
+
+#ifndef ASM
+struct thread_vector_table {
+	uint32_t std_smc_entry;
+	uint32_t fast_smc_entry;
+	uint32_t cpu_on_entry;
+	uint32_t cpu_off_entry;
+	uint32_t cpu_resume_entry;
+	uint32_t cpu_suspend_entry;
+	uint32_t fiq_entry;
+	uint32_t system_off_entry;
+	uint32_t system_reset_entry;
+};
+extern struct thread_vector_table thread_vector_table;
+
+struct thread_specific_data {
+	TAILQ_HEAD(, tee_ta_session) sess_stack;
+	struct tee_ta_ctx *ctx;
+#ifdef CFG_SMALL_PAGE_USER_TA
+	struct pgt_cache pgt_cache;
+#endif
+	void *rpc_fs_payload;
+	paddr_t rpc_fs_payload_pa;
+	uint64_t rpc_fs_payload_cookie;
+	size_t rpc_fs_payload_size;
+};
+
+struct thread_user_vfp_state {
+	struct vfp_state vfp;
+	bool lazy_saved;
+	bool saved;
+};
+
+#ifdef ARM32
+struct thread_smc_args {
+	uint32_t a0;	/* SMC function ID */
+	uint32_t a1;	/* Parameter */
+	uint32_t a2;	/* Parameter */
+	uint32_t a3;	/* Thread ID when returning from RPC */
+	uint32_t a4;	/* Not used */
+	uint32_t a5;	/* Not used */
+	uint32_t a6;	/* Not used */
+	uint32_t a7;	/* Hypervisor Client ID */
+};
+#endif /*ARM32*/
+#ifdef ARM64
+struct thread_smc_args {
+	uint64_t a0;	/* SMC function ID */
+	uint64_t a1;	/* Parameter */
+	uint64_t a2;	/* Parameter */
+	uint64_t a3;	/* Thread ID when returning from RPC */
+	uint64_t a4;	/* Not used */
+	uint64_t a5;	/* Not used */
+	uint64_t a6;	/* Not used */
+	uint64_t a7;	/* Hypervisor Client ID */
+};
+#endif /*ARM64*/
+
+#ifdef ARM32
+struct thread_abort_regs {
+	uint32_t usr_sp;
+	uint32_t usr_lr;
+	uint32_t pad;
+	uint32_t spsr;
+	uint32_t elr;
+	uint32_t r0;
+	uint32_t r1;
+	uint32_t r2;
+	uint32_t r3;
+	uint32_t r4;
+	uint32_t r5;
+	uint32_t r6;
+	uint32_t r7;
+	uint32_t r8;
+	uint32_t r9;
+	uint32_t r10;
+	uint32_t r11;
+	uint32_t ip;
+};
+#endif /*ARM32*/
+#ifdef ARM64
+struct thread_abort_regs {
+	uint64_t x0;	/* r0_usr */
+	uint64_t x1;	/* r1_usr */
+	uint64_t x2;	/* r2_usr */
+	uint64_t x3;	/* r3_usr */
+	uint64_t x4;	/* r4_usr */
+	uint64_t x5;	/* r5_usr */
+	uint64_t x6;	/* r6_usr */
+	uint64_t x7;	/* r7_usr */
+	uint64_t x8;	/* r8_usr */
+	uint64_t x9;	/* r9_usr */
+	uint64_t x10;	/* r10_usr */
+	uint64_t x11;	/* r11_usr */
+	uint64_t x12;	/* r12_usr */
+	uint64_t x13;	/* r13/sp_usr */
+	uint64_t x14;	/* r14/lr_usr */
+	uint64_t x15;
+	uint64_t x16;
+	uint64_t x17;
+	uint64_t x18;
+	uint64_t x19;
+	uint64_t x20;
+	uint64_t x21;
+	uint64_t x22;
+	uint64_t x23;
+	uint64_t x24;
+	uint64_t x25;
+	uint64_t x26;
+	uint64_t x27;
+	uint64_t x28;
+	uint64_t x29;
+	uint64_t x30;
+	uint64_t elr;
+	uint64_t spsr;
+	uint64_t sp_el0;
+};
+#endif /*ARM64*/
+
+#ifdef ARM32
+struct thread_svc_regs {
+	uint32_t spsr;
+	uint32_t r0;
+	uint32_t r1;
+	uint32_t r2;
+	uint32_t r3;
+	uint32_t r4;
+	uint32_t r5;
+	uint32_t r6;
+	uint32_t r7;
+	uint32_t lr;
+};
+#endif /*ARM32*/
+#ifdef ARM64
+struct thread_svc_regs {
+	uint64_t elr;
+	uint64_t spsr;
+	uint64_t x0;	/* r0_usr */
+	uint64_t x1;	/* r1_usr */
+	uint64_t x2;	/* r2_usr */
+	uint64_t x3;	/* r3_usr */
+	uint64_t x4;	/* r4_usr */
+	uint64_t x5;	/* r5_usr */
+	uint64_t x6;	/* r6_usr */
+	uint64_t x7;	/* r7_usr */
+	uint64_t x8;	/* r8_usr */
+	uint64_t x9;	/* r9_usr */
+	uint64_t x10;	/* r10_usr */
+	uint64_t x11;	/* r11_usr */
+	uint64_t x12;	/* r12_usr */
+	uint64_t x13;	/* r13/sp_usr */
+	uint64_t x14;	/* r14/lr_usr */
+	uint64_t x30;
+	uint64_t sp_el0;
+	uint64_t pad;
+} __aligned(16);
+#endif /*ARM64*/
+#endif /*ASM*/
+
+#ifndef ASM
+typedef void (*thread_smc_handler_t)(struct thread_smc_args *args);
+typedef void (*thread_fiq_handler_t)(void);
+typedef unsigned long (*thread_pm_handler_t)(unsigned long a0,
+					     unsigned long a1);
+struct thread_handlers {
+	/*
+	 * stdcall and fastcall are called as regular functions and
+	 * normal ARM Calling Convention applies. Return values are passed
+	 * args->param{1-3} and forwarded into r0-r3 when returned to
+	 * non-secure world.
+	 *
+	 * stdcall handles calls which can be preemted from non-secure
+	 * world. This handler is executed with a large stack.
+	 *
+	 * 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.
+	 *
+	 * TODO investigate if we should execute fastcalls and FIQs on
+	 * different stacks allowing FIQs to be enabled during a fastcall.
+	 */
+	thread_smc_handler_t std_smc;
+	thread_smc_handler_t fast_smc;
+
+	/*
+	 * 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.
+	 */
+	thread_fiq_handler_t fiq;
+
+	/*
+	 * Power management handlers triggered from ARM Trusted Firmware.
+	 * Not used when using internal monitor.
+	 */
+	thread_pm_handler_t cpu_on;
+	thread_pm_handler_t cpu_off;
+	thread_pm_handler_t cpu_suspend;
+	thread_pm_handler_t cpu_resume;
+	thread_pm_handler_t system_off;
+	thread_pm_handler_t system_reset;
+};
+void thread_init_primary(const struct thread_handlers *handlers);
+void thread_init_per_cpu(void);
+
+/*
+ * Sets the stacks to be used by the different threads. Use THREAD_ID_0 for
+ * first stack, THREAD_ID_0 + 1 for the next and so on.
+ *
+ * Returns true on success and false on errors.
+ */
+bool thread_init_stack(uint32_t stack_id, vaddr_t sp);
+
+/*
+ * Initializes a thread to be used during boot
+ */
+void thread_init_boot_thread(void);
+
+/*
+ * Clears the current thread id
+ * Only supposed to be used during initialization.
+ */
+void thread_clr_boot_thread(void);
+
+/*
+ * Returns current thread id.
+ */
+int thread_get_id(void);
+
+/*
+ * Returns current thread id, return -1 on failure.
+ */
+int thread_get_id_may_fail(void);
+
+/* Returns Thread Specific Data (TSD) pointer. */
+struct thread_specific_data *thread_get_tsd(void);
+
+/*
+ * Sets IRQ status for current thread, must only be called from an
+ * active thread context.
+ *
+ * enable == true  -> enable IRQ
+ * enable == false -> disable IRQ
+ */
+void thread_set_irq(bool enable);
+
+/*
+ * Restores the IRQ status (in CPSR) for current thread, must only be called
+ * from an active thread context.
+ */
+void thread_restore_irq(void);
+
+/*
+ * Defines the bits for the exception mask used the the
+ * thread_*_exceptions() functions below.
+ */
+#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)
+
+/*
+ * thread_get_exceptions() - return current exception mask
+ */
+uint32_t thread_get_exceptions(void);
+
+/*
+ * thread_set_exceptions() - set exception mask
+ * @exceptions: exception mask to set
+ *
+ * Any previous exception mask is replaced by this exception mask, that is,
+ * old bits are cleared and replaced by these.
+ */
+void thread_set_exceptions(uint32_t exceptions);
+
+/*
+ * thread_mask_exceptions() - Masks (disables) specified asynchronous exceptions
+ * @exceptions	exceptions to mask
+ * @returns old exception state
+ */
+uint32_t thread_mask_exceptions(uint32_t exceptions);
+
+/*
+ * thread_unmask_exceptions() - Unmasks asynchronous exceptions
+ * @state	Old asynchronous exception state to restore (returned by
+ *		thread_mask_exceptions())
+ */
+void thread_unmask_exceptions(uint32_t state);
+
+
+static inline bool thread_irq_disabled(void)
+{
+	return !!(thread_get_exceptions() & THREAD_EXCP_IRQ);
+}
+
+#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.
+ *
+ * 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.
+ *
+ * VFP state is saved as needed.
+ *
+ * Returns a state variable that should be passed to
+ * thread_kernel_disable_vfp().
+ */
+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
+ * thread_kernel_enable_vfp().
+ *
+ * This function may only be called after a call to
+ * thread_kernel_enable_vfp().
+ */
+void thread_kernel_disable_vfp(uint32_t state);
+
+/*
+ * thread_kernel_save_vfp() - Saves kernel vfp state if enabled
+ */
+void thread_kernel_save_vfp(void);
+
+/*
+ * thread_kernel_save_vfp() - Restores kernel vfp state
+ */
+void thread_kernel_restore_vfp(void);
+
+/*
+ * thread_user_enable_vfp() - Enables vfp for user mode usage
+ * @uvfp:	pointer to where to save the vfp state if needed
+ */
+void thread_user_enable_vfp(struct thread_user_vfp_state *uvfp);
+#else /*CFG_WITH_VFP*/
+static inline void thread_kernel_save_vfp(void)
+{
+}
+
+static inline void thread_kernel_restore_vfp(void)
+{
+}
+#endif /*CFG_WITH_VFP*/
+
+/*
+ * thread_user_save_vfp() - Saves the user vfp state if enabled
+ */
+#ifdef CFG_WITH_VFP
+void thread_user_save_vfp(void);
+#else
+static inline void thread_user_save_vfp(void)
+{
+}
+#endif
+
+/*
+ * thread_user_clear_vfp() - Clears the vfp state
+ * @uvfp:	pointer to saved state to clear
+ */
+#ifdef CFG_WITH_VFP
+void thread_user_clear_vfp(struct thread_user_vfp_state *uvfp);
+#else
+static inline void thread_user_clear_vfp(
+			struct thread_user_vfp_state *uvfp __unused)
+{
+}
+#endif
+
+
+/*
+ * thread_enter_user_mode() - Enters user mode
+ * @a0:		Passed in r/x0 for user_func
+ * @a1:		Passed in r/x1 for user_func
+ * @a2:		Passed in r/x2 for user_func
+ * @a3:		Passed in r/x3 for user_func
+ * @user_sp:	Assigned sp value in user mode
+ * @user_func:	Function to execute in user mode
+ * @is_32bit:   True if TA should execute in Aarch32, false if Aarch64
+ * @exit_status0: Pointer to opaque exit staus 0
+ * @exit_status1: Pointer to opaque exit staus 1
+ *
+ * This functions enters user mode with the argument described above,
+ * @exit_status0 and @exit_status1 are filled in by thread_unwind_user_mode()
+ * when returning back to the caller of this function through an exception
+ * handler.
+ *
+ * @Returns what's passed in "ret" to thread_unwind_user_mode()
+ */
+uint32_t thread_enter_user_mode(unsigned long a0, unsigned long a1,
+		unsigned long a2, unsigned long a3, unsigned long user_sp,
+		unsigned long entry_func, bool is_32bit,
+		uint32_t *exit_status0, uint32_t *exit_status1);
+
+/*
+ * thread_unwind_user_mode() - Unwinds kernel stack from user entry
+ * @ret:	Value to return from thread_enter_user_mode()
+ * @exit_status0: Exit status 0
+ * @exit_status1: Exit status 1
+ *
+ * This is the function that exception handlers can return into
+ * to resume execution in kernel mode instead of user mode.
+ *
+ * This function is closely coupled with thread_enter_user_mode() since it
+ * need to restore registers saved by thread_enter_user_mode() and when it
+ * returns make it look like thread_enter_user_mode() just returned. It is
+ * expected that the stack pointer is where thread_enter_user_mode() left
+ * it. The stack will be unwound and the function will return to where
+ * thread_enter_user_mode() was called from.  Exit_status0 and exit_status1
+ * are filled in the corresponding pointers supplied to
+ * thread_enter_user_mode().
+ */
+void thread_unwind_user_mode(uint32_t ret, uint32_t exit_status0,
+		uint32_t exit_status1);
+
+#ifdef ARM64
+/*
+ * thread_get_saved_thread_sp() - Returns the saved sp of current thread
+ *
+ * When switching from the thread stack pointer the value is stored
+ * separately in the current thread context. This function returns this
+ * saved value.
+ *
+ * @returns stack pointer
+ */
+vaddr_t thread_get_saved_thread_sp(void);
+#endif /*ARM64*/
+
+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.
+ */
+void thread_add_mutex(struct mutex *m);
+
+/*
+ * Removes a mutex from the list of held mutexes for current thread
+ * Requires IRQs to be disabled.
+ */
+void thread_rem_mutex(struct mutex *m);
+
+/*
+ * Disables and empties the prealloc RPC cache one reference at a time. If
+ * all threads are idle this function returns true and a cookie of one shm
+ * object which was removed from the cache. When the cache is empty *cookie
+ * is set to 0 and the cache is disabled else a valid cookie value. If one
+ * thread isn't idle this function returns false.
+ */
+bool thread_disable_prealloc_rpc_cache(uint64_t *cookie);
+
+/*
+ * Enabled the prealloc RPC cache. If all threads are idle the cache is
+ * enabled and this function returns true. If one thread isn't idle this
+ * function return false.
+ */
+bool thread_enable_prealloc_rpc_cache(void);
+
+/**
+ * Allocates data for struct optee_msg_arg.
+ *
+ * @size:	size in bytes of struct optee_msg_arg
+ * @arg:	returned physcial pointer to a struct optee_msg_arg buffer,
+ *		0 if allocation failed.
+ * @cookie:	returned cookie used when freeing the buffer
+ */
+void thread_rpc_alloc_arg(size_t size, paddr_t *arg, uint64_t *cookie);
+
+/**
+ * Free physical memory previously allocated with thread_rpc_alloc_arg()
+ *
+ * @cookie:	cookie received when allocating the buffer
+ */
+void thread_rpc_free_arg(uint64_t cookie);
+
+/**
+ * Allocates data for payload buffers.
+ *
+ * @size:	size in bytes of payload buffer
+ * @payload:	returned physcial pointer to payload buffer, 0 if allocation
+ *		failed.
+ * @cookie:	returned cookie used when freeing the buffer
+ */
+void thread_rpc_alloc_payload(size_t size, paddr_t *payload, uint64_t *cookie);
+
+/**
+ * Free physical memory previously allocated with thread_rpc_alloc_payload()
+ *
+ * @cookie:	cookie received when allocating the buffer
+ */
+void thread_rpc_free_payload(uint64_t cookie);
+
+/**
+ * Does an RPC using a preallocated argument buffer
+ * @cmd: RPC cmd
+ * @num_params: number of parameters (max 2)
+ * @params: RPC parameters
+ * @returns RPC return value
+ */
+uint32_t thread_rpc_cmd(uint32_t cmd, size_t num_params,
+		struct optee_msg_param *params);
+
+#endif /*ASM*/
+
+#endif /*KERNEL_THREAD_H*/
diff --git a/core/arch/arm/include/kernel/thread_defs.h b/core/arch/arm/include/kernel/thread_defs.h
new file mode 100644
index 0000000..0f54569
--- /dev/null
+++ b/core/arch/arm/include/kernel/thread_defs.h
@@ -0,0 +1,35 @@
+/*
+ * 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.
+ */
+
+#ifndef KERNEL_THREAD_DEFS_H
+#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)
+
+#endif /*KERNEL_THREAD_DEFS_H*/
diff --git a/core/arch/arm/include/kernel/time_source.h b/core/arch/arm/include/kernel/time_source.h
new file mode 100644
index 0000000..ddabfe9
--- /dev/null
+++ b/core/arch/arm/include/kernel/time_source.h
@@ -0,0 +1,44 @@
+/*
+ * Copyright (c) 2014, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <kernel/tee_time.h>
+
+struct time_source {
+	const char *name;
+	uint32_t protection_level;
+	TEE_Result (*get_sys_time)(TEE_Time *time);
+};
+void time_source_init(void);
+
+#define REGISTER_TIME_SOURCE(source)	\
+	void time_source_init(void) { \
+		_time_source = source; \
+	}
+
+extern struct time_source _time_source;
+
+void arm_prng_add_jitter_entropy(void);
diff --git a/core/arch/arm/include/kernel/tz_proc_def.h b/core/arch/arm/include/kernel/tz_proc_def.h
new file mode 100644
index 0000000..abe281b
--- /dev/null
+++ b/core/arch/arm/include/kernel/tz_proc_def.h
@@ -0,0 +1,110 @@
+/*
+ * 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.
+ */
+/*
+ *  General constants
+ */
+
+/*
+ * CP15 Multiprocessor Affinity register (MPIDR)
+ */
+#define CP15_CONFIG_CPU_ID_MASK   0x00000003
+#define CPU_ID0    0x00000000
+#define CPU_ID1    0x00000001
+
+/*
+ * CP15 Secure configuration register
+ */
+#define CP15_CONFIG_NS_MASK   0x00000001
+#define CP15_CONFIG_IRQ_MASK  0x00000002
+#define CP15_CONFIG_FIQ_MASK  0x00000004
+#define CP15_CONFIG_EA_MASK   0x00000008
+#define CP15_CONFIG_FW_MASK   0x00000010
+#define CP15_CONFIG_AW_MASK   0x00000020
+#define CP15_CONFIG_nET_MASK  0x00000040
+
+/*
+ * CP15 Control register
+ */
+#define CP15_CONTROL_M_MASK          0x00000001
+#define CP15_CONTROL_C_MASK          0x00000004
+#define CP15_CONTROL_Z_MASK          0x00000800
+#define CP15_CONTROL_I_MASK          0x00001000
+#define CP15_CONTROL_V_MASK          0x00002000
+#define CP15_CONTROL_HA_MASK         0x00020000
+#define CP15_CONTROL_EE_MASK         0x02000000
+#define CP15_CONTROL_NMFI_MASK       0x08000000
+#define CP15_CONTROL_TRE_MASK        0x10000000
+#define CP15_CONTROL_AFE_MASK        0x20000000
+#define CP15_CONTROL_TE_MASK         0x40000000
+
+/*
+ * CP15 Auxiliary Control register
+ */
+#define CP15_CONTROL_SMP_MASK        0x00000040
+#define CP15_CONTROL_EXCL_MASK       0x00000080
+
+/*
+ * CP15 Non secure access control register
+ */
+#define CP15_NSAC_TL_MASK        0x10000
+#define CP15_NSAC_CL_MASK        0x20000
+#define CP15_NSAC_CPN_MASK       0x3FFF
+
+/*
+ * CP15 Cache register
+ */
+#define CP15_CACHE_ADDR_R_BIT    12
+#define CP15_CACHE_ADDR_L_BIT    (32-CP15_CACHE_ADDR_R_BIT)
+#define CP15_CACHE_RESULT_MASK   0x00000001
+
+/*
+ * CP15 TCM register
+ *
+ * ITCM configuration (4kbytes, @0x20100000, enabled)
+ * DTCM configuration (4kbytes, @0x20101000, enabled)
+ */
+#define CP15_TCM_ENABLE_MASK     0x00000001
+#define CP15_TCM_INSTR_TCM       0x2010000C
+#define CP15_TCM_DATA_TCM        0x2010100C
+
+/*
+ * CP15 cache lockdown register
+ *
+ * ITCM configuration (4kbytes, @0x20100000, enabled)
+ * DTCM configuration (4kbytes, @0x20101000, enabled)
+ */
+#define CP15_CACHE_LOCK_ALLWAYS_MASK     0x0000000F
+
+/*
+ * CP15 cache cleaning constant definition
+ */
+/* start of line number field offset in way/index format */
+#define LINE_FIELD_OFFSET        5
+/* Warning: this assumes a 256 lines/way cache (32kB cache) */
+#define LINE_FIELD_OVERFLOW      13
+/* start of way number field offset in way/index format */
+#define WAY_FIELD_OFFSET         30
diff --git a/core/arch/arm/include/kernel/tz_ssvce.h b/core/arch/arm/include/kernel/tz_ssvce.h
new file mode 100644
index 0000000..a886f9d
--- /dev/null
+++ b/core/arch/arm/include/kernel/tz_ssvce.h
@@ -0,0 +1,73 @@
+/*
+ * 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.
+ */
+
+#ifndef TZ_SSVCE_H
+#define TZ_SSVCE_H
+
+#ifndef ASM
+
+#include <types_ext.h>
+
+unsigned int secure_get_cpu_id(void);
+
+void arm_cl1_d_cleanbysetway(void);
+void arm_cl1_d_invbysetway(void);
+void arm_cl1_d_cleaninvbysetway(void);
+/* End address is included in the range (last address in range)*/
+void arm_cl1_d_cleanbyva(void *start, void *end);
+/* End address is included in the range (last address in range)*/
+void arm_cl1_d_invbyva(void *start, void *end);
+/* End address is included in the range (last address in range)*/
+void arm_cl1_d_cleaninvbyva(void *start, void *end);
+void arm_cl1_i_inv_all(void);
+/* End address is included in the range (last address in range)*/
+void arm_cl1_i_inv(void *start, void *end);
+
+void secure_mmu_datatlbinvall(void);
+void secure_mmu_unifiedtlbinvall(void);
+void secure_mmu_unifiedtlbinvbymva(unsigned long addr);
+void secure_mmu_unifiedtlbinv_curasid(void);
+void secure_mmu_unifiedtlbinv_byasid(unsigned long asid);
+
+void secure_mmu_disable(void);
+#endif /*!ASM*/
+
+#ifdef ARM64
+/* D$ set/way op type defines */
+#define DCISW			0x0
+#define DCCISW			0x1
+#define DCCSW			0x2
+
+#ifndef ASM
+void flush_dcache_range(vaddr_t va, size_t len);
+void inv_dcache_range(vaddr_t va, size_t len);
+void dcsw_op_louis(uint32_t op);
+void dcsw_op_all(uint32_t op);
+#endif /*!ASM*/
+#endif /*ARM64*/
+
+#endif
diff --git a/core/arch/arm/include/kernel/tz_ssvce_def.h b/core/arch/arm/include/kernel/tz_ssvce_def.h
new file mode 100644
index 0000000..3e9f9fc
--- /dev/null
+++ b/core/arch/arm/include/kernel/tz_ssvce_def.h
@@ -0,0 +1,141 @@
+/*
+ * 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.
+ */
+#ifndef TZ_SSVCE_DEF_H
+#define TZ_SSVCE_DEF_H
+
+#include <util.h>
+
+/*
+ * ARMv7 Secure Services library
+ */
+
+#define CPSR_OFFSET                0x00
+#define STACK_INT_USAGE            0x04
+
+/*
+ * tee service IDs (TODO: align with the service ID list).
+ * Set by NSec in R4 before SMC to request a TEE service.
+ */
+#define SSAPI_RET_FROM_INT_SERV             4
+#define SSAPI_RET_FROM_RPC_SERV             5
+
+/*
+ * TEE monitor: status returned by the routine that checks the entry
+ * reason (valid Service ID / secure context).
+ */
+#define SEC_INVALID_ENTRY                  0
+#define SEC_PRE_INIT_ENTRY                 1
+#define SEC_RET_FROM_INT_ENTRY             2
+#define SEC_RET_FROM_RPC_ENTRY             3
+#define SEC_NORMAL_ENTRY                   4
+
+/*
+ * teecore exit reason.
+ * Set by Secure in R4 before SMC to request a switch to NSec.
+ */
+#define SEC_EXIT_NORMAL                    1
+#define SEC_EXIT_START_EXT_CODE            2
+#define SEC_EXIT_INT                       3
+#define SEC_EXIT_RPC_CALL                  4
+#define SEC_EXIT_FIRST                     5
+#define SEC_EXIT_DEEP_SLEEP                6
+
+/* misc */
+
+#define  SEC_UNDEF_STACK_OFFSET             4
+#define  SEC_ABORT_STACK_OFFSET             12
+
+#define  SEC_ENTRY_STATUS_NOK             0
+#define  SEC_ENTRY_STATUS_OK              1
+
+/*
+ * Outer cache iomem
+ */
+#define PL310_LINE_SIZE		32
+#define PL310_8_WAYS		8
+
+/* reg1 */
+#define PL310_CTRL		0x100
+#define PL310_AUX_CTRL		0x104
+#define PL310_TAG_RAM_CTRL	0x108
+#define PL310_DATA_RAM_CTRL	0x10C
+/* reg7 */
+#define PL310_SYNC		0x730
+#define PL310_INV_BY_WAY	0x77C
+#define PL310_CLEAN_BY_WAY	0x7BC
+#define PL310_FLUSH_BY_WAY	0x7FC
+#define PL310_INV_BY_PA		0x770
+#define PL310_CLEAN_BY_PA	0x7B0
+#define PL310_FLUSH_BY_PA	0x7F0
+#define PL310_FLUSH_BY_INDEXWAY	0x7F8
+/* reg9 */
+#define PL310_DCACHE_LOCKDOWN_BASE 0x900
+#define PL310_ICACHE_LOCKDOWN_BASE 0x904
+/* reg12 */
+#define PL310_ADDR_FILT_START	0xC00
+#define PL310_ADDR_FILT_END	0xC04
+/* reg15 */
+#define PL310_DEBUG_CTRL	0xF40
+#define PL310_PREFETCH_CTRL	0xF60
+#define PL310_POWER_CTRL	0xF80
+
+#define PL310_CTRL_ENABLE_BIT	BIT32(0)
+#define PL310_AUX_16WAY_BIT	BIT32(16)
+
+/*
+ * SCU iomem
+ */
+#define SCU_CTRL	0x00
+#define SCU_CONFIG	0x04
+#define SCU_POWER	0x08
+#define SCU_INV_SEC	0x0C
+#define SCU_FILT_SA	0x40
+#define SCU_FILT_EA	0x44
+#define SCU_SAC		0x50
+#define SCU_NSAC	0x54
+#define SCU_ERRATA744369 0x30
+
+#define SCU_ACCESS_CONTROL_CPU0		BIT32(0)
+#define SCU_ACCESS_CONTROL_CPU1		BIT32(1)
+#define SCU_ACCESS_CONTROL_CPU2		BIT32(2)
+#define SCU_ACCESS_CONTROL_CPU3		BIT32(3)
+#define SCU_NSAC_SCU_SHIFT		0
+#define SCU_NSAC_PTIMER_SHIFT		4
+#define SCU_NSAC_GTIMER_SHIFT		8
+
+/*
+ * GIC iomem
+ */
+#define GIC_DIST_ISR0	0x080
+#define GIC_DIST_ISR1	0x084
+
+/*
+ * CPU iomem
+ */
+#define CORE_ICC_ICCPMR	0x0004
+
+#endif /* TZ_SSVCE_DEF_H */
diff --git a/core/arch/arm/include/kernel/tz_ssvce_pl310.h b/core/arch/arm/include/kernel/tz_ssvce_pl310.h
new file mode 100644
index 0000000..88b91e1
--- /dev/null
+++ b/core/arch/arm/include/kernel/tz_ssvce_pl310.h
@@ -0,0 +1,46 @@
+/*
+ * 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.
+ */
+
+#ifndef TZ_SSVCE_PL310_H
+#define TZ_SSVCE_PL310_H
+
+#include <util.h>
+#include <kernel/tz_ssvce_def.h>
+#include <types_ext.h>
+
+vaddr_t pl310_base(void);
+/*
+ * End address is included in the range (last address in range)
+ */
+void arm_cl2_cleaninvbyway(vaddr_t pl310_base);
+void arm_cl2_invbyway(vaddr_t pl310_base);
+void arm_cl2_cleanbyway(vaddr_t pl310_base);
+void arm_cl2_cleanbypa(vaddr_t pl310_base, paddr_t start, paddr_t end);
+void arm_cl2_invbypa(vaddr_t pl310_base, paddr_t start, paddr_t end);
+void arm_cl2_cleaninvbypa(vaddr_t pl310_base, paddr_t start, paddr_t end);
+
+#endif /* TZ_SSVCE_PL310_H */
diff --git a/core/arch/arm/include/kernel/unwind.h b/core/arch/arm/include/kernel/unwind.h
new file mode 100644
index 0000000..cc5ff5a
--- /dev/null
+++ b/core/arch/arm/include/kernel/unwind.h
@@ -0,0 +1,77 @@
+/*-
+ * Copyright (c) 2015, Linaro Limited
+ * Copyright (c) 2000, 2001 Ben Harris
+ * Copyright (c) 1996 Scott K. Stevens
+ *
+ * Mach Operating System
+ * Copyright (c) 1991,1990 Carnegie Mellon University
+ * All Rights Reserved.
+ *
+ * Permission to use, copy, modify and distribute this software and its
+ * documentation is hereby granted, provided that both the copyright
+ * notice and this permission notice appear in all copies of the
+ * software, derivative works or modified versions, and any portions
+ * thereof, and that both notices appear in supporting documentation.
+ *
+ * CARNEGIE MELLON ALLOWS FREE USE OF THIS SOFTWARE IN ITS "AS IS"
+ * CONDITION.  CARNEGIE MELLON DISCLAIMS ANY LIABILITY OF ANY KIND FOR
+ * ANY DAMAGES WHATSOEVER RESULTING FROM THE USE OF THIS SOFTWARE.
+ *
+ * Carnegie Mellon requests users of this software to return to
+ *
+ *  Software Distribution Coordinator  or  Software.Distribution@CS.CMU.EDU
+ *  School of Computer Science
+ *  Carnegie Mellon University
+ *  Pittsburgh PA 15213-3890
+ *
+ * any improvements or extensions that they make and grant Carnegie the
+ * rights to redistribute these changes.
+ *
+ * $FreeBSD$
+ */
+
+#ifndef KERNEL_UNWIND
+#define KERNEL_UNWIND
+
+#ifndef ASM
+#include <types_ext.h>
+
+#ifdef ARM32
+/* The state of the unwind process */
+struct unwind_state {
+	uint32_t registers[16];
+	uint32_t start_pc;
+	uint32_t *insn;
+	unsigned entries;
+	unsigned byte;
+	uint16_t update_mask;
+};
+#endif /*ARM32*/
+
+#ifdef ARM64
+struct unwind_state {
+	uint64_t fp;
+	uint64_t sp;
+	uint64_t pc;
+};
+#endif /*ARM64*/
+
+bool unwind_stack(struct unwind_state *state);
+
+#if defined(CFG_CORE_UNWIND) && (TRACE_LEVEL > 0)
+void print_stack(int level);
+#else
+static inline void print_stack(int level __unused)
+{
+}
+#endif
+
+#endif /*ASM*/
+
+#ifdef CFG_CORE_UNWIND
+#define UNWIND(...)	__VA_ARGS__
+#else
+#define UNWIND(...)
+#endif
+
+#endif /*KERNEL_UNWIND*/
diff --git a/core/arch/arm/include/kernel/user_ta.h b/core/arch/arm/include/kernel/user_ta.h
new file mode 100644
index 0000000..196c0af
--- /dev/null
+++ b/core/arch/arm/include/kernel/user_ta.h
@@ -0,0 +1,93 @@
+/*
+ * 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.
+ */
+#ifndef KERNEL_USER_TA_H
+#define KERNEL_USER_TA_H
+
+#include <assert.h>
+#include <kernel/tee_ta_manager.h>
+#include <kernel/thread.h>
+#include <mm/tee_mm.h>
+#include <tee_api_types.h>
+#include <types_ext.h>
+#include <util.h>
+
+TAILQ_HEAD(tee_cryp_state_head, tee_cryp_state);
+TAILQ_HEAD(tee_obj_head, tee_obj);
+TAILQ_HEAD(tee_storage_enum_head, tee_storage_enum);
+
+struct user_ta_ctx {
+	uaddr_t entry_func;
+	bool is_32bit;		/* true if 32-bit ta, false if 64-bit ta */
+	/* list of sessions opened by this TA */
+	struct tee_ta_session_head open_sessions;
+	/* List of cryp states created by this TA */
+	struct tee_cryp_state_head cryp_states;
+	/* List of storage objects opened by this TA */
+	struct tee_obj_head objects;
+	/* List of storage enumerators opened by this TA */
+	struct tee_storage_enum_head storage_enums;
+	struct mobj *mobj_code; /* secure world memory */
+	struct mobj *mobj_stack; /* stack */
+	uint32_t load_addr;	/* elf load addr (from TAs address space) */
+	uint32_t context;	/* Context ID of the process */
+	struct tee_mmu_info *mmu;	/* Saved MMU information (ddr only) */
+	void *ta_time_offs;	/* Time reference used by the TA */
+	struct tee_pager_area_head *areas;
+#if defined(CFG_SE_API)
+	struct tee_se_service *se_service;
+#endif
+#if defined(CFG_WITH_VFP)
+	struct thread_user_vfp_state vfp;
+#endif
+	struct tee_ta_ctx ctx;
+
+};
+
+static inline bool is_user_ta_ctx(struct tee_ta_ctx *ctx)
+{
+	return !!(ctx->flags & TA_FLAG_USER_MODE);
+}
+
+static inline struct user_ta_ctx *to_user_ta_ctx(struct tee_ta_ctx *ctx)
+{
+	assert(is_user_ta_ctx(ctx));
+	return container_of(ctx, struct user_ta_ctx, ctx);
+}
+
+#ifdef CFG_WITH_USER_TA
+TEE_Result tee_ta_init_user_ta_session(const TEE_UUID *uuid,
+			struct tee_ta_session *s);
+#else
+static inline TEE_Result tee_ta_init_user_ta_session(
+			const TEE_UUID *uuid __unused,
+			struct tee_ta_session *s __unused)
+{
+	return TEE_ERROR_ITEM_NOT_FOUND;
+}
+#endif
+
+#endif /*KERNEL_USER_TA_H*/
diff --git a/core/arch/arm/include/kernel/vfp.h b/core/arch/arm/include/kernel/vfp.h
new file mode 100644
index 0000000..267dee2
--- /dev/null
+++ b/core/arch/arm/include/kernel/vfp.h
@@ -0,0 +1,127 @@
+/*
+ * 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.
+ */
+
+#ifndef KERNEL_VFP_H
+#define KERNEL_VFP_H
+
+#include <types_ext.h>
+#include <compiler.h>
+
+#ifdef ARM32
+/*
+ * Advanced SIMD/floating point state on ARMv7-A or ARMv8-A AArch32 has:
+ * - 32 64-bit data registers
+ * - FPSCR (32 bits)
+ * - FPEXC (32 bits)
+ */
+
+#define VFP_NUM_REGS	32
+
+struct vfp_reg {
+	uint64_t v;
+};
+
+struct vfp_state {
+	uint32_t fpexc;
+	uint32_t fpscr;
+	struct vfp_reg reg[VFP_NUM_REGS];
+};
+#endif
+
+#ifdef ARM64
+/*
+ * Advanced SIMD/floating point state on ARMv8-A AArch64 has:
+ * - 32 128-bit data registers
+ * - FPSR (32 bits)
+ * - FPCR (32 bits)
+ * - CPACR_EL1.FPEN (2 bits)
+ */
+
+#define VFP_NUM_REGS	32
+
+struct vfp_reg {
+	uint8_t v[16];
+} __aligned(16);
+
+struct vfp_state {
+	struct vfp_reg reg[VFP_NUM_REGS];
+	uint32_t fpsr;
+	uint32_t fpcr;
+	uint32_t cpacr_el1;
+	bool force_save; /* Save to reg even if VFP was not enabled */
+};
+#endif
+
+#ifdef CFG_WITH_VFP
+/* vfp_is_enabled() - Returns true if VFP is enabled */
+bool vfp_is_enabled(void);
+
+/* vfp_enable() - Enables vfp */
+void vfp_enable(void);
+
+/* vfp_disable() - Disables vfp */
+void vfp_disable(void);
+#else
+static inline bool vfp_is_enabled(void)
+{
+	return false;
+}
+
+static inline void vfp_enable(void)
+{
+}
+
+static inline void vfp_disable(void)
+{
+}
+#endif
+
+/*
+ * vfp_lazy_save_state_init() - Saves VFP enable status and disables VFP
+ * @state:	VFP state structure to initialize
+ */
+void vfp_lazy_save_state_init(struct vfp_state *state);
+
+/*
+ * vfp_lazy_save_state_final() - Saves rest of VFP state
+ * @state:	VFP state to save to
+ *
+ * If VFP was enabled when vfp_lazy_save_state_init() was called: save rest
+ * of state and disable VFP. Otherwise, do nothing.
+ */
+void vfp_lazy_save_state_final(struct vfp_state *state);
+
+/*
+ * vfp_lazy_restore_state() - Lazy restore VFP state
+ * @state:		VFP state to restore
+ *
+ * Restores VFP enable status and also restores rest of VFP state if
+ * vfp_lazy_save_state_final() was called on this state.
+ */
+void vfp_lazy_restore_state(struct vfp_state *state, bool full_state);
+
+#endif /*KERNEL_VFP_H*/
diff --git a/core/arch/arm/include/kernel/wait_queue.h b/core/arch/arm/include/kernel/wait_queue.h
new file mode 100644
index 0000000..eb8f881
--- /dev/null
+++ b/core/arch/arm/include/kernel/wait_queue.h
@@ -0,0 +1,85 @@
+/*
+ * 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.
+ */
+#ifndef KERNEL_WAIT_QUEUE_H
+#define KERNEL_WAIT_QUEUE_H
+
+#include <types_ext.h>
+#include <sys/queue.h>
+
+struct wait_queue_elem;
+SLIST_HEAD(wait_queue, wait_queue_elem);
+
+#define WAIT_QUEUE_INITIALIZER { .slh_first = NULL }
+
+struct condvar;
+struct wait_queue_elem {
+	short handle;
+	bool done;
+	struct condvar *cv;
+	SLIST_ENTRY(wait_queue_elem) link;
+};
+
+/*
+ * Initializes a wait queue
+ */
+void wq_init(struct wait_queue *wq);
+
+/*
+ * Initializes a wait queue element and adds it to the wait queue.  This
+ * function is supposed to be called before the lock that protects the
+ * resource we need to wait for is released.
+ *
+ * One call to this function must be followed by one call to wq_wait_final()
+ * on the same wait queue element.
+ */
+void wq_wait_init_condvar(struct wait_queue *wq, struct wait_queue_elem *wqe,
+			struct condvar *cv);
+
+static inline void wq_wait_init(struct wait_queue *wq,
+			struct wait_queue_elem *wqe)
+{
+	wq_wait_init_condvar(wq, wqe, NULL);
+}
+
+/* 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);
+
+/* 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,
+		const char *fname, int lineno);
+
+/* Returns true if the wait queue doesn't contain any elements */
+bool wq_is_empty(struct wait_queue *wq);
+
+void wq_promote_condvar(struct wait_queue *wq, struct condvar *cv,
+			bool only_one, const void *sync_obj, const char *fname,
+			int lineno);
+bool wq_have_condvar(struct wait_queue *wq, struct condvar *cv);
+
+#endif /*KERNEL_WAIT_QUEUE_H*/
+
diff --git a/core/arch/arm/include/mm/core_memprot.h b/core/arch/arm/include/mm/core_memprot.h
new file mode 100644
index 0000000..b7ccd21
--- /dev/null
+++ b/core/arch/arm/include/mm/core_memprot.h
@@ -0,0 +1,103 @@
+/*
+ * Copyright (c) 2016, Linaro Limited
+ * 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.
+ */
+#ifndef CORE_MEMPROT_H
+#define CORE_MEMPROT_H
+
+#include <mm/core_mmu.h>
+#include <types_ext.h>
+
+/*
+ * "pbuf_is" support.
+ *
+ * core_vbuf_is()/core_pbuf_is() can be used to check if a teecore mapped
+ * virtual address or a physical address is "Secure", "Unsecure", "external
+ * RAM" and some other fancy attributes.
+ *
+ * DO NOT use 'buf_is(Secure, buffer)==false' as a assumption that buffer is
+ * UnSecured ! This is NOT a valid asumption ! A buffer is certified UnSecured
+ * only if 'buf_is(UnSecure, buffer)==true'.
+ */
+
+/* memory atttributes */
+enum buf_is_attr {
+	CORE_MEM_SEC,
+	CORE_MEM_NON_SEC,
+	CORE_MEM_TEE_RAM,
+	CORE_MEM_TA_RAM,
+	CORE_MEM_NSEC_SHM,
+	CORE_MEM_EXTRAM,
+	CORE_MEM_INTRAM,
+	CORE_MEM_CACHED,
+};
+
+/* redirect legacy tee_vbuf_is() and tee_pbuf_is() to our routines */
+#define tee_pbuf_is     core_pbuf_is
+#define tee_vbuf_is     core_vbuf_is
+
+/* Convenience macros */
+#define tee_pbuf_is_non_sec(buf, len) \
+		core_pbuf_is(CORE_MEM_NON_SEC, (paddr_t)(buf), (len))
+
+#define tee_pbuf_is_sec(buf, len) \
+		core_pbuf_is(CORE_MEM_SEC, (paddr_t)(buf), (len))
+
+#define tee_vbuf_is_non_sec(buf, len) \
+		core_vbuf_is(CORE_MEM_NON_SEC, (void *)(buf), (len))
+
+#define tee_vbuf_is_sec(buf, len) \
+		core_vbuf_is(CORE_MEM_SEC, (void *)(buf), (len))
+
+/*
+ * This function return true if the buf complies with supplied flags.
+ * If this function returns false buf doesn't comply with supplied flags
+ * or something went wrong.
+ *
+ * Note that returning false doesn't guarantee that buf complies with
+ * the complement of the supplied flags.
+ */
+bool core_pbuf_is(uint32_t flags, paddr_t pbuf, size_t len);
+
+/*
+ * Translates the supplied virtual address to a physical address and uses
+ * tee_phys_buf_is() to check the compliance of the buffer.
+ */
+bool core_vbuf_is(uint32_t flags, const void *vbuf, size_t len);
+
+/*
+ * Translate physical address to virtual address using specified mapping
+ * Returns NULL on failure or a valid virtual address on success.
+ */
+void *phys_to_virt(paddr_t pa, enum teecore_memtypes m);
+
+/*
+ * Translate virtual address to physical address
+ * Returns 0 on failure or a valid physical address on success.
+ */
+paddr_t virt_to_phys(void *va);
+
+#endif /* CORE_MEMPROT_H */
diff --git a/core/arch/arm/include/mm/core_mmu.h b/core/arch/arm/include/mm/core_mmu.h
new file mode 100644
index 0000000..03ad93d
--- /dev/null
+++ b/core/arch/arm/include/mm/core_mmu.h
@@ -0,0 +1,399 @@
+/*
+ * Copyright (c) 2016, Linaro Limited
+ * 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.
+ */
+#ifndef CORE_MMU_H
+#define CORE_MMU_H
+
+#include <compiler.h>
+#include <kernel/user_ta.h>
+#include <mm/tee_mmu_types.h>
+#include <types_ext.h>
+
+/* A small page is the smallest unit of memory that can be mapped */
+#define SMALL_PAGE_SHIFT	12
+#define SMALL_PAGE_MASK		0x00000fff
+#define SMALL_PAGE_SIZE		0x00001000
+
+/*
+ * PGDIR is the translation table above the translation table that holds
+ * the pages.
+ */
+#ifdef CFG_WITH_LPAE
+#define CORE_MMU_PGDIR_SHIFT	21
+#else
+#define CORE_MMU_PGDIR_SHIFT	20
+#endif
+#define CORE_MMU_PGDIR_SIZE		(1 << CORE_MMU_PGDIR_SHIFT)
+#define CORE_MMU_PGDIR_MASK		(CORE_MMU_PGDIR_SIZE - 1)
+
+/* Devices are mapped using this granularity */
+#define CORE_MMU_DEVICE_SHIFT		CORE_MMU_PGDIR_SHIFT
+#define CORE_MMU_DEVICE_SIZE		(1 << CORE_MMU_DEVICE_SHIFT)
+#define CORE_MMU_DEVICE_MASK		(CORE_MMU_DEVICE_SIZE - 1)
+
+/* TA user space code, data, stack and heap are mapped using this granularity */
+#ifdef CFG_SMALL_PAGE_USER_TA
+#define CORE_MMU_USER_CODE_SHIFT	SMALL_PAGE_SHIFT
+#else
+#define CORE_MMU_USER_CODE_SHIFT	CORE_MMU_PGDIR_SHIFT
+#endif
+#define CORE_MMU_USER_CODE_SIZE		(1 << CORE_MMU_USER_CODE_SHIFT)
+#define CORE_MMU_USER_CODE_MASK		(CORE_MMU_USER_CODE_SIZE - 1)
+
+/* TA user space parameters are mapped using this granularity */
+#ifdef CFG_SMALL_PAGE_USER_TA
+#define CORE_MMU_USER_PARAM_SHIFT	SMALL_PAGE_SHIFT
+#else
+#define CORE_MMU_USER_PARAM_SHIFT	CORE_MMU_PGDIR_SHIFT
+#endif
+#define CORE_MMU_USER_PARAM_SIZE	(1 << CORE_MMU_USER_PARAM_SHIFT)
+#define CORE_MMU_USER_PARAM_MASK	(CORE_MMU_USER_PARAM_SIZE - 1)
+
+/*
+ * Memory area type:
+ * MEM_AREA_NOTYPE:   Undefined type. Used as end of table.
+ * MEM_AREA_TEE_RAM:  teecore execution RAM (secure, reserved to TEE, unused)
+ * MEM_AREA_TEE_COHERENT: teecore coherent RAM (secure, reserved to TEE)
+ * MEM_AREA_TA_RAM:   Secure RAM where teecore loads/exec TA instances.
+ * MEM_AREA_NSEC_SHM: NonSecure shared RAM between NSec and TEE.
+ * MEM_AREA_RAM_NSEC: NonSecure RAM storing data
+ * MEM_AREA_RAM_SEC:  Secure RAM storing some secrets
+ * MEM_AREA_IO_NSEC:  NonSecure HW mapped registers
+ * MEM_AREA_IO_SEC:   Secure HW mapped registers
+ * MEM_AREA_RES_VASPACE: Reserved virtual memory space
+ * MEM_AREA_TA_VASPACE: TA va space, only used with phys_to_virt()
+ * MEM_AREA_MAXTYPE:  lower invalid 'type' value
+ */
+enum teecore_memtypes {
+	MEM_AREA_NOTYPE = 0,
+	MEM_AREA_TEE_RAM,
+	MEM_AREA_TEE_COHERENT,
+	MEM_AREA_TA_RAM,
+	MEM_AREA_NSEC_SHM,
+	MEM_AREA_RAM_NSEC,
+	MEM_AREA_RAM_SEC,
+	MEM_AREA_IO_NSEC,
+	MEM_AREA_IO_SEC,
+	MEM_AREA_RES_VASPACE,
+	MEM_AREA_TA_VASPACE,
+	MEM_AREA_MAXTYPE
+};
+
+struct core_mmu_phys_mem {
+	const char *name;
+	enum teecore_memtypes type;
+	paddr_t addr;
+	size_t size;
+};
+
+#define register_phys_mem(type, addr, size) \
+	static const struct core_mmu_phys_mem __phys_mem_ ## addr \
+		__used __section("phys_mem_map_section") = \
+		{ #addr, (type), (addr), (size) }
+
+
+/* Default NSec shared memory allocated from NSec world */
+extern unsigned long default_nsec_shm_paddr;
+extern unsigned long default_nsec_shm_size;
+
+void core_init_mmu_map(void);
+void core_init_mmu_regs(void);
+
+bool core_mmu_place_tee_ram_at_top(paddr_t paddr);
+
+#ifdef CFG_WITH_LPAE
+/*
+ * struct core_mmu_user_map - current user mapping register state
+ * @user_map:	physical address of user map translation table
+ * @asid:	ASID for the user map
+ *
+ * Note that this struct should be treated as an opaque struct since
+ * the content depends on descriptor table format.
+ */
+struct core_mmu_user_map {
+	uint64_t user_map;
+	uint32_t asid;
+};
+#else
+/*
+ * struct core_mmu_user_map - current user mapping register state
+ * @ttbr0:	content of ttbr0
+ * @ctxid:	content of contextidr
+ *
+ * Note that this struct should be treated as an opaque struct since
+ * the content depends on descriptor table format.
+ */
+struct core_mmu_user_map {
+	uint32_t ttbr0;
+	uint32_t ctxid;
+};
+#endif
+
+#ifdef CFG_WITH_LPAE
+bool core_mmu_user_va_range_is_defined(void);
+#else
+static inline bool core_mmu_user_va_range_is_defined(void)
+{
+	return true;
+}
+#endif
+
+/*
+ * core_mmu_get_user_va_range() - Return range of user va space
+ * @base:	Lowest user virtual address
+ * @size:	Size in bytes of user address space
+ */
+void core_mmu_get_user_va_range(vaddr_t *base, size_t *size);
+
+/*
+ * enum core_mmu_fault - different kinds of faults
+ * @CORE_MMU_FAULT_ALIGNMENT:		alignment fault
+ * @CORE_MMU_FAULT_DEBUG_EVENT:		debug event
+ * @CORE_MMU_FAULT_TRANSLATION:		translation fault
+ * @CORE_MMU_FAULT_WRITE_PERMISSION:	Permission fault during write
+ * @CORE_MMU_FAULT_READ_PERMISSION:	Permission fault during read
+ * @CORE_MMU_FAULT_ASYNC_EXTERNAL:	asynchronous external abort
+ * @CORE_MMU_FAULT_ACCESS_BIT:		access bit fault
+ * @CORE_MMU_FAULT_OTHER:		Other/unknown fault
+ */
+enum core_mmu_fault {
+	CORE_MMU_FAULT_ALIGNMENT,
+	CORE_MMU_FAULT_DEBUG_EVENT,
+	CORE_MMU_FAULT_TRANSLATION,
+	CORE_MMU_FAULT_WRITE_PERMISSION,
+	CORE_MMU_FAULT_READ_PERMISSION,
+	CORE_MMU_FAULT_ASYNC_EXTERNAL,
+	CORE_MMU_FAULT_ACCESS_BIT,
+	CORE_MMU_FAULT_OTHER,
+};
+
+/*
+ * core_mmu_get_fault_type() - get fault type
+ * @fault_descr:	Content of fault status or exception syndrome register
+ * @returns an enum describing the content of fault status register.
+ */
+enum core_mmu_fault core_mmu_get_fault_type(uint32_t fault_descr);
+
+/*
+ * core_mm_type_to_attr() - convert memory type to attribute
+ * @t: memory type
+ * @returns an attribute that can be passed to core_mm_set_entry() and friends
+ */
+uint32_t core_mmu_type_to_attr(enum teecore_memtypes t);
+
+/*
+ * core_mmu_create_user_map() - Create user space mapping
+ * @utc:	Pointer to user TA context
+ * @map:	MMU configuration to use when activating this VA space
+ */
+void core_mmu_create_user_map(struct user_ta_ctx *utc,
+			      struct core_mmu_user_map *map);
+/*
+ * core_mmu_get_user_map() - Reads current MMU configuration for user VA space
+ * @map:	MMU configuration for current user VA space.
+ */
+void core_mmu_get_user_map(struct core_mmu_user_map *map);
+
+/*
+ * core_mmu_set_user_map() - Set new MMU configuration for user VA space
+ * @map:	If NULL will disable user VA space, if not NULL the user
+ *		VA space to activate.
+ */
+void core_mmu_set_user_map(struct core_mmu_user_map *map);
+
+/*
+ * struct core_mmu_table_info - Properties for a translation table
+ * @table:	Pointer to translation table
+ * @va_base:	VA base address of the transaltion table
+ * @level:	Translation table level
+ * @shift:	The shift of each entry in the table
+ * @num_entries: Number of entries in this table.
+ */
+struct core_mmu_table_info {
+	void *table;
+	vaddr_t va_base;
+	unsigned level;
+	unsigned shift;
+	unsigned num_entries;
+};
+
+/*
+ * core_mmu_find_table() - Locates a translation table
+ * @va:		Virtual address for the table to cover
+ * @max_level:	Don't traverse beyond this level
+ * @tbl_info:	Pointer to where to store properties.
+ * @return true if a translation table was found, false on error
+ */
+bool core_mmu_find_table(vaddr_t va, unsigned max_level,
+		struct core_mmu_table_info *tbl_info);
+
+/*
+ * core_mmu_divide_block() - divide larger block/section into smaller ones
+ * @tbl_info:	table where target record located
+ * @idx:	index of record
+ * @return true if function was able to divide block, false on error
+ */
+bool core_mmu_divide_block(struct core_mmu_table_info *tbl_info,
+			   unsigned int idx);
+
+void core_mmu_set_entry_primitive(void *table, size_t level, size_t idx,
+				  paddr_t pa, uint32_t attr);
+
+void core_mmu_get_user_pgdir(struct core_mmu_table_info *pgd_info);
+
+/*
+ * core_mmu_set_entry() - Set entry in translation table
+ * @tbl_info:	Translation table properties
+ * @idx:	Index of entry to update
+ * @pa:		Physical address to assign entry
+ * @attr:	Attributes to assign entry
+ */
+void core_mmu_set_entry(struct core_mmu_table_info *tbl_info, unsigned idx,
+			paddr_t pa, uint32_t attr);
+
+void core_mmu_get_entry_primitive(const void *table, size_t level, size_t idx,
+				  paddr_t *pa, uint32_t *attr);
+
+/*
+ * core_mmu_get_entry() - Get entry from translation table
+ * @tbl_info:	Translation table properties
+ * @idx:	Index of entry to read
+ * @pa:		Physical address is returned here if pa is not NULL
+ * @attr:	Attributues are returned here if attr is not NULL
+ */
+void core_mmu_get_entry(struct core_mmu_table_info *tbl_info, unsigned idx,
+			paddr_t *pa, uint32_t *attr);
+
+/*
+ * core_mmu_va2idx() - Translate from virtual address to table index
+ * @tbl_info:	Translation table properties
+ * @va:		Virtual address to translate
+ * @returns index in transaltion table
+ */
+static inline unsigned core_mmu_va2idx(struct core_mmu_table_info *tbl_info,
+			vaddr_t va)
+{
+	return (va - tbl_info->va_base) >> tbl_info->shift;
+}
+
+/*
+ * core_mmu_idx2va() - Translate from table index to virtual address
+ * @tbl_info:	Translation table properties
+ * @idx:	Index to translate
+ * @returns Virtual address
+ */
+static inline vaddr_t core_mmu_idx2va(struct core_mmu_table_info *tbl_info,
+			unsigned idx)
+{
+	return (idx << tbl_info->shift) + tbl_info->va_base;
+}
+
+/*
+ * core_mmu_get_block_offset() - Get offset inside a block/page
+ * @tbl_info:	Translation table properties
+ * @pa:		Physical address
+ * @returns offset within one block of the translation table
+ */
+static inline size_t core_mmu_get_block_offset(
+			struct core_mmu_table_info *tbl_info, paddr_t pa)
+{
+	return pa & ((1 << tbl_info->shift) - 1);
+}
+
+/*
+ * core_mmu_user_mapping_is_active() - Report if user mapping is active
+ * @returns true if a user VA space is active, false if user VA space is
+ *          inactive.
+ */
+bool core_mmu_user_mapping_is_active(void);
+
+/*
+ * core_mmu_mattr_is_ok() - Check that supplied mem attributes can be used
+ * @returns true if the attributes can be used, false if not.
+ */
+bool core_mmu_mattr_is_ok(uint32_t mattr);
+
+void core_mmu_get_mem_by_type(enum teecore_memtypes type, vaddr_t *s,
+			      vaddr_t *e);
+
+enum teecore_memtypes core_mmu_get_type_by_pa(paddr_t pa);
+
+/* Function is deprecated, use virt_to_phys() instead */
+int core_va2pa_helper(void *va, paddr_t *pa);
+
+/* routines to retreive shared mem configuration */
+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 */
+	TLBINV_CURRENT_ASID,	/* invalidate unified tlb for current ASID */
+	TLBINV_BY_ASID,		/* invalidate unified tlb by ASID */
+	TLBINV_BY_MVA,		/* invalidate unified tlb by MVA */
+};
+
+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;
+
+/* Check cpu mmu enabled or not */
+bool cpu_mmu_enabled(void);
+
+#endif /* CORE_MMU_H */
diff --git a/core/arch/arm/include/mm/mobj.h b/core/arch/arm/include/mm/mobj.h
new file mode 100644
index 0000000..d5eeb69
--- /dev/null
+++ b/core/arch/arm/include/mm/mobj.h
@@ -0,0 +1,130 @@
+/*
+ * Copyright (c) 2016-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 __MM_MOBJ_H
+#define __MM_MOBJ_H
+
+#include <compiler.h>
+#include <mm/core_memprot.h>
+#include <optee_msg.h>
+#include <sys/queue.h>
+#include <tee_api_types.h>
+#include <types_ext.h>
+
+
+struct mobj {
+	const struct mobj_ops *ops;
+	size_t size;
+};
+
+struct mobj_ops {
+	void *(*get_va)(struct mobj *mobj, size_t offs);
+	TEE_Result (*get_pa)(struct mobj *mobj, size_t offs, size_t granule,
+			     paddr_t *pa);
+	TEE_Result (*get_cattr)(struct mobj *mobj, uint32_t *cattr);
+	bool (*matches)(struct mobj *mobj, enum buf_is_attr attr);
+	void (*free)(struct mobj *mobj);
+	void (*update_mapping)(struct mobj *mobj, struct user_ta_ctx *utc,
+			       vaddr_t va);
+};
+
+extern struct mobj mobj_virt;
+extern struct mobj *mobj_sec_ddr;
+
+static inline void *mobj_get_va(struct mobj *mobj, size_t offset)
+{
+	if (mobj && mobj->ops && mobj->ops->get_va)
+		return mobj->ops->get_va(mobj, offset);
+	return NULL;
+}
+
+static inline TEE_Result mobj_get_pa(struct mobj *mobj, size_t offs,
+				     size_t granule, paddr_t *pa)
+{
+	if (mobj && mobj->ops && mobj->ops->get_pa)
+		return mobj->ops->get_pa(mobj, offs, granule, pa);
+	return TEE_ERROR_GENERIC;
+}
+
+static inline TEE_Result mobj_get_cattr(struct mobj *mobj, uint32_t *cattr)
+{
+	if (mobj && mobj->ops && mobj->ops->get_cattr)
+		return mobj->ops->get_cattr(mobj, cattr);
+	return TEE_ERROR_GENERIC;
+}
+
+static inline bool mobj_matches(struct mobj *mobj, enum buf_is_attr attr)
+{
+	if (mobj && mobj->ops && mobj->ops->matches)
+		return mobj->ops->matches(mobj, attr);
+	return false;
+}
+
+static inline void mobj_free(struct mobj *mobj)
+{
+	if (mobj && mobj->ops && mobj->ops->free)
+		mobj->ops->free(mobj);
+}
+
+
+static inline void mobj_update_mapping(struct mobj *mobj,
+				       struct user_ta_ctx *utc, vaddr_t va)
+{
+	if (mobj && mobj->ops && mobj->ops->update_mapping)
+		mobj->ops->update_mapping(mobj, utc, va);
+}
+
+static inline bool mobj_is_nonsec(struct mobj *mobj)
+{
+	return mobj_matches(mobj, CORE_MEM_NON_SEC);
+}
+
+static inline bool mobj_is_secure(struct mobj *mobj)
+{
+	return mobj_matches(mobj, CORE_MEM_SEC);
+}
+
+struct mobj *mobj_mm_alloc(struct mobj *mobj_parent, size_t size,
+			   tee_mm_pool_t *pool);
+
+struct mobj *mobj_phys_alloc(paddr_t pa, size_t size, uint32_t cattr,
+			     enum buf_is_attr battr);
+
+struct mobj *mobj_paged_alloc(size_t size);
+
+#ifdef CFG_PAGED_USER_TA
+bool mobj_is_paged(struct mobj *mobj);
+#else
+static inline bool mobj_is_paged(struct mobj *mobj __unused)
+{
+	return false;
+}
+#endif
+
+struct mobj *mobj_seccpy_shm_alloc(size_t size);
+
+#endif /*__MM_MOBJ_H*/
diff --git a/core/arch/arm/include/mm/pgt_cache.h b/core/arch/arm/include/mm/pgt_cache.h
new file mode 100644
index 0000000..8812758
--- /dev/null
+++ b/core/arch/arm/include/mm/pgt_cache.h
@@ -0,0 +1,144 @@
+/*
+ * Copyright (c) 2016, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+#ifndef MM_PGT_CACHE_H
+#define MM_PGT_CACHE_H
+
+#ifdef CFG_WITH_LPAE
+#define PGT_SIZE	(4 * 1024)
+#define PGT_NUM_PGT_PER_PAGE	1
+#else
+#define PGT_SIZE	(1 * 1024)
+#define PGT_NUM_PGT_PER_PAGE	4
+#endif
+
+#include <kernel/tee_ta_manager.h>
+#include <sys/queue.h>
+#include <types_ext.h>
+#include <util.h>
+
+struct pgt {
+	void *tbl;
+#if defined(CFG_PAGED_USER_TA)
+	vaddr_t vabase;
+	struct tee_ta_ctx *ctx;
+	size_t num_used_entries;
+#endif
+#if defined(CFG_WITH_PAGER)
+#if !defined(CFG_WITH_LPAE)
+	struct pgt_parent *parent;
+#endif
+#endif
+#ifdef CFG_SMALL_PAGE_USER_TA
+	SLIST_ENTRY(pgt) link;
+#endif
+};
+
+#ifdef CFG_SMALL_PAGE_USER_TA
+/*
+ * Reserve 2 page tables per thread, but at least 4 page tables in total
+ */
+#if CFG_NUM_THREADS < 2
+#define PGT_CACHE_SIZE	4
+#else
+#define PGT_CACHE_SIZE	ROUNDUP(CFG_NUM_THREADS * 2, PGT_NUM_PGT_PER_PAGE)
+#endif
+
+SLIST_HEAD(pgt_cache, pgt);
+
+static inline bool pgt_check_avail(size_t num_tbls)
+{
+	return num_tbls <= PGT_CACHE_SIZE;
+}
+
+void pgt_alloc(struct pgt_cache *pgt_cache, void *owning_ctx,
+	       vaddr_t begin, vaddr_t last);
+void pgt_free(struct pgt_cache *pgt_cache, bool save_ctx);
+
+#ifdef CFG_PAGED_USER_TA
+void pgt_flush_ctx_range(struct pgt_cache *pgt_cache, void *ctx,
+			 vaddr_t begin, vaddr_t last);
+#else
+static inline void pgt_flush_ctx_range(struct pgt_cache *pgt_cache __unused,
+				       void *ctx __unused,
+				       vaddr_t begin __unused,
+				       vaddr_t last __unused)
+{
+}
+#endif
+
+void pgt_transfer(struct pgt_cache *pgt_cache, void *old_ctx, vaddr_t old_va,
+		  void *new_ctx, vaddr_t new_va, size_t size);
+
+void pgt_init(void);
+
+#else
+
+static inline void pgt_init(void)
+{
+}
+
+#endif
+
+#if defined(CFG_PAGED_USER_TA)
+void pgt_flush_ctx(struct tee_ta_ctx *ctx);
+
+static inline void pgt_inc_used_entries(struct pgt *pgt)
+{
+	pgt->num_used_entries++;
+}
+
+static inline void pgt_dec_used_entries(struct pgt *pgt)
+{
+	pgt->num_used_entries--;
+}
+
+static inline void pgt_set_used_entries(struct pgt *pgt, size_t val)
+{
+	pgt->num_used_entries = val;
+}
+
+#else
+static inline void pgt_flush_ctx(struct tee_ta_ctx *ctx __unused)
+{
+}
+
+static inline void pgt_inc_used_entries(struct pgt *pgt __unused)
+{
+}
+
+static inline void pgt_dec_used_entries(struct pgt *pgt __unused)
+{
+}
+
+static inline void pgt_set_used_entries(struct pgt *pgt __unused,
+					size_t val __unused)
+{
+}
+
+#endif
+
+#endif /*MM_PGT_CACHE_H*/
diff --git a/core/arch/arm/include/mm/tee_pager.h b/core/arch/arm/include/mm/tee_pager.h
new file mode 100644
index 0000000..d48577a
--- /dev/null
+++ b/core/arch/arm/include/mm/tee_pager.h
@@ -0,0 +1,226 @@
+/*
+ * Copyright (c) 2016, Linaro Limited
+ * 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.
+ */
+
+#ifndef MM_TEE_PAGER_H
+#define MM_TEE_PAGER_H
+
+#include <kernel/abort.h>
+#include <kernel/panic.h>
+#include <kernel/user_ta.h>
+#include <mm/tee_mm.h>
+#include <string.h>
+#include <trace.h>
+
+/*
+ * Reference to translation table used to map the virtual memory range
+ * covered by the pager.
+ */
+extern struct core_mmu_table_info tee_pager_tbl_info;
+
+struct tee_pager_area_head;
+
+/*
+ * tee_pager_init() - Initialized the pager
+ * @mm_alias:	The alias area where all physical pages managed by the
+ *		pager are aliased
+ *
+ * Panics if called twice or some other error occurs.
+ */
+void tee_pager_init(tee_mm_entry_t *mm_alias);
+
+/*
+ * tee_pager_add_core_area() - Adds a pageable core area
+ * @base:	base of covered memory area
+ * @size:	size of covered memory area
+ * @flags:	describes attributes of mapping
+ * @store:	backing store for the memory area
+ * @hashes:	hashes of the pages in the backing store
+ *
+ * TEE_MATTR_PW		- read-write mapping else read-only mapping
+ * TEE_MATTR_PX		- executable mapping
+ * TEE_MATTR_LOCKED	- on demand locked mapping, requires TEE_MATTR_PW,
+ *			  will only be unmapped by a call to
+ *			  tee_pager_release_phys()
+ *
+ * !TEE_MATTR_PW requires store and hashes to be !NULL while
+ * TEE_MATTR_PW requires store and hashes to be NULL.
+ *
+ * Invalid use of flags or non-page aligned base or size or size == 0 will
+ * cause a panic.
+ *
+ * Return true on success or false if area can't be added.
+ */
+bool tee_pager_add_core_area(vaddr_t base, size_t size, uint32_t flags,
+			const void *store, const void *hashes);
+
+/*
+ * tee_pager_add_uta_area() - Adds a pageable user ta area
+ * @utc:	user ta context of the area
+ * @base:	base of covered memory area
+ * @size:	size of covered memory area
+ *
+ * The mapping is created suitable to initialize the memory content while
+ * loading the TA. Once the TA is properly loaded the areas should be
+ * finalized with tee_pager_set_uta_area_attr() to get more strict settings.
+ *
+ * Return true on success of false if the area can't be added
+ */
+bool tee_pager_add_uta_area(struct user_ta_ctx *utc, vaddr_t base, size_t size);
+
+/*
+ * tee_pager_set_uta_area_attr() - Set attributes of a initialized memory area
+ * @utc:	user ta context of the area
+ * @base:	base of covered memory area
+ * @size:	size of covered memory area
+ * @flags:	TEE_MATTR_U* flags describing permissions of the area
+ *
+ * Return true on success of false if the area can't be updated
+ */
+bool tee_pager_set_uta_area_attr(struct user_ta_ctx *utc, vaddr_t base,
+				 size_t size, uint32_t flags);
+
+void tee_pager_transfer_uta_region(struct user_ta_ctx *src_utc,
+				   vaddr_t src_base,
+				   struct user_ta_ctx *dst_utc,
+				   vaddr_t dst_base, struct pgt **dst_pgt,
+				   size_t size);
+void tee_pager_rem_uta_region(struct user_ta_ctx *utc, vaddr_t base,
+			      size_t size);
+
+/*
+ * tee_pager_rem_uta_areas() - Remove all user ta areas
+ * @utc:	user ta context
+ *
+ * This function is called when a user ta context is teared down.
+ */
+#ifdef CFG_PAGED_USER_TA
+void tee_pager_rem_uta_areas(struct user_ta_ctx *utc);
+#else
+static inline void tee_pager_rem_uta_areas(struct user_ta_ctx *utc __unused)
+{
+}
+#endif
+
+/*
+ * tee_pager_assign_uta_tables() - Assigns translation table to a user ta
+ * @utc:	user ta context
+ *
+ * This function is called to assign translation tables for the pageable
+ * areas of a user TA.
+ */
+#ifdef CFG_PAGED_USER_TA
+void tee_pager_assign_uta_tables(struct user_ta_ctx *utc);
+#else
+static inline void tee_pager_assign_uta_tables(struct user_ta_ctx *utc __unused)
+{
+}
+#endif
+
+/*
+ * Adds physical pages to the pager to use. The supplied virtual address range
+ * is searched for mapped physical pages and unmapped pages are ignored.
+ *
+ * vaddr is the first virtual address
+ * npages is the number of pages to add
+ */
+void tee_pager_add_pages(vaddr_t vaddr, size_t npages, bool unmap);
+
+/*
+ * tee_pager_alloc() - Allocate read-write virtual memory from pager.
+ * @size:	size of memory in bytes
+ * @flags:	flags for allocation
+ *
+ * Allocates read-write memory from pager, all flags but the optional
+ * TEE_MATTR_LOCKED is ignored.
+ *
+ * @return NULL on failure or a pointer to the virtual memory on success.
+ */
+void *tee_pager_alloc(size_t size, uint32_t flags);
+
+#ifdef CFG_PAGED_USER_TA
+/*
+ * tee_pager_pgt_save_and_release_entries() - Save dirty pages to backing store
+ * and remove physical page from translation table
+ * @pgt: page table descriptor
+ *
+ * This function is called when a translation table needs to be recycled
+ */
+void tee_pager_pgt_save_and_release_entries(struct pgt *pgt);
+#endif
+
+/*
+ * tee_pager_release_phys() - Release physical pages used for mapping
+ * @addr:	virtual address of first page to release
+ * @size:	number of bytes to release
+ *
+ * Only pages completely covered by the supplied range are affected.  This
+ * function only supplies a hint to the pager that the physical page can be
+ * reused. The caller can't expect a released memory range to hold a
+ * specific bit pattern when used next time.
+ *
+ * Note that the virtual memory allocation is still valid after this
+ * function has returned, it's just the content that may or may not have
+ * changed.
+ */
+#ifdef CFG_WITH_PAGER
+void tee_pager_release_phys(void *addr, size_t size);
+#else
+static inline void tee_pager_release_phys(void *addr __unused,
+			size_t size __unused)
+{
+}
+#endif
+
+/*
+ * Statistics on the pager
+ */
+struct tee_pager_stats {
+	size_t hidden_hits;
+	size_t ro_hits;
+	size_t rw_hits;
+	size_t zi_released;
+	size_t npages;		/* number of load pages */
+	size_t npages_all;	/* number of pages */
+};
+
+#ifdef CFG_WITH_PAGER
+void tee_pager_get_stats(struct tee_pager_stats *stats);
+bool tee_pager_handle_fault(struct abort_info *ai);
+#else /*CFG_WITH_PAGER*/
+static inline bool tee_pager_handle_fault(struct abort_info *ai __unused)
+{
+	return false;
+}
+
+static inline void tee_pager_get_stats(struct tee_pager_stats *stats)
+{
+	memset(stats, 0, sizeof(struct tee_pager_stats));
+}
+#endif /*CFG_WITH_PAGER*/
+
+#endif /*MM_TEE_PAGER_H*/
diff --git a/core/arch/arm/include/sm/optee_smc.h b/core/arch/arm/include/sm/optee_smc.h
new file mode 100644
index 0000000..b6fcd65
--- /dev/null
+++ b/core/arch/arm/include/sm/optee_smc.h
@@ -0,0 +1,533 @@
+/*
+ * 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.
+ */
+#ifndef OPTEE_SMC_H
+#define OPTEE_SMC_H
+
+/*
+ * This file is exported by OP-TEE and is in kept in sync between secure
+ * world and normal world kernel driver. We're following ARM SMC Calling
+ * Convention as specified in
+ * http://infocenter.arm.com/help/topic/com.arm.doc.den0028a/index.html
+ *
+ * This file depends on optee_msg.h being included to expand the SMC id
+ * macros below.
+ */
+
+#define OPTEE_SMC_32			0
+#define OPTEE_SMC_64			0x40000000
+#define OPTEE_SMC_FAST_CALL		0x80000000
+#define OPTEE_SMC_STD_CALL		0
+
+#define OPTEE_SMC_OWNER_MASK		0x3F
+#define OPTEE_SMC_OWNER_SHIFT		24
+
+#define OPTEE_SMC_FUNC_MASK		0xFFFF
+
+#define OPTEE_SMC_IS_FAST_CALL(smc_val)	((smc_val) & OPTEE_SMC_FAST_CALL)
+#define OPTEE_SMC_IS_64(smc_val)	((smc_val) & OPTEE_SMC_64)
+#define OPTEE_SMC_FUNC_NUM(smc_val)	((smc_val) & OPTEE_SMC_FUNC_MASK)
+#define OPTEE_SMC_OWNER_NUM(smc_val) \
+	(((smc_val) >> OPTEE_SMC_OWNER_SHIFT) & OPTEE_SMC_OWNER_MASK)
+
+#define OPTEE_SMC_CALL_VAL(type, calling_convention, owner, func_num) \
+			((type) | (calling_convention) | \
+			(((owner) & OPTEE_SMC_OWNER_MASK) << \
+				OPTEE_SMC_OWNER_SHIFT) |\
+			((func_num) & OPTEE_SMC_FUNC_MASK))
+
+#define OPTEE_SMC_STD_CALL_VAL(func_num) \
+	OPTEE_SMC_CALL_VAL(OPTEE_SMC_32, OPTEE_SMC_STD_CALL, \
+			   OPTEE_SMC_OWNER_TRUSTED_OS, (func_num))
+#define OPTEE_SMC_FAST_CALL_VAL(func_num) \
+	OPTEE_SMC_CALL_VAL(OPTEE_SMC_32, OPTEE_SMC_FAST_CALL, \
+			   OPTEE_SMC_OWNER_TRUSTED_OS, (func_num))
+
+#define OPTEE_SMC_OWNER_ARCH		0
+#define OPTEE_SMC_OWNER_CPU		1
+#define OPTEE_SMC_OWNER_SIP		2
+#define OPTEE_SMC_OWNER_OEM		3
+#define OPTEE_SMC_OWNER_STANDARD	4
+#define OPTEE_SMC_OWNER_TRUSTED_APP	48
+#define OPTEE_SMC_OWNER_TRUSTED_OS	50
+
+#define OPTEE_SMC_OWNER_TRUSTED_OS_OPTEED 62
+#define OPTEE_SMC_OWNER_TRUSTED_OS_API	63
+
+/*
+ * Function specified by SMC Calling convention.
+ */
+#define OPTEE_SMC_FUNCID_CALLS_COUNT	0xFF00
+#define OPTEE_SMC_CALLS_COUNT \
+	OPTEE_SMC_CALL_VAL(OPTEE_SMC_32, OPTEE_SMC_FAST_CALL, \
+			   OPTEE_SMC_OWNER_TRUSTED_OS_API, \
+			   OPTEE_SMC_FUNCID_CALLS_COUNT)
+
+/*
+ * Normal cached memory (write-back), shareable for SMP systems and not
+ * shareable for UP systems.
+ */
+#define OPTEE_SMC_SHM_CACHED		1
+
+/*
+ * a0..a7 is used as register names in the descriptions below, on arm32
+ * that translates to r0..r7 and on arm64 to w0..w7. In both cases it's
+ * 32-bit registers.
+ */
+
+/*
+ * Function specified by SMC Calling convention
+ *
+ * Return the following UID if using API specified in this file
+ * without further extensions:
+ * 384fb3e0-e7f8-11e3-af63-0002a5d5c51b.
+ * see also OPTEE_MSG_UID_* in optee_msg.h
+ */
+#define OPTEE_SMC_FUNCID_CALLS_UID OPTEE_MSG_FUNCID_CALLS_UID
+#define OPTEE_SMC_CALLS_UID \
+	OPTEE_SMC_CALL_VAL(OPTEE_SMC_32, OPTEE_SMC_FAST_CALL, \
+			   OPTEE_SMC_OWNER_TRUSTED_OS_API, \
+			   OPTEE_SMC_FUNCID_CALLS_UID)
+
+/*
+ * Function specified by SMC Calling convention
+ *
+ * Returns 2.0 if using API specified in this file without further extensions.
+ * see also OPTEE_MSG_REVISION_* in optee_msg.h
+ */
+#define OPTEE_SMC_FUNCID_CALLS_REVISION OPTEE_MSG_FUNCID_CALLS_REVISION
+#define OPTEE_SMC_CALLS_REVISION \
+	OPTEE_SMC_CALL_VAL(OPTEE_SMC_32, OPTEE_SMC_FAST_CALL, \
+			   OPTEE_SMC_OWNER_TRUSTED_OS_API, \
+			   OPTEE_SMC_FUNCID_CALLS_REVISION)
+
+/*
+ * Get UUID of Trusted OS.
+ *
+ * Used by non-secure world to figure out which Trusted OS is installed.
+ * Note that returned UUID is the UUID of the Trusted OS, not of the API.
+ *
+ * Returns UUID in a0-4 in the same way as OPTEE_SMC_CALLS_UID
+ * described above.
+ */
+#define OPTEE_SMC_FUNCID_GET_OS_UUID OPTEE_MSG_FUNCID_GET_OS_UUID
+#define OPTEE_SMC_CALL_GET_OS_UUID \
+	OPTEE_SMC_FAST_CALL_VAL(OPTEE_SMC_FUNCID_GET_OS_UUID)
+
+/*
+ * Get revision of Trusted OS.
+ *
+ * Used by non-secure world to figure out which version of the Trusted OS
+ * is installed. Note that the returned revision is the revision of the
+ * Trusted OS, not of the API.
+ *
+ * Returns revision in a0-1 in the same way as OPTEE_SMC_CALLS_REVISION
+ * described above.
+ */
+#define OPTEE_SMC_FUNCID_GET_OS_REVISION OPTEE_MSG_FUNCID_GET_OS_REVISION
+#define OPTEE_SMC_CALL_GET_OS_REVISION \
+	OPTEE_SMC_FAST_CALL_VAL(OPTEE_SMC_FUNCID_GET_OS_REVISION)
+
+/*
+ * Call with struct optee_msg_arg as argument
+ *
+ * Call register usage:
+ * a0	SMC Function ID, OPTEE_SMC*CALL_WITH_ARG
+ * a1	Upper 32 bits of a 64-bit physical pointer to a struct optee_msg_arg
+ * a2	Lower 32 bits of a 64-bit physical pointer to a struct optee_msg_arg
+ * a3	Cache settings, not used if physical pointer is in a predefined shared
+ *	memory area else per OPTEE_SMC_SHM_*
+ * a4-6	Not used
+ * a7	Hypervisor Client ID register
+ *
+ * Normal return register usage:
+ * a0	Return value, OPTEE_SMC_RETURN_*
+ * a1-3	Not used
+ * a4-7	Preserved
+ *
+ * OPTEE_SMC_RETURN_ETHREAD_LIMIT return register usage:
+ * a0	Return value, OPTEE_SMC_RETURN_ETHREAD_LIMIT
+ * a1-3	Preserved
+ * a4-7	Preserved
+ *
+ * RPC return register usage:
+ * a0	Return value, OPTEE_SMC_RETURN_IS_RPC(val)
+ * a1-2	RPC parameters
+ * a3-7	Resume information, must be preserved
+ *
+ * Possible return values:
+ * OPTEE_SMC_RETURN_UNKNOWN_FUNCTION	Trusted OS does not recognize this
+ *					function.
+ * OPTEE_SMC_RETURN_OK			Call completed, result updated in
+ *					the previously supplied struct
+ *					optee_msg_arg.
+ * OPTEE_SMC_RETURN_ETHREAD_LIMIT	Number of Trusted OS threads exceeded,
+ *					try again later.
+ * OPTEE_SMC_RETURN_EBADADDR		Bad physical pointer to struct
+ *					optee_msg_arg.
+ * OPTEE_SMC_RETURN_EBADCMD		Bad/unknown cmd in struct optee_msg_arg
+ * OPTEE_SMC_RETURN_IS_RPC()		Call suspended by RPC call to normal
+ *					world.
+ */
+#define OPTEE_SMC_FUNCID_CALL_WITH_ARG OPTEE_MSG_FUNCID_CALL_WITH_ARG
+#define OPTEE_SMC_CALL_WITH_ARG \
+	OPTEE_SMC_STD_CALL_VAL(OPTEE_SMC_FUNCID_CALL_WITH_ARG)
+
+/*
+ * Get Shared Memory Config
+ *
+ * Returns the Secure/Non-secure shared memory config.
+ *
+ * Call register usage:
+ * a0	SMC Function ID, OPTEE_SMC_GET_SHM_CONFIG
+ * a1-6	Not used
+ * a7	Hypervisor Client ID register
+ *
+ * Have config return register usage:
+ * a0	OPTEE_SMC_RETURN_OK
+ * a1	Physical address of start of SHM
+ * a2	Size of of SHM
+ * a3	Cache settings of memory, as defined by the
+ *	OPTEE_SMC_SHM_* values above
+ * a4-7	Preserved
+ *
+ * Not available register usage:
+ * a0	OPTEE_SMC_RETURN_ENOTAVAIL
+ * a1-3 Not used
+ * a4-7	Preserved
+ */
+#define OPTEE_SMC_FUNCID_GET_SHM_CONFIG	7
+#define OPTEE_SMC_GET_SHM_CONFIG \
+	OPTEE_SMC_FAST_CALL_VAL(OPTEE_SMC_FUNCID_GET_SHM_CONFIG)
+
+/*
+ * Configures L2CC mutex
+ *
+ * Disables, enables usage of L2CC mutex. Returns or sets physical address
+ * of L2CC mutex.
+ *
+ * Call register usage:
+ * a0	SMC Function ID, OPTEE_SMC_L2CC_MUTEX
+ * a1	OPTEE_SMC_L2CC_MUTEX_GET_ADDR	Get physical address of mutex
+ *	OPTEE_SMC_L2CC_MUTEX_SET_ADDR	Set physical address of mutex
+ *	OPTEE_SMC_L2CC_MUTEX_ENABLE	Enable usage of mutex
+ *	OPTEE_SMC_L2CC_MUTEX_DISABLE	Disable usage of mutex
+ * a2	if a1 == OPTEE_SMC_L2CC_MUTEX_SET_ADDR, upper 32bit of a 64bit
+ *      physical address of mutex
+ * a3	if a1 == OPTEE_SMC_L2CC_MUTEX_SET_ADDR, lower 32bit of a 64bit
+ *      physical address of mutex
+ * a3-6	Not used
+ * a7	Hypervisor Client ID register
+ *
+ * Have config return register usage:
+ * a0	OPTEE_SMC_RETURN_OK
+ * a1	Preserved
+ * a2	if a1 == OPTEE_SMC_L2CC_MUTEX_GET_ADDR, upper 32bit of a 64bit
+ *      physical address of mutex
+ * a3	if a1 == OPTEE_SMC_L2CC_MUTEX_GET_ADDR, lower 32bit of a 64bit
+ *      physical address of mutex
+ * a3-7	Preserved
+ *
+ * Error return register usage:
+ * a0	OPTEE_SMC_RETURN_ENOTAVAIL	Physical address not available
+ *	OPTEE_SMC_RETURN_EBADADDR	Bad supplied physical address
+ *	OPTEE_SMC_RETURN_EBADCMD	Unsupported value in a1
+ * a1-7	Preserved
+ */
+#define OPTEE_SMC_L2CC_MUTEX_GET_ADDR	0
+#define OPTEE_SMC_L2CC_MUTEX_SET_ADDR	1
+#define OPTEE_SMC_L2CC_MUTEX_ENABLE	2
+#define OPTEE_SMC_L2CC_MUTEX_DISABLE	3
+#define OPTEE_SMC_FUNCID_L2CC_MUTEX	8
+#define OPTEE_SMC_L2CC_MUTEX \
+	OPTEE_SMC_FAST_CALL_VAL(OPTEE_SMC_FUNCID_L2CC_MUTEX)
+
+/*
+ * Exchanges capabilities between normal world and secure world
+ *
+ * Call register usage:
+ * a0	SMC Function ID, OPTEE_SMC_EXCHANGE_CAPABILITIES
+ * a1	bitfield of normal world capabilities OPTEE_SMC_NSEC_CAP_*
+ * a2-6	Not used
+ * a7	Hypervisor Client ID register
+ *
+ * Normal return register usage:
+ * a0	OPTEE_SMC_RETURN_OK
+ * a1	bitfield of secure world capabilities OPTEE_SMC_SEC_CAP_*
+ * a2-7	Preserved
+ *
+ * Error return register usage:
+ * a0	OPTEE_SMC_RETURN_ENOTAVAIL, can't use the capabilities from normal world
+ * a1	bitfield of secure world capabilities OPTEE_SMC_SEC_CAP_*
+ * a2-7 Preserved
+ */
+/* Normal world works as a uniprocessor system */
+#define OPTEE_SMC_NSEC_CAP_UNIPROCESSOR		(1 << 0)
+/* Secure world has reserved shared memory for normal world to use */
+#define OPTEE_SMC_SEC_CAP_HAVE_RESERVED_SHM	(1 << 0)
+/* Secure world can communicate via previously unregistered shared memory */
+#define OPTEE_SMC_SEC_CAP_UNREGISTERED_SHM	(1 << 1)
+#define OPTEE_SMC_FUNCID_EXCHANGE_CAPABILITIES	9
+#define OPTEE_SMC_EXCHANGE_CAPABILITIES \
+	OPTEE_SMC_FAST_CALL_VAL(OPTEE_SMC_FUNCID_EXCHANGE_CAPABILITIES)
+
+/*
+ * Disable and empties cache of shared memory objects
+ *
+ * Secure world can cache frequently used shared memory objects, for
+ * example objects used as RPC arguments. When secure world is idle this
+ * function returns one shared memory reference to free. To disable the
+ * cache and free all cached objects this function has to be called until
+ * it returns OPTEE_SMC_RETURN_ENOTAVAIL.
+ *
+ * Call register usage:
+ * a0	SMC Function ID, OPTEE_SMC_DISABLE_SHM_CACHE
+ * a1-6	Not used
+ * a7	Hypervisor Client ID register
+ *
+ * Normal return register usage:
+ * a0	OPTEE_SMC_RETURN_OK
+ * a1	Upper 32 bits of a 64-bit Shared memory cookie
+ * a2	Lower 32 bits of a 64-bit Shared memory cookie
+ * a3-7	Preserved
+ *
+ * Cache empty return register usage:
+ * a0	OPTEE_SMC_RETURN_ENOTAVAIL
+ * a1-7	Preserved
+ *
+ * Not idle return register usage:
+ * a0	OPTEE_SMC_RETURN_EBUSY
+ * a1-7	Preserved
+ */
+#define OPTEE_SMC_FUNCID_DISABLE_SHM_CACHE	10
+#define OPTEE_SMC_DISABLE_SHM_CACHE \
+	OPTEE_SMC_FAST_CALL_VAL(OPTEE_SMC_FUNCID_DISABLE_SHM_CACHE)
+
+/*
+ * Enable cache of shared memory objects
+ *
+ * Secure world can cache frequently used shared memory objects, for
+ * example objects used as RPC arguments. When secure world is idle this
+ * function returns OPTEE_SMC_RETURN_OK and the cache is enabled. If
+ * secure world isn't idle OPTEE_SMC_RETURN_EBUSY is returned.
+ *
+ * Call register usage:
+ * a0	SMC Function ID, OPTEE_SMC_ENABLE_SHM_CACHE
+ * a1-6	Not used
+ * a7	Hypervisor Client ID register
+ *
+ * Normal return register usage:
+ * a0	OPTEE_SMC_RETURN_OK
+ * a1-7	Preserved
+ *
+ * Not idle return register usage:
+ * a0	OPTEE_SMC_RETURN_EBUSY
+ * a1-7	Preserved
+ */
+#define OPTEE_SMC_FUNCID_ENABLE_SHM_CACHE	11
+#define OPTEE_SMC_ENABLE_SHM_CACHE \
+	OPTEE_SMC_FAST_CALL_VAL(OPTEE_SMC_FUNCID_ENABLE_SHM_CACHE)
+
+/*
+ * Release of secondary cores
+ *
+ * OP-TEE in secure world is in charge of the release process of secondary
+ * cores. The Rich OS issue the this request to ask OP-TEE to boot up the
+ * secondary cores, go through the OP-TEE per-core initialization, and then
+ * switch to the Non-seCure world with the Rich OS provided entry address.
+ * The secondary cores enter Non-Secure world in SVC mode, with Thumb, FIQ,
+ * IRQ and Abort bits disabled.
+ *
+ * Call register usage:
+ * a0	SMC Function ID, OPTEE_SMC_BOOT_SECONDARY
+ * a1	Index of secondary core to boot
+ * a2	Upper 32 bits of a 64-bit Non-Secure world entry physical address
+ * a3	Lower 32 bits of a 64-bit Non-Secure world entry physical address
+ * a4-7	Not used
+ *
+ * Normal return register usage:
+ * a0	OPTEE_SMC_RETURN_OK
+ * a1-7	Preserved
+ *
+ * Error return:
+ * a0	OPTEE_SMC_RETURN_EBADCMD		Core index out of range
+ * a1-7	Preserved
+ *
+ * Not idle return register usage:
+ * a0	OPTEE_SMC_RETURN_EBUSY
+ * a1-7	Preserved
+ */
+#define OPTEE_SMC_FUNCID_BOOT_SECONDARY  12
+#define OPTEE_SMC_BOOT_SECONDARY \
+	OPTEE_SMC_FAST_CALL_VAL(OPTEE_SMC_FUNCID_BOOT_SECONDARY)
+
+/*
+ * Resume from RPC (for example after processing an IRQ)
+ *
+ * Call register usage:
+ * a0	SMC Function ID, OPTEE_SMC_CALL_RETURN_FROM_RPC
+ * a1-3	Value of a1-3 when OPTEE_SMC_CALL_WITH_ARG returned
+ *	OPTEE_SMC_RETURN_RPC in a0
+ *
+ * Return register usage is the same as for OPTEE_SMC_*CALL_WITH_ARG above.
+ *
+ * Possible return values
+ * OPTEE_SMC_RETURN_UNKNOWN_FUNCTION	Trusted OS does not recognize this
+ *					function.
+ * OPTEE_SMC_RETURN_OK			Original call completed, result
+ *					updated in the previously supplied.
+ *					struct optee_msg_arg
+ * OPTEE_SMC_RETURN_RPC			Call suspended by RPC call to normal
+ *					world.
+ * OPTEE_SMC_RETURN_ERESUME		Resume failed, the opaque resume
+ *					information was corrupt.
+ */
+#define OPTEE_SMC_FUNCID_RETURN_FROM_RPC	3
+#define OPTEE_SMC_CALL_RETURN_FROM_RPC \
+	OPTEE_SMC_STD_CALL_VAL(OPTEE_SMC_FUNCID_RETURN_FROM_RPC)
+
+#define OPTEE_SMC_RETURN_RPC_PREFIX_MASK	0xFFFF0000
+#define OPTEE_SMC_RETURN_RPC_PREFIX		0xFFFF0000
+#define OPTEE_SMC_RETURN_RPC_FUNC_MASK		0x0000FFFF
+
+#define OPTEE_SMC_RETURN_GET_RPC_FUNC(ret) \
+	((ret) & OPTEE_SMC_RETURN_RPC_FUNC_MASK)
+
+#define OPTEE_SMC_RPC_VAL(func)		((func) | OPTEE_SMC_RETURN_RPC_PREFIX)
+
+/*
+ * Allocate memory for RPC parameter passing. The memory is used to hold a
+ * struct optee_msg_arg.
+ *
+ * "Call" register usage:
+ * a0	This value, OPTEE_SMC_RETURN_RPC_ALLOC
+ * a1	Size in bytes of required argument memory
+ * a2	Not used
+ * a3	Resume information, must be preserved
+ * a4-5	Not used
+ * a6-7	Resume information, must be preserved
+ *
+ * "Return" register usage:
+ * a0	SMC Function ID, OPTEE_SMC_CALL_RETURN_FROM_RPC.
+ * a1	Upper 32 bits of 64-bit physical pointer to allocated
+ *	memory, (a1 == 0 && a2 == 0) if size was 0 or if memory can't
+ *	be allocated.
+ * a2	Lower 32 bits of 64-bit physical pointer to allocated
+ *	memory, (a1 == 0 && a2 == 0) if size was 0 or if memory can't
+ *	be allocated
+ * a3	Preserved
+ * a4	Upper 32 bits of 64-bit Shared memory cookie used when freeing
+ *	the memory or doing an RPC
+ * a5	Lower 32 bits of 64-bit Shared memory cookie used when freeing
+ *	the memory or doing an RPC
+ * a6-7	Preserved
+ */
+#define OPTEE_SMC_RPC_FUNC_ALLOC	0
+#define OPTEE_SMC_RETURN_RPC_ALLOC \
+	OPTEE_SMC_RPC_VAL(OPTEE_SMC_RPC_FUNC_ALLOC)
+
+/*
+ * Free memory previously allocated by OPTEE_SMC_RETURN_RPC_ALLOC
+ *
+ * "Call" register usage:
+ * a0	This value, OPTEE_SMC_RETURN_RPC_FREE
+ * a1	Upper 32 bits of 64-bit shared memory cookie belonging to this
+ *	argument memory
+ * a2	Lower 32 bits of 64-bit shared memory cookie belonging to this
+ *	argument memory
+ * a3-7	Resume information, must be preserved
+ *
+ * "Return" register usage:
+ * a0	SMC Function ID, OPTEE_SMC_CALL_RETURN_FROM_RPC.
+ * a1-2	Not used
+ * a3-7	Preserved
+ */
+#define OPTEE_SMC_RPC_FUNC_FREE		2
+#define OPTEE_SMC_RETURN_RPC_FREE \
+	OPTEE_SMC_RPC_VAL(OPTEE_SMC_RPC_FUNC_FREE)
+
+/*
+ * Deliver an IRQ in normal world.
+ *
+ * "Call" register usage:
+ * a0	OPTEE_SMC_RETURN_RPC_IRQ
+ * 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)
+
+/*
+ * Do an RPC request. The supplied struct optee_msg_arg tells which
+ * request to do and the parameters for the request. The following fields
+ * are used (the rest are unused):
+ * - cmd		the Request ID
+ * - ret		return value of the request, filled in by normal world
+ * - num_params		number of parameters for the request
+ * - params		the parameters
+ * - param_attrs	attributes of the parameters
+ *
+ * "Call" register usage:
+ * a0	OPTEE_SMC_RETURN_RPC_CMD
+ * a1	Upper 32 bits of a 64-bit Shared memory cookie holding a
+ *	struct optee_msg_arg, must be preserved, only the data should
+ *	be updated
+ * a2	Lower 32 bits of a 64-bit Shared memory cookie holding a
+ *	struct optee_msg_arg, must be preserved, only the data should
+ *	be updated
+ * a3-7	Resume information, must be preserved
+ *
+ * "Return" register usage:
+ * a0	SMC Function ID, OPTEE_SMC_CALL_RETURN_FROM_RPC.
+ * a1-2	Not used
+ * a3-7	Preserved
+ */
+#define OPTEE_SMC_RPC_FUNC_CMD		5
+#define OPTEE_SMC_RETURN_RPC_CMD \
+	OPTEE_SMC_RPC_VAL(OPTEE_SMC_RPC_FUNC_CMD)
+
+/* Returned in a0 */
+#define OPTEE_SMC_RETURN_UNKNOWN_FUNCTION 0xFFFFFFFF
+
+/* Returned in a0 only from Trusted OS functions */
+#define OPTEE_SMC_RETURN_OK		0x0
+#define OPTEE_SMC_RETURN_ETHREAD_LIMIT	0x1
+#define OPTEE_SMC_RETURN_EBUSY		0x2
+#define OPTEE_SMC_RETURN_ERESUME	0x3
+#define OPTEE_SMC_RETURN_EBADADDR	0x4
+#define OPTEE_SMC_RETURN_EBADCMD	0x5
+#define OPTEE_SMC_RETURN_ENOMEM		0x6
+#define OPTEE_SMC_RETURN_ENOTAVAIL	0x7
+#define OPTEE_SMC_RETURN_IS_RPC(ret) \
+	(((ret) != OPTEE_SMC_RETURN_UNKNOWN_FUNCTION) && \
+	((((ret) & OPTEE_SMC_RETURN_RPC_PREFIX_MASK) == \
+		OPTEE_SMC_RETURN_RPC_PREFIX)))
+
+#endif /* OPTEE_SMC_H */
diff --git a/core/arch/arm/include/sm/psci.h b/core/arch/arm/include/sm/psci.h
new file mode 100644
index 0000000..57d8f07
--- /dev/null
+++ b/core/arch/arm/include/sm/psci.h
@@ -0,0 +1,60 @@
+#include <kernel/thread.h>
+#include <stdint.h>
+
+#define PSCI_FN_BASE			(0x84000000U)
+#define PSCI_FN(n)			(PSCI_FN_BASE + (n))
+
+#define PSCI_VERSION_0_2		(0x00000002)
+#define PSCI_VERSION_1_0		(0x00010000)
+#define PSCI_VERSION			PSCI_FN(0)
+#define PSCI_CPU_SUSPEND		PSCI_FN(1)
+#define PSCI_CPU_OFF			PSCI_FN(2)
+#define PSCI_CPU_ON			PSCI_FN(3)
+#define PSCI_AFFINITY_INFO		PSCI_FN(4)
+#define PSCI_MIGRATE			PSCI_FN(5)
+#define PSCI_MIGRATE_INFO_TYPE		PSCI_FN(6)
+#define PSCI_MIGRATE_INFO_UP_CPU	PSCI_FN(7)
+#define PSCI_SYSTEM_OFF			PSCI_FN(8)
+#define PSCI_SYSTEM_RESET		PSCI_FN(9)
+#define PSCI_PSCI_FEATURES		PSCI_FN(10)
+#define PSCI_CPU_FREEZE			PSCI_FN(11)
+#define PSCI_CPU_DEFAULT_SUSPEND	PSCI_FN(12)
+#define PSCI_NODE_HW_STATE		PSCI_FN(13)
+#define PSCI_SYSTEM_SUSPEND		PSCI_FN(14)
+#define PSCI_PSCI_SET_SUSPEND_MODE	PSCI_FN(15)
+#define PSCI_FN_STAT_RESIDENCY		PSCI_FN(16)
+#define PSCI_FN_STAT_COUNT		PSCI_FN(17)
+
+#define PSCI_NUM_CALLS			18
+
+#define PSCI_AFFINITY_LEVEL_ON		0
+#define PSCI_AFFINITY_LEVEL_OFF		1
+#define PSCI_AFFINITY_LEVEL_ON_PENDING	2
+
+#define PSCI_RET_SUCCESS		(0)
+#define PSCI_RET_NOT_SUPPORTED		(-1)
+#define PSCI_RET_INVALID_PARAMETERS	(-2)
+#define PSCI_RET_DENIED			(-3)
+#define PSCI_RET_ALREADY_ON		(-4)
+#define PSCI_RET_ON_PENDING		(-5)
+#define PSCI_RET_INTERNAL_FAILURE	(-6)
+#define PSCI_RET_NOT_PRESENT		(-7)
+#define PSCI_RET_DISABLED		(-8)
+#define PSCI_RET_INVALID_ADDRESS	(-9)
+
+uint32_t psci_version(void);
+int psci_cpu_suspend(uint32_t power_state, uintptr_t entry,
+		     uint32_t context_id);
+int psci_cpu_off(void);
+int psci_cpu_on(uint32_t cpu_id, uint32_t entry, uint32_t context_id);
+int psci_affinity_info(uint32_t affinity, uint32_t lowest_affnity_level);
+int psci_migrate(uint32_t cpu_id);
+int psci_migrate_info_type(void);
+int psci_migrate_info_up_cpu(void);
+void psci_system_off(void);
+void psci_system_reset(void);
+int psci_features(uint32_t psci_fid);
+int psci_node_hw_state(uint32_t cpu_id, uint32_t power_level);
+int psci_stat_residency(uint32_t cpu_id, uint32_t power_state);
+int psci_stat_count(uint32_t cpu_id, uint32_t power_state);
+void tee_psci_handler(struct thread_smc_args *args);
diff --git a/core/arch/arm/include/sm/sm.h b/core/arch/arm/include/sm/sm.h
new file mode 100644
index 0000000..6368359
--- /dev/null
+++ b/core/arch/arm/include/sm/sm.h
@@ -0,0 +1,123 @@
+/*
+ * Copyright (c) 2016, Linaro Limited
+ * 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.
+ */
+
+#ifndef SM_SM_H
+#define SM_SM_H
+
+#include <types_ext.h>
+
+struct sm_mode_regs {
+	uint32_t usr_sp;
+	uint32_t usr_lr;
+	uint32_t irq_spsr;
+	uint32_t irq_sp;
+	uint32_t irq_lr;
+	uint32_t fiq_spsr;
+	uint32_t fiq_sp;
+	uint32_t fiq_lr;
+	/*
+	 * Note that fiq_r{8-12} are not saved here. Instead thread_fiq_handler
+	 * preserves r{8-12}.
+	 */
+	uint32_t svc_spsr;
+	uint32_t svc_sp;
+	uint32_t svc_lr;
+	uint32_t abt_spsr;
+	uint32_t abt_sp;
+	uint32_t abt_lr;
+	uint32_t und_spsr;
+	uint32_t und_sp;
+	uint32_t und_lr;
+};
+
+struct sm_nsec_ctx {
+	struct sm_mode_regs mode_regs;
+
+	uint32_t r8;
+	uint32_t r9;
+	uint32_t r10;
+	uint32_t r11;
+	uint32_t r12;
+
+	uint32_t r0;
+	uint32_t r1;
+	uint32_t r2;
+	uint32_t r3;
+	uint32_t r4;
+	uint32_t r5;
+	uint32_t r6;
+	uint32_t r7;
+
+	/* return state */
+	uint32_t mon_lr;
+	uint32_t mon_spsr;
+};
+
+struct sm_sec_ctx {
+	struct sm_mode_regs mode_regs;
+
+	uint32_t r0;
+	uint32_t r1;
+	uint32_t r2;
+	uint32_t r3;
+	uint32_t r4;
+	uint32_t r5;
+	uint32_t r6;
+	uint32_t r7;
+
+	/* return state */
+	uint32_t mon_lr;
+	uint32_t mon_spsr;
+};
+
+struct sm_ctx {
+	uint32_t pad;
+	struct sm_sec_ctx sec;
+	struct sm_nsec_ctx nsec;
+};
+
+/*
+ * The secure monitor reserves space at top of stack_tmp to hold struct
+ * sm_ctx.
+ */
+#define SM_STACK_TMP_RESERVE_SIZE	sizeof(struct sm_ctx)
+
+
+
+/* Returns storage location of non-secure context for current CPU */
+struct sm_nsec_ctx *sm_get_nsec_ctx(void);
+
+/* Returns stack pointer to use in monitor mode for current CPU */
+void *sm_get_sp(void);
+
+/*
+ * Initializes secure monitor, must be called by each CPU
+ */
+void sm_init(vaddr_t stack_pointer);
+
+#endif /*SM_SM_H*/
diff --git a/core/arch/arm/include/sm/std_smc.h b/core/arch/arm/include/sm/std_smc.h
new file mode 100644
index 0000000..2b2e54d
--- /dev/null
+++ b/core/arch/arm/include/sm/std_smc.h
@@ -0,0 +1,22 @@
+#ifndef __STD_SMC_H__
+#define __STD_SMC_H__
+
+/* SMC function IDs for Standard Service queries */
+
+#define ARM_STD_SVC_CALL_COUNT		0x8400ff00
+#define ARM_STD_SVC_UID			0x8400ff01
+/*                                      0x8400ff02 is reserved */
+#define ARM_STD_SVC_VERSION		0x8400ff03
+
+/* ARM Standard Service Calls version numbers */
+#define STD_SVC_VERSION_MAJOR		0x0
+#define STD_SVC_VERSION_MINOR		0x1
+
+/* The macros below are used to identify PSCI calls from the SMC function ID */
+#define PSCI_FID_MASK			0xffe0u
+#define PSCI_FID_VALUE			0u
+#define is_psci_fid(_fid) \
+	(((_fid) & PSCI_FID_MASK) == PSCI_FID_VALUE)
+
+void smc_std_handler(struct thread_smc_args *args);
+#endif
diff --git a/core/arch/arm/include/sm/tee_mon.h b/core/arch/arm/include/sm/tee_mon.h
new file mode 100644
index 0000000..725afb9
--- /dev/null
+++ b/core/arch/arm/include/sm/tee_mon.h
@@ -0,0 +1,38 @@
+/*
+ * 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.
+ */
+#ifndef TEE_MON_H
+#define TEE_MON_H
+
+#include <stdbool.h>
+#include <stdint.h>
+#include <stdarg.h>
+#include "tee_api_types.h"
+#include "user_ta_header.h"
+
+extern TEE_Result init_teecore(void);
+
+#endif /* TEE_MON_H */
diff --git a/core/arch/arm/include/sm/teesmc_opteed.h b/core/arch/arm/include/sm/teesmc_opteed.h
new file mode 100644
index 0000000..c6e25e2
--- /dev/null
+++ b/core/arch/arm/include/sm/teesmc_opteed.h
@@ -0,0 +1,142 @@
+/*
+ * Copyright (c) 2014, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef TEESMC_OPTEED_H
+#define TEESMC_OPTEED_H
+
+/*
+ * This file specify SMC function IDs used when returning from TEE to the
+ * secure monitor.
+ *
+ * All SMC Function IDs indicates SMC32 Calling Convention but will carry
+ * full 64 bit values in the argument registers if invoked from Aarch64
+ * mode. This violates the SMC Calling Convention, but since this
+ * convention only coveres API towards Normwal World it's something that
+ * only concerns the OP-TEE Dispatcher in ARM Trusted Firmware and OP-TEE
+ * OS at Secure EL1.
+ */
+
+/*
+ * Issued when returning from initial entry.
+ *
+ * Register usage:
+ * r0/x0	SMC Function ID, TEESMC_OPTEED_RETURN_ENTRY_DONE
+ * r1/x1	Pointer to entry vector
+ */
+#define TEESMC_OPTEED_FUNCID_RETURN_ENTRY_DONE		0
+#define TEESMC_OPTEED_RETURN_ENTRY_DONE \
+	TEESMC_OPTEED_RV(TEESMC_OPTEED_FUNCID_RETURN_ENTRY_DONE)
+
+
+
+/*
+ * Issued when returning from "cpu_on" vector
+ *
+ * Register usage:
+ * r0/x0	SMC Function ID, TEESMC_OPTEED_RETURN_ON_DONE
+ * r1/x1	0 on success and anything else to indicate error condition
+ */
+#define TEESMC_OPTEED_FUNCID_RETURN_ON_DONE		1
+#define TEESMC_OPTEED_RETURN_ON_DONE \
+	TEESMC_OPTEED_RV(TEESMC_OPTEED_FUNCID_RETURN_ON_DONE)
+
+/*
+ * Issued when returning from "cpu_off" vector
+ *
+ * Register usage:
+ * r0/x0	SMC Function ID, TEESMC_OPTEED_RETURN_OFF_DONE
+ * r1/x1	0 on success and anything else to indicate error condition
+ */
+#define TEESMC_OPTEED_FUNCID_RETURN_OFF_DONE		2
+#define TEESMC_OPTEED_RETURN_OFF_DONE \
+	TEESMC_OPTEED_RV(TEESMC_OPTEED_FUNCID_RETURN_OFF_DONE)
+
+/*
+ * Issued when returning from "cpu_suspend" vector
+ *
+ * Register usage:
+ * r0/x0	SMC Function ID, TEESMC_OPTEED_RETURN_SUSPEND_DONE
+ * r1/x1	0 on success and anything else to indicate error condition
+ */
+#define TEESMC_OPTEED_FUNCID_RETURN_SUSPEND_DONE	3
+#define TEESMC_OPTEED_RETURN_SUSPEND_DONE \
+	TEESMC_OPTEED_RV(TEESMC_OPTEED_FUNCID_RETURN_SUSPEND_DONE)
+
+/*
+ * Issued when returning from "cpu_resume" vector
+ *
+ * Register usage:
+ * r0/x0	SMC Function ID, TEESMC_OPTEED_RETURN_RESUME_DONE
+ * r1/x1	0 on success and anything else to indicate error condition
+ */
+#define TEESMC_OPTEED_FUNCID_RETURN_RESUME_DONE		4
+#define TEESMC_OPTEED_RETURN_RESUME_DONE \
+	TEESMC_OPTEED_RV(TEESMC_OPTEED_FUNCID_RETURN_RESUME_DONE)
+
+/*
+ * Issued when returning from "std_smc" or "fast_smc" vector
+ *
+ * Register usage:
+ * r0/x0	SMC Function ID, TEESMC_OPTEED_RETURN_CALL_DONE
+ * r1-4/x1-4	Return value 0-3 which will passed to normal world in
+ *		r0-3/x0-3
+ */
+#define TEESMC_OPTEED_FUNCID_RETURN_CALL_DONE		5
+#define TEESMC_OPTEED_RETURN_CALL_DONE \
+	TEESMC_OPTEED_RV(TEESMC_OPTEED_FUNCID_RETURN_CALL_DONE)
+
+/*
+ * Issued when returning from "fiq" vector
+ *
+ * Register usage:
+ * r0/x0	SMC Function ID, TEESMC_OPTEED_RETURN_FIQ_DONE
+ */
+#define TEESMC_OPTEED_FUNCID_RETURN_FIQ_DONE		6
+#define TEESMC_OPTEED_RETURN_FIQ_DONE \
+	TEESMC_OPTEED_RV(TEESMC_OPTEED_FUNCID_RETURN_FIQ_DONE)
+
+/*
+ * Issued when returning from "system_off" vector
+ *
+ * Register usage:
+ * r0/x0	SMC Function ID, TEESMC_OPTEED_RETURN_SYSTEM_OFF_DONE
+ */
+#define TEESMC_OPTEED_FUNCID_RETURN_SYSTEM_OFF_DONE	7
+#define TEESMC_OPTEED_RETURN_SYSTEM_OFF_DONE \
+	TEESMC_OPTEED_RV(TEESMC_OPTEED_FUNCID_RETURN_SYSTEM_OFF_DONE)
+
+/*
+ * Issued when returning from "system_reset" vector
+ *
+ * Register usage:
+ * r0/x0	SMC Function ID, TEESMC_OPTEED_RETURN_SYSTEM_RESET_DONE
+ */
+#define TEESMC_OPTEED_FUNCID_RETURN_SYSTEM_RESET_DONE	8
+#define TEESMC_OPTEED_RETURN_SYSTEM_RESET_DONE \
+	TEESMC_OPTEED_RV(TEESMC_OPTEED_FUNCID_RETURN_SYSTEM_RESET_DONE)
+
+#endif /*TEESMC_OPTEED_H*/
diff --git a/core/arch/arm/include/sm/teesmc_opteed_macros.h b/core/arch/arm/include/sm/teesmc_opteed_macros.h
new file mode 100644
index 0000000..00e9eed
--- /dev/null
+++ b/core/arch/arm/include/sm/teesmc_opteed_macros.h
@@ -0,0 +1,35 @@
+/*
+ * Copyright (c) 2014, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef TEESMC_OPTEED_MACROS_H
+#define TEESMC_OPTEED_MACROS_H
+
+#define TEESMC_OPTEED_RV(func_num) \
+	OPTEE_SMC_CALL_VAL(OPTEE_SMC_32, OPTEE_SMC_FAST_CALL, \
+			   OPTEE_SMC_OWNER_TRUSTED_OS_OPTEED, (func_num))
+
+#endif /*TEESMC_OPTEED_MACROS_H*/
diff --git a/core/arch/arm/include/tee/arch_svc.h b/core/arch/arm/include/tee/arch_svc.h
new file mode 100644
index 0000000..1848865
--- /dev/null
+++ b/core/arch/arm/include/tee/arch_svc.h
@@ -0,0 +1,43 @@
+/*
+ * Copyright (c) 2014, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+#ifndef TEE_ARCH_SVC_H
+#define TEE_ARCH_SVC_H
+
+struct thread_svc_regs;
+
+void tee_svc_handler(struct thread_svc_regs *regs);
+
+/*
+ * Called from the assembly functions syscall_sys_return() and
+ * syscall_panic() to update the register values in the struct
+ * thread_svc_regs to return back to TEE Core from an erlier call to
+ * thread_enter_user_mode().
+ */
+uint32_t tee_svc_sys_return_helper(uint32_t ret, bool panic,
+			uint32_t panic_code, struct thread_svc_regs *regs);
+
+#endif /*TEE_ARCH_SVC_H*/
diff --git a/core/arch/arm/include/tee/entry_fast.h b/core/arch/arm/include/tee/entry_fast.h
new file mode 100644
index 0000000..a9951f2
--- /dev/null
+++ b/core/arch/arm/include/tee/entry_fast.h
@@ -0,0 +1,52 @@
+/*
+ * Copyright (c) 2015, Linaro Limited
+ * All rights reserved.
+ * 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.
+ */
+
+#ifndef TEE_ENTRY_FAST_H
+#define TEE_ENTRY_FAST_H
+
+#include <kernel/thread.h>
+
+/* These functions are overridable by the specific target */
+void tee_entry_get_api_call_count(struct thread_smc_args *args);
+void tee_entry_get_api_uuid(struct thread_smc_args *args);
+void tee_entry_get_api_revision(struct thread_smc_args *args);
+void tee_entry_get_os_uuid(struct thread_smc_args *args);
+void tee_entry_get_os_revision(struct thread_smc_args *args);
+
+/*
+ * Returns the number of calls recognized by tee_entry(). Used by the
+ * specific target to calculate the total number of supported calls when
+ * overriding tee_entry_get_api_call_count().
+ */
+size_t tee_entry_generic_get_api_call_count(void);
+
+/* Fast call entry */
+void tee_entry_fast(struct thread_smc_args *args);
+
+#endif /* TEE_ENTRY_FAST_H */
diff --git a/core/arch/arm/include/tee/entry_std.h b/core/arch/arm/include/tee/entry_std.h
new file mode 100644
index 0000000..d545912
--- /dev/null
+++ b/core/arch/arm/include/tee/entry_std.h
@@ -0,0 +1,38 @@
+/*
+ * Copyright (c) 2015, Linaro Limited
+ * All rights reserved.
+ * 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.
+ */
+
+#ifndef TEE_ENTRY_STD_H
+#define TEE_ENTRY_STD_H
+
+#include <kernel/thread.h>
+
+/* Standard call entry */
+void tee_entry_std(struct thread_smc_args *args);
+
+#endif /* TEE_ENTRY_STD_H */
diff --git a/core/arch/arm/kernel/abort.c b/core/arch/arm/kernel/abort.c
new file mode 100644
index 0000000..3d29521
--- /dev/null
+++ b/core/arch/arm/kernel/abort.c
@@ -0,0 +1,582 @@
+/*
+ * 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/abort.h>
+#include <kernel/misc.h>
+#include <kernel/tee_ta_manager.h>
+#include <kernel/panic.h>
+#include <kernel/user_ta.h>
+#include <kernel/unwind.h>
+#include <mm/core_mmu.h>
+#include <mm/tee_pager.h>
+#include <tee/tee_svc.h>
+#include <trace.h>
+#include <arm.h>
+
+enum fault_type {
+	FAULT_TYPE_USER_TA_PANIC,
+	FAULT_TYPE_USER_TA_VFP,
+	FAULT_TYPE_PAGEABLE,
+	FAULT_TYPE_IGNORE,
+};
+
+#ifdef CFG_CORE_UNWIND
+#ifdef ARM32
+static void __print_stack_unwind(struct abort_info *ai)
+{
+	struct unwind_state state;
+
+	memset(&state, 0, sizeof(state));
+	state.registers[0] = ai->regs->r0;
+	state.registers[1] = ai->regs->r1;
+	state.registers[2] = ai->regs->r2;
+	state.registers[3] = ai->regs->r3;
+	state.registers[4] = ai->regs->r4;
+	state.registers[5] = ai->regs->r5;
+	state.registers[6] = ai->regs->r6;
+	state.registers[7] = ai->regs->r7;
+	state.registers[8] = ai->regs->r8;
+	state.registers[9] = ai->regs->r9;
+	state.registers[10] = ai->regs->r10;
+	state.registers[11] = ai->regs->r11;
+	state.registers[13] = read_mode_sp(ai->regs->spsr & CPSR_MODE_MASK);
+	state.registers[14] = read_mode_lr(ai->regs->spsr & CPSR_MODE_MASK);
+	state.registers[15] = ai->pc;
+
+	do {
+		EMSG_RAW(" pc 0x%08x", state.registers[15]);
+	} while (unwind_stack(&state));
+}
+#endif /*ARM32*/
+
+#ifdef ARM64
+static void __print_stack_unwind(struct abort_info *ai)
+{
+	struct unwind_state state;
+
+	memset(&state, 0, sizeof(state));
+	state.pc = ai->regs->elr;
+	state.fp = ai->regs->x29;
+
+	do {
+		EMSG_RAW("pc  0x%016" PRIx64, state.pc);
+	} while (unwind_stack(&state));
+}
+#endif /*ARM64*/
+
+static void print_stack_unwind(struct abort_info *ai)
+{
+	EMSG_RAW("Call stack:");
+	__print_stack_unwind(ai);
+}
+#else /*CFG_CORE_UNWIND*/
+static void print_stack_unwind(struct abort_info *ai __unused)
+{
+}
+#endif /*CFG_CORE_UNWIND*/
+
+static __maybe_unused const char *abort_type_to_str(uint32_t abort_type)
+{
+	if (abort_type == ABORT_TYPE_DATA)
+		return "data";
+	if (abort_type == ABORT_TYPE_PREFETCH)
+		return "prefetch";
+	return "undef";
+}
+
+static __maybe_unused const char *fault_to_str(uint32_t abort_type,
+			uint32_t fault_descr)
+{
+	/* fault_descr is only valid for data or prefetch abort */
+	if (abort_type != ABORT_TYPE_DATA && abort_type != ABORT_TYPE_PREFETCH)
+		return "";
+
+	switch (core_mmu_get_fault_type(fault_descr)) {
+	case CORE_MMU_FAULT_ALIGNMENT:
+		return " (alignment fault)";
+	case CORE_MMU_FAULT_TRANSLATION:
+		return " (translation fault)";
+	case CORE_MMU_FAULT_READ_PERMISSION:
+		return " (read permission fault)";
+	case CORE_MMU_FAULT_WRITE_PERMISSION:
+		return " (write permission fault)";
+	default:
+		return "";
+	}
+}
+
+static __maybe_unused void print_detailed_abort(
+				struct abort_info *ai __maybe_unused,
+				const char *ctx __maybe_unused)
+{
+	EMSG_RAW("\n");
+	EMSG_RAW("%s %s-abort at address 0x%" PRIxVA "%s\n",
+		ctx, abort_type_to_str(ai->abort_type), ai->va,
+		fault_to_str(ai->abort_type, ai->fault_descr));
+#ifdef ARM32
+	EMSG_RAW(" fsr 0x%08x  ttbr0 0x%08x  ttbr1 0x%08x  cidr 0x%X\n",
+		 ai->fault_descr, read_ttbr0(), read_ttbr1(),
+		 read_contextidr());
+	EMSG_RAW(" cpu #%zu          cpsr 0x%08x\n",
+		 get_core_pos(), ai->regs->spsr);
+	EMSG_RAW(" r0 0x%08x      r4 0x%08x    r8 0x%08x   r12 0x%08x\n",
+		 ai->regs->r0, ai->regs->r4, ai->regs->r8, ai->regs->ip);
+	EMSG_RAW(" r1 0x%08x      r5 0x%08x    r9 0x%08x    sp 0x%08x\n",
+		 ai->regs->r1, ai->regs->r5, ai->regs->r9,
+		 read_mode_sp(ai->regs->spsr & CPSR_MODE_MASK));
+	EMSG_RAW(" r2 0x%08x      r6 0x%08x   r10 0x%08x    lr 0x%08x\n",
+		 ai->regs->r2, ai->regs->r6, ai->regs->r10,
+		 read_mode_lr(ai->regs->spsr & CPSR_MODE_MASK));
+	EMSG_RAW(" r3 0x%08x      r7 0x%08x   r11 0x%08x    pc 0x%08x\n",
+		 ai->regs->r3, ai->regs->r7, ai->regs->r11, ai->pc);
+#endif /*ARM32*/
+#ifdef ARM64
+	EMSG_RAW(" esr 0x%08x  ttbr0 0x%08" PRIx64 "   ttbr1 0x%08" PRIx64 "   cidr 0x%X\n",
+		 ai->fault_descr, read_ttbr0_el1(), read_ttbr1_el1(),
+		 read_contextidr_el1());
+	EMSG_RAW(" cpu #%zu          cpsr 0x%08x\n",
+		 get_core_pos(), (uint32_t)ai->regs->spsr);
+	EMSG_RAW("x0  %016" PRIx64 " x1  %016" PRIx64,
+		 ai->regs->x0, ai->regs->x1);
+	EMSG_RAW("x2  %016" PRIx64 " x3  %016" PRIx64,
+		 ai->regs->x2, ai->regs->x3);
+	EMSG_RAW("x4  %016" PRIx64 " x5  %016" PRIx64,
+		 ai->regs->x4, ai->regs->x5);
+	EMSG_RAW("x6  %016" PRIx64 " x7  %016" PRIx64,
+		 ai->regs->x6, ai->regs->x7);
+	EMSG_RAW("x8  %016" PRIx64 " x9  %016" PRIx64,
+		 ai->regs->x8, ai->regs->x9);
+	EMSG_RAW("x10 %016" PRIx64 " x11 %016" PRIx64,
+		 ai->regs->x10, ai->regs->x11);
+	EMSG_RAW("x12 %016" PRIx64 " x13 %016" PRIx64,
+		 ai->regs->x12, ai->regs->x13);
+	EMSG_RAW("x14 %016" PRIx64 " x15 %016" PRIx64,
+		 ai->regs->x14, ai->regs->x15);
+	EMSG_RAW("x16 %016" PRIx64 " x17 %016" PRIx64,
+		 ai->regs->x16, ai->regs->x17);
+	EMSG_RAW("x18 %016" PRIx64 " x19 %016" PRIx64,
+		 ai->regs->x18, ai->regs->x19);
+	EMSG_RAW("x20 %016" PRIx64 " x21 %016" PRIx64,
+		 ai->regs->x20, ai->regs->x21);
+	EMSG_RAW("x22 %016" PRIx64 " x23 %016" PRIx64,
+		 ai->regs->x22, ai->regs->x23);
+	EMSG_RAW("x24 %016" PRIx64 " x25 %016" PRIx64,
+		 ai->regs->x24, ai->regs->x25);
+	EMSG_RAW("x26 %016" PRIx64 " x27 %016" PRIx64,
+		 ai->regs->x26, ai->regs->x27);
+	EMSG_RAW("x28 %016" PRIx64 " x29 %016" PRIx64,
+		 ai->regs->x28, ai->regs->x29);
+	EMSG_RAW("x30 %016" PRIx64 " elr %016" PRIx64,
+		 ai->regs->x30, ai->regs->elr);
+	EMSG_RAW("sp_el0 %016" PRIx64, ai->regs->sp_el0);
+#endif /*ARM64*/
+}
+
+static void print_user_abort(struct abort_info *ai __maybe_unused)
+{
+#ifdef CFG_TEE_CORE_TA_TRACE
+	print_detailed_abort(ai, "user TA");
+	tee_ta_dump_current();
+#endif
+}
+
+void abort_print(struct abort_info *ai __maybe_unused)
+{
+#if (TRACE_LEVEL >= TRACE_INFO)
+	print_detailed_abort(ai, "core");
+#endif /*TRACE_LEVEL >= TRACE_DEBUG*/
+}
+
+void abort_print_error(struct abort_info *ai)
+{
+#if (TRACE_LEVEL >= TRACE_INFO)
+	/* full verbose log at DEBUG level */
+	print_detailed_abort(ai, "core");
+#else
+#ifdef ARM32
+	EMSG("%s-abort at 0x%" PRIxVA "\n"
+	     "FSR 0x%x PC 0x%x TTBR0 0x%X CONTEXIDR 0x%X\n"
+	     "CPUID 0x%x CPSR 0x%x (read from SPSR)",
+	     abort_type_to_str(ai->abort_type),
+	     ai->va, ai->fault_descr, ai->pc, read_ttbr0(), read_contextidr(),
+	     read_mpidr(), read_spsr());
+#endif /*ARM32*/
+#ifdef ARM64
+	EMSG("%s-abort at 0x%" PRIxVA "\n"
+	     "ESR 0x%x PC 0x%x TTBR0 0x%" PRIx64 " CONTEXIDR 0x%X\n"
+	     "CPUID 0x%" PRIx64 " CPSR 0x%x (read from SPSR)",
+	     abort_type_to_str(ai->abort_type),
+	     ai->va, ai->fault_descr, ai->pc, read_ttbr0_el1(),
+	     read_contextidr_el1(),
+	     read_mpidr_el1(), (uint32_t)ai->regs->spsr);
+#endif /*ARM64*/
+#endif /*TRACE_LEVEL >= TRACE_DEBUG*/
+	print_stack_unwind(ai);
+}
+
+#ifdef ARM32
+static void set_abort_info(uint32_t abort_type, struct thread_abort_regs *regs,
+		struct abort_info *ai)
+{
+	switch (abort_type) {
+	case ABORT_TYPE_DATA:
+		ai->fault_descr = read_dfsr();
+		ai->va = read_dfar();
+		break;
+	case ABORT_TYPE_PREFETCH:
+		ai->fault_descr = read_ifsr();
+		ai->va = read_ifar();
+		break;
+	default:
+		ai->fault_descr = 0;
+		ai->va = regs->elr;
+		break;
+	}
+	ai->abort_type = abort_type;
+	ai->pc = regs->elr;
+	ai->regs = regs;
+}
+#endif /*ARM32*/
+
+#ifdef ARM64
+static void set_abort_info(uint32_t abort_type __unused,
+		struct thread_abort_regs *regs, struct abort_info *ai)
+{
+	ai->fault_descr = read_esr_el1();
+	switch ((ai->fault_descr >> ESR_EC_SHIFT) & ESR_EC_MASK) {
+	case ESR_EC_IABT_EL0:
+	case ESR_EC_IABT_EL1:
+		ai->abort_type = ABORT_TYPE_PREFETCH;
+		ai->va = read_far_el1();
+		break;
+	case ESR_EC_DABT_EL0:
+	case ESR_EC_DABT_EL1:
+	case ESR_EC_SP_ALIGN:
+		ai->abort_type = ABORT_TYPE_DATA;
+		ai->va = read_far_el1();
+		break;
+	default:
+		ai->abort_type = ABORT_TYPE_UNDEF;
+		ai->va = regs->elr;
+	}
+	ai->pc = regs->elr;
+	ai->regs = regs;
+}
+#endif /*ARM64*/
+
+#ifdef ARM32
+static void handle_user_ta_panic(struct abort_info *ai)
+{
+	/*
+	 * It was a user exception, stop user execution and return
+	 * to TEE Core.
+	 */
+	ai->regs->r0 = TEE_ERROR_TARGET_DEAD;
+	ai->regs->r1 = true;
+	ai->regs->r2 = 0xdeadbeef;
+	ai->regs->elr = (uint32_t)thread_unwind_user_mode;
+	ai->regs->spsr = read_cpsr();
+	ai->regs->spsr &= ~CPSR_MODE_MASK;
+	ai->regs->spsr |= CPSR_MODE_SVC;
+	ai->regs->spsr &= ~CPSR_FIA;
+	ai->regs->spsr |= read_spsr() & CPSR_FIA;
+	/* Select Thumb or ARM mode */
+	if (ai->regs->elr & 1)
+		ai->regs->spsr |= CPSR_T;
+	else
+		ai->regs->spsr &= ~CPSR_T;
+}
+#endif /*ARM32*/
+
+#ifdef ARM64
+static void handle_user_ta_panic(struct abort_info *ai)
+{
+	uint32_t daif;
+
+	/*
+	 * It was a user exception, stop user execution and return
+	 * to TEE Core.
+	 */
+	ai->regs->x0 = TEE_ERROR_TARGET_DEAD;
+	ai->regs->x1 = true;
+	ai->regs->x2 = 0xdeadbeef;
+	ai->regs->elr = (vaddr_t)thread_unwind_user_mode;
+	ai->regs->sp_el0 = thread_get_saved_thread_sp();
+
+	daif = (ai->regs->spsr >> SPSR_32_AIF_SHIFT) & SPSR_32_AIF_MASK;
+	/* XXX what about DAIF_D? */
+	ai->regs->spsr = SPSR_64(SPSR_64_MODE_EL1, SPSR_64_MODE_SP_EL0, daif);
+}
+#endif /*ARM64*/
+
+#ifdef CFG_WITH_VFP
+static void handle_user_ta_vfp(void)
+{
+	struct tee_ta_session *s;
+
+	if (tee_ta_get_current_session(&s) != TEE_SUCCESS)
+		panic();
+
+	thread_user_enable_vfp(&to_user_ta_ctx(s->ctx)->vfp);
+}
+#endif /*CFG_WITH_VFP*/
+
+#ifdef CFG_WITH_USER_TA
+#ifdef ARM32
+/* Returns true if the exception originated from user mode */
+bool abort_is_user_exception(struct abort_info *ai)
+{
+	return (ai->regs->spsr & ARM32_CPSR_MODE_MASK) == ARM32_CPSR_MODE_USR;
+}
+#endif /*ARM32*/
+
+#ifdef ARM64
+/* Returns true if the exception originated from user mode */
+bool abort_is_user_exception(struct abort_info *ai)
+{
+	uint32_t spsr = ai->regs->spsr;
+
+	if (spsr & (SPSR_MODE_RW_32 << SPSR_MODE_RW_SHIFT))
+		return true;
+	if (((spsr >> SPSR_64_MODE_EL_SHIFT) & SPSR_64_MODE_EL_MASK) ==
+	    SPSR_64_MODE_EL0)
+		return true;
+	return false;
+}
+#endif /*ARM64*/
+#else /*CFG_WITH_USER_TA*/
+bool abort_is_user_exception(struct abort_info *ai __unused)
+{
+	return false;
+}
+#endif /*CFG_WITH_USER_TA*/
+
+#ifdef ARM32
+/* Returns true if the exception originated from abort mode */
+static bool is_abort_in_abort_handler(struct abort_info *ai)
+{
+	return (ai->regs->spsr & ARM32_CPSR_MODE_MASK) == ARM32_CPSR_MODE_ABT;
+}
+#endif /*ARM32*/
+
+#ifdef ARM64
+/* Returns true if the exception originated from abort mode */
+static bool is_abort_in_abort_handler(struct abort_info *ai __unused)
+{
+	return false;
+}
+#endif /*ARM64*/
+
+
+#if defined(CFG_WITH_VFP) && defined(CFG_WITH_USER_TA)
+#ifdef ARM32
+
+#define T32_INSTR(w1, w0) \
+	((((uint32_t)(w0) & 0xffff) << 16) | ((uint32_t)(w1) & 0xffff))
+
+#define T32_VTRANS32_MASK	T32_INSTR(0xff << 8, (7 << 9) | 1 << 4)
+#define T32_VTRANS32_VAL	T32_INSTR(0xee << 8, (5 << 9) | 1 << 4)
+
+#define T32_VTRANS64_MASK	T32_INSTR((0xff << 8) | (7 << 5), 7 << 9)
+#define T32_VTRANS64_VAL	T32_INSTR((0xec << 8) | (2 << 5), 5 << 9)
+
+#define T32_VLDST_MASK		T32_INSTR((0xff << 8) | (1 << 4), 0)
+#define T32_VLDST_VAL		T32_INSTR( 0xf9 << 8            , 0)
+
+#define T32_VXLDST_MASK		T32_INSTR(0xfc << 8, 7 << 9)
+#define T32_VXLDST_VAL		T32_INSTR(0xec << 8, 5 << 9)
+
+#define T32_VPROC_MASK		T32_INSTR(0xef << 8, 0)
+#define T32_VPROC_VAL		T32_VPROC_MASK
+
+#define A32_INSTR(x)		((uint32_t)(x))
+
+#define A32_VTRANS32_MASK	A32_INSTR(SHIFT_U32(0xf, 24) | \
+					  SHIFT_U32(7, 9) | BIT32(4))
+#define A32_VTRANS32_VAL	A32_INSTR(SHIFT_U32(0xe, 24) | \
+					  SHIFT_U32(5, 9) | BIT32(4))
+
+#define A32_VTRANS64_MASK	A32_INSTR(SHIFT_U32(0x7f, 21) | SHIFT_U32(7, 9))
+#define A32_VTRANS64_VAL	A32_INSTR(SHIFT_U32(0x62, 21) | SHIFT_U32(5, 9))
+
+#define A32_VLDST_MASK		A32_INSTR(SHIFT_U32(0xff, 24) | BIT32(20))
+#define A32_VLDST_VAL		A32_INSTR(SHIFT_U32(0xf4, 24))
+#define A32_VXLDST_MASK		A32_INSTR(SHIFT_U32(7, 25) | SHIFT_U32(7, 9))
+#define A32_VXLDST_VAL		A32_INSTR(SHIFT_U32(6, 25) | SHIFT_U32(5, 9))
+
+#define A32_VPROC_MASK		A32_INSTR(SHIFT_U32(0x7f, 25))
+#define A32_VPROC_VAL		A32_INSTR(SHIFT_U32(0x79, 25))
+
+static bool is_vfp_fault(struct abort_info *ai)
+{
+	TEE_Result res;
+	uint32_t instr;
+
+	if ((ai->abort_type != ABORT_TYPE_UNDEF) || vfp_is_enabled())
+		return false;
+
+	res = tee_svc_copy_from_user(&instr, (void *)ai->pc, sizeof(instr));
+	if (res != TEE_SUCCESS)
+		return false;
+
+	if (ai->regs->spsr & CPSR_T) {
+		/* Thumb mode */
+		return ((instr & T32_VTRANS32_MASK) == T32_VTRANS32_VAL) ||
+		       ((instr & T32_VTRANS64_MASK) == T32_VTRANS64_VAL) ||
+		       ((instr & T32_VLDST_MASK) == T32_VLDST_VAL) ||
+		       ((instr & T32_VXLDST_MASK) == T32_VXLDST_VAL) ||
+		       ((instr & T32_VPROC_MASK) == T32_VPROC_VAL);
+	} else {
+		/* ARM mode */
+		return ((instr & A32_VTRANS32_MASK) == A32_VTRANS32_VAL) ||
+		       ((instr & A32_VTRANS64_MASK) == A32_VTRANS64_VAL) ||
+		       ((instr & A32_VLDST_MASK) == A32_VLDST_VAL) ||
+		       ((instr & A32_VXLDST_MASK) == A32_VXLDST_VAL) ||
+		       ((instr & A32_VPROC_MASK) == A32_VPROC_VAL);
+	}
+}
+#endif /*ARM32*/
+
+#ifdef ARM64
+static bool is_vfp_fault(struct abort_info *ai)
+{
+	switch ((ai->fault_descr >> ESR_EC_SHIFT) & ESR_EC_MASK) {
+	case ESR_EC_FP_ASIMD:
+	case ESR_EC_AARCH32_FP:
+	case ESR_EC_AARCH64_FP:
+		return true;
+	default:
+		return false;
+	}
+}
+#endif /*ARM64*/
+#else /*CFG_WITH_VFP && CFG_WITH_USER_TA*/
+static bool is_vfp_fault(struct abort_info *ai __unused)
+{
+	return false;
+}
+#endif  /*CFG_WITH_VFP && CFG_WITH_USER_TA*/
+
+static enum fault_type get_fault_type(struct abort_info *ai)
+{
+	if (abort_is_user_exception(ai)) {
+		if (is_vfp_fault(ai))
+			return FAULT_TYPE_USER_TA_VFP;
+#ifndef CFG_WITH_PAGER
+		return FAULT_TYPE_USER_TA_PANIC;
+#endif
+	}
+
+	if (is_abort_in_abort_handler(ai)) {
+		abort_print_error(ai);
+		panic("[abort] abort in abort handler (trap CPU)");
+	}
+
+	if (ai->abort_type == ABORT_TYPE_UNDEF) {
+		if (abort_is_user_exception(ai))
+			return FAULT_TYPE_USER_TA_PANIC;
+		abort_print_error(ai);
+		panic("[abort] undefined abort (trap CPU)");
+	}
+
+	switch (core_mmu_get_fault_type(ai->fault_descr)) {
+	case CORE_MMU_FAULT_ALIGNMENT:
+		if (abort_is_user_exception(ai))
+			return FAULT_TYPE_USER_TA_PANIC;
+		abort_print_error(ai);
+		panic("[abort] alignement fault!  (trap CPU)");
+		break;
+
+	case CORE_MMU_FAULT_ACCESS_BIT:
+		if (abort_is_user_exception(ai))
+			return FAULT_TYPE_USER_TA_PANIC;
+		abort_print_error(ai);
+		panic("[abort] access bit fault!  (trap CPU)");
+		break;
+
+	case CORE_MMU_FAULT_DEBUG_EVENT:
+		abort_print(ai);
+		DMSG("[abort] Ignoring debug event!");
+		return FAULT_TYPE_IGNORE;
+
+	case CORE_MMU_FAULT_TRANSLATION:
+	case CORE_MMU_FAULT_WRITE_PERMISSION:
+	case CORE_MMU_FAULT_READ_PERMISSION:
+		return FAULT_TYPE_PAGEABLE;
+
+	case CORE_MMU_FAULT_ASYNC_EXTERNAL:
+		abort_print(ai);
+		DMSG("[abort] Ignoring async external abort!");
+		return FAULT_TYPE_IGNORE;
+
+	case CORE_MMU_FAULT_OTHER:
+	default:
+		abort_print(ai);
+		DMSG("[abort] Unhandled fault!");
+		return FAULT_TYPE_IGNORE;
+	}
+}
+
+void abort_handler(uint32_t abort_type, struct thread_abort_regs *regs)
+{
+	struct abort_info ai;
+	bool handled;
+
+	set_abort_info(abort_type, regs, &ai);
+
+	switch (get_fault_type(&ai)) {
+	case FAULT_TYPE_IGNORE:
+		break;
+	case FAULT_TYPE_USER_TA_PANIC:
+		DMSG("[abort] abort in User mode (TA will panic)");
+		print_user_abort(&ai);
+		vfp_disable();
+		handle_user_ta_panic(&ai);
+		break;
+#ifdef CFG_WITH_VFP
+	case FAULT_TYPE_USER_TA_VFP:
+		handle_user_ta_vfp();
+		break;
+#endif
+	case FAULT_TYPE_PAGEABLE:
+	default:
+		thread_kernel_save_vfp();
+		handled = tee_pager_handle_fault(&ai);
+		thread_kernel_restore_vfp();
+		if (!handled) {
+			if (!abort_is_user_exception(&ai)) {
+				abort_print_error(&ai);
+				panic("unhandled pageable abort");
+			}
+			print_user_abort(&ai);
+			DMSG("[abort] abort in User mode (TA will panic)");
+			vfp_disable();
+			handle_user_ta_panic(&ai);
+		}
+		break;
+	}
+}
diff --git a/core/arch/arm/kernel/asm-defines.c b/core/arch/arm/kernel/asm-defines.c
new file mode 100644
index 0000000..99c0a63
--- /dev/null
+++ b/core/arch/arm/kernel/asm-defines.c
@@ -0,0 +1,107 @@
+/*
+ * Copyright (c) 2016, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <kernel/thread.h>
+#include <sm/sm.h>
+#include <types_ext.h>
+#include "thread_private.h"
+
+#define DEFINES void __defines(void); void __defines(void)
+
+#define DEFINE(def, val) \
+	asm volatile("\n==>" #def " %0 " #val : : "i" (val))
+
+DEFINES
+{
+#ifdef ARM32
+	DEFINE(SM_NSEC_CTX_R0, offsetof(struct sm_nsec_ctx, r0));
+	DEFINE(SM_NSEC_CTX_R8, offsetof(struct sm_nsec_ctx, r8));
+	DEFINE(SM_SEC_CTX_R0, offsetof(struct sm_sec_ctx, r0));
+	DEFINE(SM_SEC_CTX_MON_LR, offsetof(struct sm_sec_ctx, mon_lr));
+	DEFINE(SM_CTX_SIZE, sizeof(struct sm_ctx));
+	DEFINE(SM_CTX_NSEC, offsetof(struct sm_ctx, nsec));
+	DEFINE(SM_CTX_SEC, offsetof(struct sm_ctx, sec));
+
+	DEFINE(THREAD_VECTOR_TABLE_FIQ_ENTRY,
+	       offsetof(struct thread_vector_table, fiq_entry));
+
+	DEFINE(THREAD_SVC_REG_R0, offsetof(struct thread_svc_regs, r0));
+	DEFINE(THREAD_SVC_REG_R5, offsetof(struct thread_svc_regs, r5));
+	DEFINE(THREAD_SVC_REG_R6, offsetof(struct thread_svc_regs, r6));
+#endif /*ARM32*/
+
+#ifdef ARM64
+	DEFINE(THREAD_SMC_ARGS_X0, offsetof(struct thread_smc_args, a0));
+	DEFINE(THREAD_SMC_ARGS_SIZE, sizeof(struct thread_smc_args));
+
+	DEFINE(THREAD_SVC_REG_X0, offsetof(struct thread_svc_regs, x0));
+	DEFINE(THREAD_SVC_REG_X5, offsetof(struct thread_svc_regs, x5));
+	DEFINE(THREAD_SVC_REG_X6, offsetof(struct thread_svc_regs, x6));
+	DEFINE(THREAD_SVC_REG_X30, offsetof(struct thread_svc_regs, x30));
+	DEFINE(THREAD_SVC_REG_ELR, offsetof(struct thread_svc_regs, elr));
+	DEFINE(THREAD_SVC_REG_SPSR, offsetof(struct thread_svc_regs, spsr));
+	DEFINE(THREAD_SVC_REG_SP_EL0, offsetof(struct thread_svc_regs, sp_el0));
+	DEFINE(THREAD_SVC_REG_SIZE, sizeof(struct thread_svc_regs));
+
+	/* struct thread_abort_regs */
+	DEFINE(THREAD_ABT_REG_X0, offsetof(struct thread_abort_regs, x0));
+	DEFINE(THREAD_ABT_REG_X2, offsetof(struct thread_abort_regs, x2));
+	DEFINE(THREAD_ABT_REG_X30, offsetof(struct thread_abort_regs, x30));
+	DEFINE(THREAD_ABT_REG_SPSR, offsetof(struct thread_abort_regs, spsr));
+	DEFINE(THREAD_ABT_REGS_SIZE, sizeof(struct thread_abort_regs));
+
+	/* struct thread_ctx */
+	DEFINE(THREAD_CTX_KERN_SP, offsetof(struct thread_ctx, kern_sp));
+	DEFINE(THREAD_CTX_SIZE, sizeof(struct thread_ctx));
+
+	/* struct thread_ctx_regs */
+	DEFINE(THREAD_CTX_REGS_SP, offsetof(struct thread_ctx_regs, sp));
+	DEFINE(THREAD_CTX_REGS_X0, offsetof(struct thread_ctx_regs, x[0]));
+	DEFINE(THREAD_CTX_REGS_X1, offsetof(struct thread_ctx_regs, x[1]));
+	DEFINE(THREAD_CTX_REGS_X4, offsetof(struct thread_ctx_regs, x[4]));
+	DEFINE(THREAD_CTX_REGS_X19, offsetof(struct thread_ctx_regs, x[19]));
+
+	/* struct thread_user_mode_rec */
+	DEFINE(THREAD_USER_MODE_REC_EXIT_STATUS0_PTR,
+		offsetof(struct thread_user_mode_rec, exit_status0_ptr));
+	DEFINE(THREAD_USER_MODE_REC_X19,
+		offsetof(struct thread_user_mode_rec, x[0]));
+	DEFINE(THREAD_USER_MODE_REC_SIZE, sizeof(struct thread_user_mode_rec));
+
+	/* struct thread_core_local */
+	DEFINE(THREAD_CORE_LOCAL_TMP_STACK_VA_END,
+		offsetof(struct thread_core_local, tmp_stack_va_end));
+	DEFINE(THREAD_CORE_LOCAL_CURR_THREAD,
+		offsetof(struct thread_core_local, curr_thread));
+	DEFINE(THREAD_CORE_LOCAL_FLAGS,
+		offsetof(struct thread_core_local, flags));
+	DEFINE(THREAD_CORE_LOCAL_ABT_STACK_VA_END,
+		offsetof(struct thread_core_local, abt_stack_va_end));
+	DEFINE(THREAD_CORE_LOCAL_X0, offsetof(struct thread_core_local, x[0]));
+	DEFINE(THREAD_CORE_LOCAL_X2, offsetof(struct thread_core_local, x[2]));
+#endif /*ARM64*/
+}
diff --git a/core/arch/arm/kernel/cache_helpers_a64.S b/core/arch/arm/kernel/cache_helpers_a64.S
new file mode 100644
index 0000000..d3a0248
--- /dev/null
+++ b/core/arch/arm/kernel/cache_helpers_a64.S
@@ -0,0 +1,207 @@
+/*
+ * 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.
+ */
+
+/*
+ * Copyright (c) 2013-2014, ARM Limited and Contributors. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * 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.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE 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 <arm64.h>
+#include <asm.S>
+
+	.macro	dcache_line_size  reg, tmp
+	mrs	\tmp, ctr_el0
+	ubfx	\tmp, \tmp, #16, #4
+	mov	\reg, #4
+	lsl	\reg, \reg, \tmp
+	.endm
+
+	.macro	icache_line_size  reg, tmp
+	mrs	\tmp, ctr_el0
+	and	\tmp, \tmp, #0xf
+	mov	\reg, #4
+	lsl	\reg, \reg, \tmp
+	.endm
+
+
+	/* ------------------------------------------
+	 * Clean+Invalidate from base address till
+	 * size. 'x0' = addr, 'x1' = size
+	 * ------------------------------------------
+	 */
+FUNC flush_dcache_range , :
+	dcache_line_size x2, x3
+	add	x1, x0, x1
+	sub	x3, x2, #1
+	bic	x0, x0, x3
+flush_loop:
+	dc	civac, x0
+	add	x0, x0, x2
+	cmp	x0, x1
+	b.lo    flush_loop
+	dsb	sy
+	ret
+END_FUNC flush_dcache_range
+
+
+	/* ------------------------------------------
+	 * Invalidate from base address till
+	 * size. 'x0' = addr, 'x1' = size
+	 * ------------------------------------------
+	 */
+FUNC inv_dcache_range , :
+	dcache_line_size x2, x3
+	add	x1, x0, x1
+	sub	x3, x2, #1
+	bic	x0, x0, x3
+inv_loop:
+	dc	ivac, x0
+	add	x0, x0, x2
+	cmp	x0, x1
+	b.lo    inv_loop
+	dsb	sy
+	ret
+END_FUNC inv_dcache_range
+
+
+	/* ---------------------------------------------------------------
+	 * Data cache operations by set/way to the level specified
+	 *
+	 * The main function, do_dcsw_op requires:
+	 * x0: The operation type (0-2), as defined in arch.h
+	 * x3: The last cache level to operate on
+	 * x9: clidr_el1
+	 * and will carry out the operation on each data cache from level 0
+	 * to the level in x3 in sequence
+	 *
+	 * The dcsw_op macro sets up the x3 and x9 parameters based on
+	 * clidr_el1 cache information before invoking the main function
+	 * ---------------------------------------------------------------
+	 */
+
+	.macro	dcsw_op shift, fw, ls
+	mrs	x9, clidr_el1
+	ubfx	x3, x9, \shift, \fw
+	lsl	x3, x3, \ls
+	b	do_dcsw_op
+	.endm
+
+LOCAL_FUNC do_dcsw_op , :
+	cbz	x3, exit
+	mov	x10, xzr
+	adr	x14, dcsw_loop_table	// compute inner loop address
+	add	x14, x14, x0, lsl #5	// inner loop is 8x32-bit instructions
+	mov	x0, x9
+	mov	w8, #1
+loop1:
+	add	x2, x10, x10, lsr #1	// work out 3x current cache level
+	lsr	x1, x0, x2		// extract cache type bits from clidr
+	and	x1, x1, #7		// mask the bits for current cache only
+	cmp	x1, #2			// see what cache we have at this level
+	b.lt	level_done		// nothing to do if no cache or icache
+
+	msr	csselr_el1, x10		// select current cache level in csselr
+	isb				// isb to sych the new cssr&csidr
+	mrs	x1, ccsidr_el1		// read the new ccsidr
+	and	x2, x1, #7		// extract the length of the cache lines
+	add	x2, x2, #4		// add 4 (line length offset)
+	ubfx	x4, x1, #3, #10		// maximum way number
+	clz	w5, w4			// bit position of way size increment
+	lsl	w9, w4, w5		// w9 = aligned max way number
+	lsl	w16, w8, w5		// w16 = way number loop decrement
+	orr	w9, w10, w9		// w9 = combine way and cache number
+	ubfx	w6, w1, #13, #15	// w6 = max set number
+	lsl	w17, w8, w2		// w17 = set number loop decrement
+	dsb	sy			// barrier before we start this level
+	br	x14			// jump to DC operation specific loop
+
+	.macro	dcsw_loop _op
+loop2_\_op:
+	lsl	w7, w6, w2		// w7 = aligned max set number
+
+loop3_\_op:
+	orr	w11, w9, w7		// combine cache, way and set number
+	dc	\_op, x11
+	subs	w7, w7, w17		// decrement set number
+	b.ge	loop3_\_op
+
+	subs	x9, x9, x16		// decrement way number
+	b.ge	loop2_\_op
+
+	b	level_done
+	.endm
+
+level_done:
+	add	x10, x10, #2		// increment cache number
+	cmp	x3, x10
+	b.gt    loop1
+	msr	csselr_el1, xzr		// select cache level 0 in csselr
+	dsb	sy			// barrier to complete final cache op
+	isb
+exit:
+	ret
+
+dcsw_loop_table:
+	dcsw_loop isw
+	dcsw_loop cisw
+	dcsw_loop csw
+END_FUNC do_dcsw_op
+
+
+FUNC dcsw_op_louis , :
+	dcsw_op #CLIDR_LOUIS_SHIFT, #CLIDR_FIELD_WIDTH, #CSSELR_LEVEL_SHIFT
+END_FUNC dcsw_op_louis
+
+
+FUNC dcsw_op_all , :
+	dcsw_op #CLIDR_LOC_SHIFT, #CLIDR_FIELD_WIDTH, #CSSELR_LEVEL_SHIFT
+END_FUNC dcsw_op_all
diff --git a/core/arch/arm/kernel/elf32.h b/core/arch/arm/kernel/elf32.h
new file mode 100644
index 0000000..d374208
--- /dev/null
+++ b/core/arch/arm/kernel/elf32.h
@@ -0,0 +1,245 @@
+/*-
+ * Copyright (c) 1996-1998 John D. Polstra.
+ * 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 AUTHOR 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 AUTHOR 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.
+ *
+ * $FreeBSD$
+ */
+
+#ifndef _SYS_ELF32_H_
+#define _SYS_ELF32_H_ 1
+
+#include "elf_common.h"
+
+/*
+ * ELF definitions common to all 32-bit architectures.
+ */
+
+typedef uint32_t	Elf32_Addr;
+typedef uint16_t	Elf32_Half;
+typedef uint32_t	Elf32_Off;
+typedef int32_t		Elf32_Sword;
+typedef uint32_t	Elf32_Word;
+typedef uint64_t	Elf32_Lword;
+
+typedef Elf32_Word	Elf32_Hashelt;
+
+/* Non-standard class-dependent datatype used for abstraction. */
+typedef Elf32_Word	Elf32_Size;
+typedef Elf32_Sword	Elf32_Ssize;
+
+/*
+ * ELF header.
+ */
+
+typedef struct {
+	unsigned char	e_ident[EI_NIDENT];	/* File identification. */
+	Elf32_Half	e_type;		/* File type. */
+	Elf32_Half	e_machine;	/* Machine architecture. */
+	Elf32_Word	e_version;	/* ELF format version. */
+	Elf32_Addr	e_entry;	/* Entry point. */
+	Elf32_Off	e_phoff;	/* Program header file offset. */
+	Elf32_Off	e_shoff;	/* Section header file offset. */
+	Elf32_Word	e_flags;	/* Architecture-specific flags. */
+	Elf32_Half	e_ehsize;	/* Size of ELF header in bytes. */
+	Elf32_Half	e_phentsize;	/* Size of program header entry. */
+	Elf32_Half	e_phnum;	/* Number of program header entries. */
+	Elf32_Half	e_shentsize;	/* Size of section header entry. */
+	Elf32_Half	e_shnum;	/* Number of section header entries. */
+	Elf32_Half	e_shstrndx;	/* Section name strings section. */
+} Elf32_Ehdr;
+
+/*
+ * Section header.
+ */
+
+typedef struct {
+	Elf32_Word	sh_name;	/* Section name (index into the
+					   section header string table). */
+	Elf32_Word	sh_type;	/* Section type. */
+	Elf32_Word	sh_flags;	/* Section flags. */
+	Elf32_Addr	sh_addr;	/* Address in memory image. */
+	Elf32_Off	sh_offset;	/* Offset in file. */
+	Elf32_Word	sh_size;	/* Size in bytes. */
+	Elf32_Word	sh_link;	/* Index of a related section. */
+	Elf32_Word	sh_info;	/* Depends on section type. */
+	Elf32_Word	sh_addralign;	/* Alignment in bytes. */
+	Elf32_Word	sh_entsize;	/* Size of each entry in section. */
+} Elf32_Shdr;
+
+/*
+ * Program header.
+ */
+
+typedef struct {
+	Elf32_Word	p_type;		/* Entry type. */
+	Elf32_Off	p_offset;	/* File offset of contents. */
+	Elf32_Addr	p_vaddr;	/* Virtual address in memory image. */
+	Elf32_Addr	p_paddr;	/* Physical address (not used). */
+	Elf32_Word	p_filesz;	/* Size of contents in file. */
+	Elf32_Word	p_memsz;	/* Size of contents in memory. */
+	Elf32_Word	p_flags;	/* Access permission flags. */
+	Elf32_Word	p_align;	/* Alignment in memory and file. */
+} Elf32_Phdr;
+
+/*
+ * Dynamic structure.  The ".dynamic" section contains an array of them.
+ */
+
+typedef struct {
+	Elf32_Sword	d_tag;		/* Entry type. */
+	union {
+		Elf32_Word	d_val;	/* Integer value. */
+		Elf32_Addr	d_ptr;	/* Address value. */
+	} d_un;
+} Elf32_Dyn;
+
+/*
+ * Relocation entries.
+ */
+
+/* Relocations that don't need an addend field. */
+typedef struct {
+	Elf32_Addr	r_offset;	/* Location to be relocated. */
+	Elf32_Word	r_info;		/* Relocation type and symbol index. */
+} Elf32_Rel;
+
+/* Relocations that need an addend field. */
+typedef struct {
+	Elf32_Addr	r_offset;	/* Location to be relocated. */
+	Elf32_Word	r_info;		/* Relocation type and symbol index. */
+	Elf32_Sword	r_addend;	/* Addend. */
+} Elf32_Rela;
+
+/* Macros for accessing the fields of r_info. */
+#define ELF32_R_SYM(info)	((info) >> 8)
+#define ELF32_R_TYPE(info)	((unsigned char)(info))
+
+/* Macro for constructing r_info from field values. */
+#define ELF32_R_INFO(sym, type)	(((sym) << 8) + (unsigned char)(type))
+
+/*
+ *	Note entry header
+ */
+typedef Elf_Note Elf32_Nhdr;
+
+/*
+ *	Move entry
+ */
+typedef struct {
+	Elf32_Lword	m_value;	/* symbol value */
+	Elf32_Word 	m_info;		/* size + index */
+	Elf32_Word	m_poffset;	/* symbol offset */
+	Elf32_Half	m_repeat;	/* repeat count */
+	Elf32_Half	m_stride;	/* stride info */
+} Elf32_Move;
+
+/*
+ *	The macros compose and decompose values for Move.r_info
+ *
+ *	sym = ELF32_M_SYM(M.m_info)
+ *	size = ELF32_M_SIZE(M.m_info)
+ *	M.m_info = ELF32_M_INFO(sym, size)
+ */
+#define	ELF32_M_SYM(info)	((info)>>8)
+#define	ELF32_M_SIZE(info)	((unsigned char)(info))
+#define	ELF32_M_INFO(sym, size)	(((sym)<<8)+(unsigned char)(size))
+
+/*
+ *	Hardware/Software capabilities entry
+ */
+typedef struct {
+	Elf32_Word	c_tag;		/* how to interpret value */
+	union {
+		Elf32_Word	c_val;
+		Elf32_Addr	c_ptr;
+	} c_un;
+} Elf32_Cap;
+
+/*
+ * Symbol table entries.
+ */
+
+typedef struct {
+	Elf32_Word	st_name;	/* String table index of name. */
+	Elf32_Addr	st_value;	/* Symbol value. */
+	Elf32_Word	st_size;	/* Size of associated object. */
+	unsigned char	st_info;	/* Type and binding information. */
+	unsigned char	st_other;	/* Reserved (not used). */
+	Elf32_Half	st_shndx;	/* Section index of symbol. */
+} Elf32_Sym;
+
+/* Macros for accessing the fields of st_info. */
+#define ELF32_ST_BIND(info)		((info) >> 4)
+#define ELF32_ST_TYPE(info)		((info) & 0xf)
+
+/* Macro for constructing st_info from field values. */
+#define ELF32_ST_INFO(bind, type)	(((bind) << 4) + ((type) & 0xf))
+
+/* Macro for accessing the fields of st_other. */
+#define ELF32_ST_VISIBILITY(oth)	((oth) & 0x3)
+
+/* Structures used by Sun & GNU symbol versioning. */
+typedef struct
+{
+	Elf32_Half	vd_version;
+	Elf32_Half	vd_flags;
+	Elf32_Half	vd_ndx;
+	Elf32_Half	vd_cnt;
+	Elf32_Word	vd_hash;
+	Elf32_Word	vd_aux;
+	Elf32_Word	vd_next;
+} Elf32_Verdef;
+
+typedef struct
+{
+	Elf32_Word	vda_name;
+	Elf32_Word	vda_next;
+} Elf32_Verdaux;
+
+typedef struct
+{
+	Elf32_Half	vn_version;
+	Elf32_Half	vn_cnt;
+	Elf32_Word	vn_file;
+	Elf32_Word	vn_aux;
+	Elf32_Word	vn_next;
+} Elf32_Verneed;
+
+typedef struct
+{
+	Elf32_Word	vna_hash;
+	Elf32_Half	vna_flags;
+	Elf32_Half	vna_other;
+	Elf32_Word	vna_name;
+	Elf32_Word	vna_next;
+} Elf32_Vernaux;
+
+typedef Elf32_Half Elf32_Versym;
+
+typedef struct {
+	Elf32_Half	si_boundto;	/* direct bindings - symbol bound to */
+	Elf32_Half	si_flags;	/* per symbol flags */
+} Elf32_Syminfo;
+
+#endif /* !_SYS_ELF32_H_ */
diff --git a/core/arch/arm/kernel/elf64.h b/core/arch/arm/kernel/elf64.h
new file mode 100644
index 0000000..c468dcd
--- /dev/null
+++ b/core/arch/arm/kernel/elf64.h
@@ -0,0 +1,248 @@
+/*-
+ * Copyright (c) 1996-1998 John D. Polstra.
+ * 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 AUTHOR 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 AUTHOR 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.
+ *
+ * $FreeBSD$
+ */
+
+#ifndef _SYS_ELF64_H_
+#define _SYS_ELF64_H_ 1
+
+#include "elf_common.h"
+
+/*
+ * ELF definitions common to all 64-bit architectures.
+ */
+
+typedef uint64_t	Elf64_Addr;
+typedef uint16_t	Elf64_Half;
+typedef uint64_t	Elf64_Off;
+typedef int32_t		Elf64_Sword;
+typedef int64_t		Elf64_Sxword;
+typedef uint32_t	Elf64_Word;
+typedef uint64_t	Elf64_Lword;
+typedef uint64_t	Elf64_Xword;
+
+/*
+ * Types of dynamic symbol hash table bucket and chain elements.
+ *
+ * This is inconsistent among 64 bit architectures, so a machine dependent
+ * typedef is required.
+ */
+
+typedef Elf64_Word	Elf64_Hashelt;
+
+/* Non-standard class-dependent datatype used for abstraction. */
+typedef Elf64_Xword	Elf64_Size;
+typedef Elf64_Sxword	Elf64_Ssize;
+
+/*
+ * ELF header.
+ */
+
+typedef struct {
+	unsigned char	e_ident[EI_NIDENT];	/* File identification. */
+	Elf64_Half	e_type;		/* File type. */
+	Elf64_Half	e_machine;	/* Machine architecture. */
+	Elf64_Word	e_version;	/* ELF format version. */
+	Elf64_Addr	e_entry;	/* Entry point. */
+	Elf64_Off	e_phoff;	/* Program header file offset. */
+	Elf64_Off	e_shoff;	/* Section header file offset. */
+	Elf64_Word	e_flags;	/* Architecture-specific flags. */
+	Elf64_Half	e_ehsize;	/* Size of ELF header in bytes. */
+	Elf64_Half	e_phentsize;	/* Size of program header entry. */
+	Elf64_Half	e_phnum;	/* Number of program header entries. */
+	Elf64_Half	e_shentsize;	/* Size of section header entry. */
+	Elf64_Half	e_shnum;	/* Number of section header entries. */
+	Elf64_Half	e_shstrndx;	/* Section name strings section. */
+} Elf64_Ehdr;
+
+/*
+ * Section header.
+ */
+
+typedef struct {
+	Elf64_Word	sh_name;	/* Section name (index into the
+					   section header string table). */
+	Elf64_Word	sh_type;	/* Section type. */
+	Elf64_Xword	sh_flags;	/* Section flags. */
+	Elf64_Addr	sh_addr;	/* Address in memory image. */
+	Elf64_Off	sh_offset;	/* Offset in file. */
+	Elf64_Xword	sh_size;	/* Size in bytes. */
+	Elf64_Word	sh_link;	/* Index of a related section. */
+	Elf64_Word	sh_info;	/* Depends on section type. */
+	Elf64_Xword	sh_addralign;	/* Alignment in bytes. */
+	Elf64_Xword	sh_entsize;	/* Size of each entry in section. */
+} Elf64_Shdr;
+
+/*
+ * Program header.
+ */
+
+typedef struct {
+	Elf64_Word	p_type;		/* Entry type. */
+	Elf64_Word	p_flags;	/* Access permission flags. */
+	Elf64_Off	p_offset;	/* File offset of contents. */
+	Elf64_Addr	p_vaddr;	/* Virtual address in memory image. */
+	Elf64_Addr	p_paddr;	/* Physical address (not used). */
+	Elf64_Xword	p_filesz;	/* Size of contents in file. */
+	Elf64_Xword	p_memsz;	/* Size of contents in memory. */
+	Elf64_Xword	p_align;	/* Alignment in memory and file. */
+} Elf64_Phdr;
+
+/*
+ * Dynamic structure.  The ".dynamic" section contains an array of them.
+ */
+
+typedef struct {
+	Elf64_Sxword	d_tag;		/* Entry type. */
+	union {
+		Elf64_Xword	d_val;	/* Integer value. */
+		Elf64_Addr	d_ptr;	/* Address value. */
+	} d_un;
+} Elf64_Dyn;
+
+/*
+ * Relocation entries.
+ */
+
+/* Relocations that don't need an addend field. */
+typedef struct {
+	Elf64_Addr	r_offset;	/* Location to be relocated. */
+	Elf64_Xword	r_info;		/* Relocation type and symbol index. */
+} Elf64_Rel;
+
+/* Relocations that need an addend field. */
+typedef struct {
+	Elf64_Addr	r_offset;	/* Location to be relocated. */
+	Elf64_Xword	r_info;		/* Relocation type and symbol index. */
+	Elf64_Sxword	r_addend;	/* Addend. */
+} Elf64_Rela;
+
+/* Macros for accessing the fields of r_info. */
+#define	ELF64_R_SYM(info)	((info) >> 32)
+#define	ELF64_R_TYPE(info)	((info) & 0xffffffffL)
+
+/* Macro for constructing r_info from field values. */
+#define	ELF64_R_INFO(sym, type)	(((sym) << 32) + ((type) & 0xffffffffL))
+
+#define	ELF64_R_TYPE_DATA(info)	(((Elf64_Xword)(info)<<32)>>40)
+#define	ELF64_R_TYPE_ID(info)	(((Elf64_Xword)(info)<<56)>>56)
+#define	ELF64_R_TYPE_INFO(data, type)	\
+				(((Elf64_Xword)(data)<<8)+(Elf64_Xword)(type))
+
+/*
+ *	Note entry header
+ */
+typedef Elf_Note Elf64_Nhdr;
+
+/*
+ *	Move entry
+ */
+typedef struct {
+	Elf64_Lword	m_value;	/* symbol value */
+	Elf64_Xword 	m_info;		/* size + index */
+	Elf64_Xword	m_poffset;	/* symbol offset */
+	Elf64_Half	m_repeat;	/* repeat count */
+	Elf64_Half	m_stride;	/* stride info */
+} Elf64_Move;
+
+#define	ELF64_M_SYM(info)	((info)>>8)
+#define	ELF64_M_SIZE(info)	((unsigned char)(info))
+#define	ELF64_M_INFO(sym, size)	(((sym)<<8)+(unsigned char)(size))
+
+/*
+ *	Hardware/Software capabilities entry
+ */
+typedef struct {
+	Elf64_Xword	c_tag;		/* how to interpret value */
+	union {
+		Elf64_Xword	c_val;
+		Elf64_Addr	c_ptr;
+	} c_un;
+} Elf64_Cap;
+
+/*
+ * Symbol table entries.
+ */
+
+typedef struct {
+	Elf64_Word	st_name;	/* String table index of name. */
+	unsigned char	st_info;	/* Type and binding information. */
+	unsigned char	st_other;	/* Reserved (not used). */
+	Elf64_Half	st_shndx;	/* Section index of symbol. */
+	Elf64_Addr	st_value;	/* Symbol value. */
+	Elf64_Xword	st_size;	/* Size of associated object. */
+} Elf64_Sym;
+
+/* Macros for accessing the fields of st_info. */
+#define	ELF64_ST_BIND(info)		((info) >> 4)
+#define	ELF64_ST_TYPE(info)		((info) & 0xf)
+
+/* Macro for constructing st_info from field values. */
+#define	ELF64_ST_INFO(bind, type)	(((bind) << 4) + ((type) & 0xf))
+
+/* Macro for accessing the fields of st_other. */
+#define	ELF64_ST_VISIBILITY(oth)	((oth) & 0x3)
+
+/* Structures used by Sun & GNU-style symbol versioning. */
+typedef struct {
+	Elf64_Half	vd_version;
+	Elf64_Half	vd_flags;
+	Elf64_Half	vd_ndx;
+	Elf64_Half	vd_cnt;
+	Elf64_Word	vd_hash;
+	Elf64_Word	vd_aux;
+	Elf64_Word	vd_next;
+} Elf64_Verdef;
+
+typedef struct {
+	Elf64_Word	vda_name;
+	Elf64_Word	vda_next;
+} Elf64_Verdaux;
+
+typedef struct {
+	Elf64_Half	vn_version;
+	Elf64_Half	vn_cnt;
+	Elf64_Word	vn_file;
+	Elf64_Word	vn_aux;
+	Elf64_Word	vn_next;
+} Elf64_Verneed;
+
+typedef struct {
+	Elf64_Word	vna_hash;
+	Elf64_Half	vna_flags;
+	Elf64_Half	vna_other;
+	Elf64_Word	vna_name;
+	Elf64_Word	vna_next;
+} Elf64_Vernaux;
+
+typedef Elf64_Half Elf64_Versym;
+
+typedef struct {
+	Elf64_Half	si_boundto;	/* direct bindings - symbol bound to */
+	Elf64_Half	si_flags;	/* per symbol flags */
+} Elf64_Syminfo;
+
+#endif /* !_SYS_ELF64_H_ */
diff --git a/core/arch/arm/kernel/elf_common.h b/core/arch/arm/kernel/elf_common.h
new file mode 100644
index 0000000..dd8cd50
--- /dev/null
+++ b/core/arch/arm/kernel/elf_common.h
@@ -0,0 +1,1006 @@
+/*-
+ * Copyright (c) 2000, 2001, 2008, 2011, David E. O'Brien
+ * Copyright (c) 1998 John D. Polstra.
+ * 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 AUTHOR 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 AUTHOR 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.
+ *
+ * $FreeBSD$
+ */
+
+#ifndef _SYS_ELF_COMMON_H_
+#define	_SYS_ELF_COMMON_H_ 1
+
+/*
+ * ELF definitions that are independent of architecture or word size.
+ */
+
+/*
+ * Note header.  The ".note" section contains an array of notes.  Each
+ * begins with this header, aligned to a word boundary.  Immediately
+ * following the note header is n_namesz bytes of name, padded to the
+ * next word boundary.  Then comes n_descsz bytes of descriptor, again
+ * padded to a word boundary.  The values of n_namesz and n_descsz do
+ * not include the padding.
+ */
+
+typedef struct {
+	uint32_t	n_namesz;	/* Length of name. */
+	uint32_t	n_descsz;	/* Length of descriptor. */
+	uint32_t	n_type;		/* Type of this note. */
+} Elf_Note;
+
+/*
+ * The header for GNU-style hash sections.
+ */
+
+typedef struct {
+	uint32_t	gh_nbuckets;	/* Number of hash buckets. */
+	uint32_t	gh_symndx;	/* First visible symbol in .dynsym. */
+	uint32_t	gh_maskwords;	/* #maskwords used in bloom filter. */
+	uint32_t	gh_shift2;	/* Bloom filter shift count. */
+} Elf_GNU_Hash_Header;
+
+/* Indexes into the e_ident array.  Keep synced with
+   http://www.sco.com/developers/gabi/latest/ch4.eheader.html */
+#define	EI_MAG0		0	/* Magic number, byte 0. */
+#define	EI_MAG1		1	/* Magic number, byte 1. */
+#define	EI_MAG2		2	/* Magic number, byte 2. */
+#define	EI_MAG3		3	/* Magic number, byte 3. */
+#define	EI_CLASS	4	/* Class of machine. */
+#define	EI_DATA		5	/* Data format. */
+#define	EI_VERSION	6	/* ELF format version. */
+#define	EI_OSABI	7	/* Operating system / ABI identification */
+#define	EI_ABIVERSION	8	/* ABI version */
+#define	OLD_EI_BRAND	8	/* Start of architecture identification. */
+#define	EI_PAD		9	/* Start of padding (per SVR4 ABI). */
+#define	EI_NIDENT	16	/* Size of e_ident array. */
+
+/* Values for the magic number bytes. */
+#define	ELFMAG0		0x7f
+#define	ELFMAG1		'E'
+#define	ELFMAG2		'L'
+#define	ELFMAG3		'F'
+#define	ELFMAG		"\177ELF"	/* magic string */
+#define	SELFMAG		4		/* magic string size */
+
+/* Values for e_ident[EI_VERSION] and e_version. */
+#define	EV_NONE		0
+#define	EV_CURRENT	1
+
+/* Values for e_ident[EI_CLASS]. */
+#define	ELFCLASSNONE	0	/* Unknown class. */
+#define	ELFCLASS32	1	/* 32-bit architecture. */
+#define	ELFCLASS64	2	/* 64-bit architecture. */
+
+/* Values for e_ident[EI_DATA]. */
+#define	ELFDATANONE	0	/* Unknown data format. */
+#define	ELFDATA2LSB	1	/* 2's complement little-endian. */
+#define	ELFDATA2MSB	2	/* 2's complement big-endian. */
+
+/* Values for e_ident[EI_OSABI]. */
+#define	ELFOSABI_NONE		0	/* UNIX System V ABI */
+#define	ELFOSABI_HPUX		1	/* HP-UX operating system */
+#define	ELFOSABI_NETBSD		2	/* NetBSD */
+#define	ELFOSABI_LINUX		3	/* GNU/Linux */
+#define	ELFOSABI_HURD		4	/* GNU/Hurd */
+#define	ELFOSABI_86OPEN		5	/* 86Open common IA32 ABI */
+#define	ELFOSABI_SOLARIS	6	/* Solaris */
+#define	ELFOSABI_AIX		7	/* AIX */
+#define	ELFOSABI_IRIX		8	/* IRIX */
+#define	ELFOSABI_FREEBSD	9	/* FreeBSD */
+#define	ELFOSABI_TRU64		10	/* TRU64 UNIX */
+#define	ELFOSABI_MODESTO	11	/* Novell Modesto */
+#define	ELFOSABI_OPENBSD	12	/* OpenBSD */
+#define	ELFOSABI_OPENVMS	13	/* Open VMS */
+#define	ELFOSABI_NSK		14	/* HP Non-Stop Kernel */
+#define	ELFOSABI_AROS		15	/* Amiga Research OS */
+#define	ELFOSABI_ARM		97	/* ARM */
+#define	ELFOSABI_STANDALONE	255	/* Standalone (embedded) application */
+
+#define	ELFOSABI_SYSV		ELFOSABI_NONE	/* symbol used in old spec */
+#define	ELFOSABI_MONTEREY	ELFOSABI_AIX	/* Monterey */
+
+/* e_ident */
+#define	IS_ELF(ehdr)	((ehdr).e_ident[EI_MAG0] == ELFMAG0 && \
+			 (ehdr).e_ident[EI_MAG1] == ELFMAG1 && \
+			 (ehdr).e_ident[EI_MAG2] == ELFMAG2 && \
+			 (ehdr).e_ident[EI_MAG3] == ELFMAG3)
+
+/* Values for e_type. */
+#define	ET_NONE		0	/* Unknown type. */
+#define	ET_REL		1	/* Relocatable. */
+#define	ET_EXEC		2	/* Executable. */
+#define	ET_DYN		3	/* Shared object. */
+#define	ET_CORE		4	/* Core file. */
+#define	ET_LOOS		0xfe00	/* First operating system specific. */
+#define	ET_HIOS		0xfeff	/* Last operating system-specific. */
+#define	ET_LOPROC	0xff00	/* First processor-specific. */
+#define	ET_HIPROC	0xffff	/* Last processor-specific. */
+
+/* Values for e_machine. */
+#define	EM_NONE		0	/* Unknown machine. */
+#define	EM_M32		1	/* AT&T WE32100. */
+#define	EM_SPARC	2	/* Sun SPARC. */
+#define	EM_386		3	/* Intel i386. */
+#define	EM_68K		4	/* Motorola 68000. */
+#define	EM_88K		5	/* Motorola 88000. */
+#define	EM_860		7	/* Intel i860. */
+#define	EM_MIPS		8	/* MIPS R3000 Big-Endian only. */
+#define	EM_S370		9	/* IBM System/370. */
+#define	EM_MIPS_RS3_LE	10	/* MIPS R3000 Little-Endian. */
+#define	EM_PARISC	15	/* HP PA-RISC. */
+#define	EM_VPP500	17	/* Fujitsu VPP500. */
+#define	EM_SPARC32PLUS	18	/* SPARC v8plus. */
+#define	EM_960		19	/* Intel 80960. */
+#define	EM_PPC		20	/* PowerPC 32-bit. */
+#define	EM_PPC64	21	/* PowerPC 64-bit. */
+#define	EM_S390		22	/* IBM System/390. */
+#define	EM_V800		36	/* NEC V800. */
+#define	EM_FR20		37	/* Fujitsu FR20. */
+#define	EM_RH32		38	/* TRW RH-32. */
+#define	EM_RCE		39	/* Motorola RCE. */
+#define	EM_ARM		40	/* ARM. */
+#define	EM_SH		42	/* Hitachi SH. */
+#define	EM_SPARCV9	43	/* SPARC v9 64-bit. */
+#define	EM_TRICORE	44	/* Siemens TriCore embedded processor. */
+#define	EM_ARC		45	/* Argonaut RISC Core. */
+#define	EM_H8_300	46	/* Hitachi H8/300. */
+#define	EM_H8_300H	47	/* Hitachi H8/300H. */
+#define	EM_H8S		48	/* Hitachi H8S. */
+#define	EM_H8_500	49	/* Hitachi H8/500. */
+#define	EM_IA_64	50	/* Intel IA-64 Processor. */
+#define	EM_MIPS_X	51	/* Stanford MIPS-X. */
+#define	EM_COLDFIRE	52	/* Motorola ColdFire. */
+#define	EM_68HC12	53	/* Motorola M68HC12. */
+#define	EM_MMA		54	/* Fujitsu MMA. */
+#define	EM_PCP		55	/* Siemens PCP. */
+#define	EM_NCPU		56	/* Sony nCPU. */
+#define	EM_NDR1		57	/* Denso NDR1 microprocessor. */
+#define	EM_STARCORE	58	/* Motorola Star*Core processor. */
+#define	EM_ME16		59	/* Toyota ME16 processor. */
+#define	EM_ST100	60	/* STMicroelectronics ST100 processor. */
+#define	EM_TINYJ	61	/* Advanced Logic Corp. TinyJ processor. */
+#define	EM_X86_64	62	/* Advanced Micro Devices x86-64 */
+#define	EM_AMD64	EM_X86_64	/* Advanced Micro Devices x86-64 (compat) */
+#define	EM_PDSP		63	/* Sony DSP Processor. */
+#define	EM_FX66		66	/* Siemens FX66 microcontroller. */
+#define	EM_ST9PLUS	67	/* STMicroelectronics ST9+ 8/16
+				   microcontroller. */
+#define	EM_ST7		68	/* STmicroelectronics ST7 8-bit
+				   microcontroller. */
+#define	EM_68HC16	69	/* Motorola MC68HC16 microcontroller. */
+#define	EM_68HC11	70	/* Motorola MC68HC11 microcontroller. */
+#define	EM_68HC08	71	/* Motorola MC68HC08 microcontroller. */
+#define	EM_68HC05	72	/* Motorola MC68HC05 microcontroller. */
+#define	EM_SVX		73	/* Silicon Graphics SVx. */
+#define	EM_ST19		74	/* STMicroelectronics ST19 8-bit mc. */
+#define	EM_VAX		75	/* Digital VAX. */
+#define	EM_CRIS		76	/* Axis Communications 32-bit embedded
+				   processor. */
+#define	EM_JAVELIN	77	/* Infineon Technologies 32-bit embedded
+				   processor. */
+#define	EM_FIREPATH	78	/* Element 14 64-bit DSP Processor. */
+#define	EM_ZSP		79	/* LSI Logic 16-bit DSP Processor. */
+#define	EM_MMIX		80	/* Donald Knuth's educational 64-bit proc. */
+#define	EM_HUANY	81	/* Harvard University machine-independent
+				   object files. */
+#define	EM_PRISM	82	/* SiTera Prism. */
+#define	EM_AVR		83	/* Atmel AVR 8-bit microcontroller. */
+#define	EM_FR30		84	/* Fujitsu FR30. */
+#define	EM_D10V		85	/* Mitsubishi D10V. */
+#define	EM_D30V		86	/* Mitsubishi D30V. */
+#define	EM_V850		87	/* NEC v850. */
+#define	EM_M32R		88	/* Mitsubishi M32R. */
+#define	EM_MN10300	89	/* Matsushita MN10300. */
+#define	EM_MN10200	90	/* Matsushita MN10200. */
+#define	EM_PJ		91	/* picoJava. */
+#define	EM_OPENRISC	92	/* OpenRISC 32-bit embedded processor. */
+#define	EM_ARC_A5	93	/* ARC Cores Tangent-A5. */
+#define	EM_XTENSA	94	/* Tensilica Xtensa Architecture. */
+#define	EM_VIDEOCORE	95	/* Alphamosaic VideoCore processor. */
+#define	EM_TMM_GPP	96	/* Thompson Multimedia General Purpose
+				   Processor. */
+#define	EM_NS32K	97	/* National Semiconductor 32000 series. */
+#define	EM_TPC		98	/* Tenor Network TPC processor. */
+#define	EM_SNP1K	99	/* Trebia SNP 1000 processor. */
+#define	EM_ST200	100	/* STMicroelectronics ST200 microcontroller. */
+#define	EM_IP2K		101	/* Ubicom IP2xxx microcontroller family. */
+#define	EM_MAX		102	/* MAX Processor. */
+#define	EM_CR		103	/* National Semiconductor CompactRISC
+				   microprocessor. */
+#define	EM_F2MC16	104	/* Fujitsu F2MC16. */
+#define	EM_MSP430	105	/* Texas Instruments embedded microcontroller
+				   msp430. */
+#define	EM_BLACKFIN	106	/* Analog Devices Blackfin (DSP) processor. */
+#define	EM_SE_C33	107	/* S1C33 Family of Seiko Epson processors. */
+#define	EM_SEP		108	/* Sharp embedded microprocessor. */
+#define	EM_ARCA		109	/* Arca RISC Microprocessor. */
+#define	EM_UNICORE	110	/* Microprocessor series from PKU-Unity Ltd.
+				   and MPRC of Peking University */
+#define	EM_AARCH64	183	/* AArch64 (64-bit ARM) */
+
+/* Non-standard or deprecated. */
+#define	EM_486		6	/* Intel i486. */
+#define	EM_MIPS_RS4_BE	10	/* MIPS R4000 Big-Endian */
+#define	EM_ALPHA_STD	41	/* Digital Alpha (standard value). */
+#define	EM_ALPHA	0x9026	/* Alpha (written in the absence of an ABI) */
+
+/* e_flags for EM_ARM */
+#define EF_ARM_ABI_VERSION	0x05000000	/* ABI version 5 */
+#define EF_ARM_ABIMASK		0xFF000000
+#define EF_ARM_BE8		0x00800000
+#define EF_ARM_ABI_FLOAT_HARD	0x00000400	/* ABI version 5 and later */
+#define EF_ARM_ABI_FLOAT_SOFT	0x00000200	/* ABI version 5 and later */
+
+/* Special section indexes. */
+#define	SHN_UNDEF	     0		/* Undefined, missing, irrelevant. */
+#define	SHN_LORESERVE	0xff00		/* First of reserved range. */
+#define	SHN_LOPROC	0xff00		/* First processor-specific. */
+#define	SHN_HIPROC	0xff1f		/* Last processor-specific. */
+#define	SHN_LOOS	0xff20		/* First operating system-specific. */
+#define	SHN_HIOS	0xff3f		/* Last operating system-specific. */
+#define	SHN_ABS		0xfff1		/* Absolute values. */
+#define	SHN_COMMON	0xfff2		/* Common data. */
+#define	SHN_XINDEX	0xffff		/* Escape -- index stored elsewhere. */
+#define	SHN_HIRESERVE	0xffff		/* Last of reserved range. */
+
+/* sh_type */
+#define	SHT_NULL		0	/* inactive */
+#define	SHT_PROGBITS		1	/* program defined information */
+#define	SHT_SYMTAB		2	/* symbol table section */
+#define	SHT_STRTAB		3	/* string table section */
+#define	SHT_RELA		4	/* relocation section with addends */
+#define	SHT_HASH		5	/* symbol hash table section */
+#define	SHT_DYNAMIC		6	/* dynamic section */
+#define	SHT_NOTE		7	/* note section */
+#define	SHT_NOBITS		8	/* no space section */
+#define	SHT_REL			9	/* relocation section - no addends */
+#define	SHT_SHLIB		10	/* reserved - purpose unknown */
+#define	SHT_DYNSYM		11	/* dynamic symbol table section */
+#define	SHT_INIT_ARRAY		14	/* Initialization function pointers. */
+#define	SHT_FINI_ARRAY		15	/* Termination function pointers. */
+#define	SHT_PREINIT_ARRAY	16	/* Pre-initialization function ptrs. */
+#define	SHT_GROUP		17	/* Section group. */
+#define	SHT_SYMTAB_SHNDX	18	/* Section indexes (see SHN_XINDEX). */
+#define	SHT_LOOS		0x60000000	/* First of OS specific semantics */
+#define	SHT_LOSUNW		0x6ffffff4
+#define	SHT_SUNW_dof		0x6ffffff4
+#define	SHT_SUNW_cap		0x6ffffff5
+#define	SHT_SUNW_SIGNATURE	0x6ffffff6
+#define	SHT_GNU_HASH		0x6ffffff6
+#define	SHT_GNU_LIBLIST		0x6ffffff7
+#define	SHT_SUNW_ANNOTATE	0x6ffffff7
+#define	SHT_SUNW_DEBUGSTR	0x6ffffff8
+#define	SHT_SUNW_DEBUG		0x6ffffff9
+#define	SHT_SUNW_move		0x6ffffffa
+#define	SHT_SUNW_COMDAT		0x6ffffffb
+#define	SHT_SUNW_syminfo	0x6ffffffc
+#define	SHT_SUNW_verdef		0x6ffffffd
+#define	SHT_GNU_verdef		0x6ffffffd	/* Symbol versions provided */
+#define	SHT_SUNW_verneed	0x6ffffffe
+#define	SHT_GNU_verneed		0x6ffffffe	/* Symbol versions required */
+#define	SHT_SUNW_versym		0x6fffffff
+#define	SHT_GNU_versym		0x6fffffff	/* Symbol version table */
+#define	SHT_HISUNW		0x6fffffff
+#define	SHT_HIOS		0x6fffffff	/* Last of OS specific semantics */
+#define	SHT_LOPROC		0x70000000	/* reserved range for processor */
+#define	SHT_AMD64_UNWIND	0x70000001	/* unwind information */
+#define	SHT_ARM_EXIDX		0x70000001	/* Exception index table. */
+#define	SHT_ARM_PREEMPTMAP	0x70000002	/* BPABI DLL dynamic linking 
+						   pre-emption map. */
+#define	SHT_ARM_ATTRIBUTES	0x70000003	/* Object file compatibility 
+						   attributes. */
+#define	SHT_ARM_DEBUGOVERLAY	0x70000004	/* See DBGOVL for details. */
+#define	SHT_ARM_OVERLAYSECTION	0x70000005	/* See DBGOVL for details. */
+#define	SHT_MIPS_REGINFO	0x70000006
+#define	SHT_MIPS_OPTIONS	0x7000000d
+#define	SHT_MIPS_DWARF		0x7000001e	/* MIPS gcc uses MIPS_DWARF */
+#define	SHT_HIPROC		0x7fffffff	/* specific section header types */
+#define	SHT_LOUSER		0x80000000	/* reserved range for application */
+#define	SHT_HIUSER		0xffffffff	/* specific indexes */
+
+/* Flags for sh_flags. */
+#define	SHF_WRITE		0x1	/* Section contains writable data. */
+#define	SHF_ALLOC		0x2	/* Section occupies memory. */
+#define	SHF_EXECINSTR		0x4	/* Section contains instructions. */
+#define	SHF_MERGE		0x10	/* Section may be merged. */
+#define	SHF_STRINGS		0x20	/* Section contains strings. */
+#define	SHF_INFO_LINK		0x40	/* sh_info holds section index. */
+#define	SHF_LINK_ORDER		0x80	/* Special ordering requirements. */
+#define	SHF_OS_NONCONFORMING	0x100	/* OS-specific processing required. */
+#define	SHF_GROUP		0x200	/* Member of section group. */
+#define	SHF_TLS			0x400	/* Section contains TLS data. */
+#define	SHF_MASKOS	0x0ff00000	/* OS-specific semantics. */
+#define	SHF_MASKPROC	0xf0000000	/* Processor-specific semantics. */
+
+/* Values for p_type. */
+#define	PT_NULL		0	/* Unused entry. */
+#define	PT_LOAD		1	/* Loadable segment. */
+#define	PT_DYNAMIC	2	/* Dynamic linking information segment. */
+#define	PT_INTERP	3	/* Pathname of interpreter. */
+#define	PT_NOTE		4	/* Auxiliary information. */
+#define	PT_SHLIB	5	/* Reserved (not used). */
+#define	PT_PHDR		6	/* Location of program header itself. */
+#define	PT_TLS		7	/* Thread local storage segment */
+#define	PT_LOOS		0x60000000	/* First OS-specific. */
+#define	PT_SUNW_UNWIND	0x6464e550	/* amd64 UNWIND program header */
+#define	PT_GNU_EH_FRAME	0x6474e550
+#define	PT_GNU_STACK	0x6474e551
+#define	PT_GNU_RELRO	0x6474e552
+#define	PT_LOSUNW	0x6ffffffa
+#define	PT_SUNWBSS	0x6ffffffa	/* Sun Specific segment */
+#define	PT_SUNWSTACK	0x6ffffffb	/* describes the stack segment */
+#define	PT_SUNWDTRACE	0x6ffffffc	/* private */
+#define	PT_SUNWCAP	0x6ffffffd	/* hard/soft capabilities segment */
+#define	PT_HISUNW	0x6fffffff
+#define	PT_HIOS		0x6fffffff	/* Last OS-specific. */
+#define	PT_LOPROC	0x70000000	/* First processor-specific type. */
+#define	PT_HIPROC	0x7fffffff	/* Last processor-specific type. */
+
+/* Values for p_flags. */
+#define	PF_X		0x1		/* Executable. */
+#define	PF_W		0x2		/* Writable. */
+#define	PF_R		0x4		/* Readable. */
+#define	PF_MASKOS	0x0ff00000	/* Operating system-specific. */
+#define	PF_MASKPROC	0xf0000000	/* Processor-specific. */
+
+/* Extended program header index. */
+#define	PN_XNUM		0xffff
+
+/* Values for d_tag. */
+#define	DT_NULL		0	/* Terminating entry. */
+#define	DT_NEEDED	1	/* String table offset of a needed shared
+				   library. */
+#define	DT_PLTRELSZ	2	/* Total size in bytes of PLT relocations. */
+#define	DT_PLTGOT	3	/* Processor-dependent address. */
+#define	DT_HASH		4	/* Address of symbol hash table. */
+#define	DT_STRTAB	5	/* Address of string table. */
+#define	DT_SYMTAB	6	/* Address of symbol table. */
+#define	DT_RELA		7	/* Address of ElfNN_Rela relocations. */
+#define	DT_RELASZ	8	/* Total size of ElfNN_Rela relocations. */
+#define	DT_RELAENT	9	/* Size of each ElfNN_Rela relocation entry. */
+#define	DT_STRSZ	10	/* Size of string table. */
+#define	DT_SYMENT	11	/* Size of each symbol table entry. */
+#define	DT_INIT		12	/* Address of initialization function. */
+#define	DT_FINI		13	/* Address of finalization function. */
+#define	DT_SONAME	14	/* String table offset of shared object
+				   name. */
+#define	DT_RPATH	15	/* String table offset of library path. [sup] */
+#define	DT_SYMBOLIC	16	/* Indicates "symbolic" linking. [sup] */
+#define	DT_REL		17	/* Address of ElfNN_Rel relocations. */
+#define	DT_RELSZ	18	/* Total size of ElfNN_Rel relocations. */
+#define	DT_RELENT	19	/* Size of each ElfNN_Rel relocation. */
+#define	DT_PLTREL	20	/* Type of relocation used for PLT. */
+#define	DT_DEBUG	21	/* Reserved (not used). */
+#define	DT_TEXTREL	22	/* Indicates there may be relocations in
+				   non-writable segments. [sup] */
+#define	DT_JMPREL	23	/* Address of PLT relocations. */
+#define	DT_BIND_NOW	24	/* [sup] */
+#define	DT_INIT_ARRAY	25	/* Address of the array of pointers to
+				   initialization functions */
+#define	DT_FINI_ARRAY	26	/* Address of the array of pointers to
+				   termination functions */
+#define	DT_INIT_ARRAYSZ	27	/* Size in bytes of the array of
+				   initialization functions. */
+#define	DT_FINI_ARRAYSZ	28	/* Size in bytes of the array of
+				   termination functions. */
+#define	DT_RUNPATH	29	/* String table offset of a null-terminated
+				   library search path string. */
+#define	DT_FLAGS	30	/* Object specific flag values. */
+#define	DT_ENCODING	32	/* Values greater than or equal to DT_ENCODING
+				   and less than DT_LOOS follow the rules for
+				   the interpretation of the d_un union
+				   as follows: even == 'd_ptr', odd == 'd_val'
+				   or none */
+#define	DT_PREINIT_ARRAY 32	/* Address of the array of pointers to
+				   pre-initialization functions. */
+#define	DT_PREINIT_ARRAYSZ 33	/* Size in bytes of the array of
+				   pre-initialization functions. */
+#define	DT_MAXPOSTAGS	34	/* number of positive tags */
+#define	DT_LOOS		0x6000000d	/* First OS-specific */
+#define	DT_SUNW_AUXILIARY	0x6000000d	/* symbol auxiliary name */
+#define	DT_SUNW_RTLDINF		0x6000000e	/* ld.so.1 info (private) */
+#define	DT_SUNW_FILTER		0x6000000f	/* symbol filter name */
+#define	DT_SUNW_CAP		0x60000010	/* hardware/software */
+#define	DT_HIOS		0x6ffff000	/* Last OS-specific */
+
+/*
+ * DT_* entries which fall between DT_VALRNGHI & DT_VALRNGLO use the
+ * Dyn.d_un.d_val field of the Elf*_Dyn structure.
+ */
+#define	DT_VALRNGLO	0x6ffffd00
+#define	DT_CHECKSUM	0x6ffffdf8	/* elf checksum */
+#define	DT_PLTPADSZ	0x6ffffdf9	/* pltpadding size */
+#define	DT_MOVEENT	0x6ffffdfa	/* move table entry size */
+#define	DT_MOVESZ	0x6ffffdfb	/* move table size */
+#define	DT_FEATURE_1	0x6ffffdfc	/* feature holder */
+#define	DT_POSFLAG_1	0x6ffffdfd	/* flags for DT_* entries, effecting */
+					/*	the following DT_* entry. */
+					/*	See DF_P1_* definitions */
+#define	DT_SYMINSZ	0x6ffffdfe	/* syminfo table size (in bytes) */
+#define	DT_SYMINENT	0x6ffffdff	/* syminfo entry size (in bytes) */
+#define	DT_VALRNGHI	0x6ffffdff
+
+/*
+ * DT_* entries which fall between DT_ADDRRNGHI & DT_ADDRRNGLO use the
+ * Dyn.d_un.d_ptr field of the Elf*_Dyn structure.
+ *
+ * If any adjustment is made to the ELF object after it has been
+ * built, these entries will need to be adjusted.
+ */
+#define	DT_ADDRRNGLO	0x6ffffe00
+#define	DT_GNU_HASH	0x6ffffef5	/* GNU-style hash table */
+#define	DT_CONFIG	0x6ffffefa	/* configuration information */
+#define	DT_DEPAUDIT	0x6ffffefb	/* dependency auditing */
+#define	DT_AUDIT	0x6ffffefc	/* object auditing */
+#define	DT_PLTPAD	0x6ffffefd	/* pltpadding (sparcv9) */
+#define	DT_MOVETAB	0x6ffffefe	/* move table */
+#define	DT_SYMINFO	0x6ffffeff	/* syminfo table */
+#define	DT_ADDRRNGHI	0x6ffffeff
+
+#define	DT_VERSYM	0x6ffffff0	/* Address of versym section. */
+#define	DT_RELACOUNT	0x6ffffff9	/* number of RELATIVE relocations */
+#define	DT_RELCOUNT	0x6ffffffa	/* number of RELATIVE relocations */
+#define	DT_FLAGS_1	0x6ffffffb	/* state flags - see DF_1_* defs */
+#define	DT_VERDEF	0x6ffffffc	/* Address of verdef section. */
+#define	DT_VERDEFNUM	0x6ffffffd	/* Number of elems in verdef section */
+#define	DT_VERNEED	0x6ffffffe	/* Address of verneed section. */
+#define	DT_VERNEEDNUM	0x6fffffff	/* Number of elems in verneed section */
+
+#define	DT_LOPROC	0x70000000	/* First processor-specific type. */
+#define	DT_DEPRECATED_SPARC_REGISTER	0x7000001
+#define	DT_AUXILIARY	0x7ffffffd	/* shared library auxiliary name */
+#define	DT_USED		0x7ffffffe	/* ignored - same as needed */
+#define	DT_FILTER	0x7fffffff	/* shared library filter name */
+#define	DT_HIPROC	0x7fffffff	/* Last processor-specific type. */
+
+/* Values for DT_FLAGS */
+#define	DF_ORIGIN	0x0001	/* Indicates that the object being loaded may
+				   make reference to the $ORIGIN substitution
+				   string */
+#define	DF_SYMBOLIC	0x0002	/* Indicates "symbolic" linking. */
+#define	DF_TEXTREL	0x0004	/* Indicates there may be relocations in
+				   non-writable segments. */
+#define	DF_BIND_NOW	0x0008	/* Indicates that the dynamic linker should
+				   process all relocations for the object
+				   containing this entry before transferring
+				   control to the program. */
+#define	DF_STATIC_TLS	0x0010	/* Indicates that the shared object or
+				   executable contains code using a static
+				   thread-local storage scheme. */
+
+/* Values for DT_FLAGS_1 */
+#define	DF_1_BIND_NOW	0x00000001	/* Same as DF_BIND_NOW */
+#define	DF_1_GLOBAL	0x00000002	/* Set the RTLD_GLOBAL for object */
+#define	DF_1_NODELETE	0x00000008	/* Set the RTLD_NODELETE for object */
+#define	DF_1_LOADFLTR	0x00000010	/* Immediate loading of filtees */
+#define	DF_1_NOOPEN     0x00000040	/* Do not allow loading on dlopen() */
+#define	DF_1_ORIGIN	0x00000080	/* Process $ORIGIN */
+#define	DF_1_INTERPOSE	0x00000400	/* Interpose all objects but main */
+#define	DF_1_NODEFLIB	0x00000800	/* Do not search default paths */
+
+/* Values for n_type.  Used in core files. */
+#define	NT_PRSTATUS	1	/* Process status. */
+#define	NT_FPREGSET	2	/* Floating point registers. */
+#define	NT_PRPSINFO	3	/* Process state info. */
+#define	NT_THRMISC	7	/* Thread miscellaneous info. */
+#define	NT_PROCSTAT_PROC	8	/* Procstat proc data. */
+#define	NT_PROCSTAT_FILES	9	/* Procstat files data. */
+#define	NT_PROCSTAT_VMMAP	10	/* Procstat vmmap data. */
+#define	NT_PROCSTAT_GROUPS	11	/* Procstat groups data. */
+#define	NT_PROCSTAT_UMASK	12	/* Procstat umask data. */
+#define	NT_PROCSTAT_RLIMIT	13	/* Procstat rlimit data. */
+#define	NT_PROCSTAT_OSREL	14	/* Procstat osreldate data. */
+#define	NT_PROCSTAT_PSSTRINGS	15	/* Procstat ps_strings data. */
+#define	NT_PROCSTAT_AUXV	16	/* Procstat auxv data. */
+
+/* Symbol Binding - ELFNN_ST_BIND - st_info */
+#define	STB_LOCAL	0	/* Local symbol */
+#define	STB_GLOBAL	1	/* Global symbol */
+#define	STB_WEAK	2	/* like global - lower precedence */
+#define	STB_LOOS	10	/* Reserved range for operating system */
+#define	STB_HIOS	12	/*   specific semantics. */
+#define	STB_LOPROC	13	/* reserved range for processor */
+#define	STB_HIPROC	15	/*   specific semantics. */
+
+/* Symbol type - ELFNN_ST_TYPE - st_info */
+#define	STT_NOTYPE	0	/* Unspecified type. */
+#define	STT_OBJECT	1	/* Data object. */
+#define	STT_FUNC	2	/* Function. */
+#define	STT_SECTION	3	/* Section. */
+#define	STT_FILE	4	/* Source file. */
+#define	STT_COMMON	5	/* Uninitialized common block. */
+#define	STT_TLS		6	/* TLS object. */
+#define	STT_NUM		7
+#define	STT_LOOS	10	/* Reserved range for operating system */
+#define	STT_GNU_IFUNC	10
+#define	STT_HIOS	12	/*   specific semantics. */
+#define	STT_LOPROC	13	/* reserved range for processor */
+#define	STT_HIPROC	15	/*   specific semantics. */
+
+/* Symbol visibility - ELFNN_ST_VISIBILITY - st_other */
+#define	STV_DEFAULT	0x0	/* Default visibility (see binding). */
+#define	STV_INTERNAL	0x1	/* Special meaning in relocatable objects. */
+#define	STV_HIDDEN	0x2	/* Not visible. */
+#define	STV_PROTECTED	0x3	/* Visible but not preemptible. */
+#define	STV_EXPORTED	0x4
+#define	STV_SINGLETON	0x5
+#define	STV_ELIMINATE	0x6
+
+/* Special symbol table indexes. */
+#define	STN_UNDEF	0	/* Undefined symbol index. */
+
+/* Symbol versioning flags. */
+#define	VER_DEF_CURRENT	1
+#define	VER_DEF_IDX(x)	VER_NDX(x)
+
+#define	VER_FLG_BASE	0x01
+#define	VER_FLG_WEAK	0x02
+
+#define	VER_NEED_CURRENT	1
+#define	VER_NEED_WEAK	(1u << 15)
+#define	VER_NEED_HIDDEN	VER_NDX_HIDDEN
+#define	VER_NEED_IDX(x)	VER_NDX(x)
+
+#define	VER_NDX_LOCAL	0
+#define	VER_NDX_GLOBAL	1
+#define	VER_NDX_GIVEN	2
+
+#define	VER_NDX_HIDDEN	(1u << 15)
+#define	VER_NDX(x)	((x) & ~(1u << 15))
+
+#define	CA_SUNW_NULL	0
+#define	CA_SUNW_HW_1	1		/* first hardware capabilities entry */
+#define	CA_SUNW_SF_1	2		/* first software capabilities entry */
+
+/*
+ * Syminfo flag values
+ */
+#define	SYMINFO_FLG_DIRECT	0x0001	/* symbol ref has direct association */
+					/*	to object containing defn. */
+#define	SYMINFO_FLG_PASSTHRU	0x0002	/* ignored - see SYMINFO_FLG_FILTER */
+#define	SYMINFO_FLG_COPY	0x0004	/* symbol is a copy-reloc */
+#define	SYMINFO_FLG_LAZYLOAD	0x0008	/* object containing defn should be */
+					/*	lazily-loaded */
+#define	SYMINFO_FLG_DIRECTBIND	0x0010	/* ref should be bound directly to */
+					/*	object containing defn. */
+#define	SYMINFO_FLG_NOEXTDIRECT	0x0020	/* don't let an external reference */
+					/*	directly bind to this symbol */
+#define	SYMINFO_FLG_FILTER	0x0002	/* symbol ref is associated to a */
+#define	SYMINFO_FLG_AUXILIARY	0x0040	/* 	standard or auxiliary filter */
+
+/*
+ * Syminfo.si_boundto values.
+ */
+#define	SYMINFO_BT_SELF		0xffff	/* symbol bound to self */
+#define	SYMINFO_BT_PARENT	0xfffe	/* symbol bound to parent */
+#define	SYMINFO_BT_NONE		0xfffd	/* no special symbol binding */
+#define	SYMINFO_BT_EXTERN	0xfffc	/* symbol defined as external */
+#define	SYMINFO_BT_LOWRESERVE	0xff00	/* beginning of reserved entries */
+
+/*
+ * Syminfo version values.
+ */
+#define	SYMINFO_NONE		0	/* Syminfo version */
+#define	SYMINFO_CURRENT		1
+#define	SYMINFO_NUM		2
+
+/*
+ * Relocation types.
+ *
+ * All machine architectures are defined here to allow tools on one to
+ * handle others.
+ */
+
+#define	R_386_NONE		0	/* No relocation. */
+#define	R_386_32		1	/* Add symbol value. */
+#define	R_386_PC32		2	/* Add PC-relative symbol value. */
+#define	R_386_GOT32		3	/* Add PC-relative GOT offset. */
+#define	R_386_PLT32		4	/* Add PC-relative PLT offset. */
+#define	R_386_COPY		5	/* Copy data from shared object. */
+#define	R_386_GLOB_DAT		6	/* Set GOT entry to data address. */
+#define	R_386_JMP_SLOT		7	/* Set GOT entry to code address. */
+#define	R_386_RELATIVE		8	/* Add load address of shared object. */
+#define	R_386_GOTOFF		9	/* Add GOT-relative symbol address. */
+#define	R_386_GOTPC		10	/* Add PC-relative GOT table address. */
+#define	R_386_TLS_TPOFF		14	/* Negative offset in static TLS block */
+#define	R_386_TLS_IE		15	/* Absolute address of GOT for -ve static TLS */
+#define	R_386_TLS_GOTIE		16	/* GOT entry for negative static TLS block */
+#define	R_386_TLS_LE		17	/* Negative offset relative to static TLS */
+#define	R_386_TLS_GD		18	/* 32 bit offset to GOT (index,off) pair */
+#define	R_386_TLS_LDM		19	/* 32 bit offset to GOT (index,zero) pair */
+#define	R_386_TLS_GD_32		24	/* 32 bit offset to GOT (index,off) pair */
+#define	R_386_TLS_GD_PUSH	25	/* pushl instruction for Sun ABI GD sequence */
+#define	R_386_TLS_GD_CALL	26	/* call instruction for Sun ABI GD sequence */
+#define	R_386_TLS_GD_POP	27	/* popl instruction for Sun ABI GD sequence */
+#define	R_386_TLS_LDM_32	28	/* 32 bit offset to GOT (index,zero) pair */
+#define	R_386_TLS_LDM_PUSH	29	/* pushl instruction for Sun ABI LD sequence */
+#define	R_386_TLS_LDM_CALL	30	/* call instruction for Sun ABI LD sequence */
+#define	R_386_TLS_LDM_POP	31	/* popl instruction for Sun ABI LD sequence */
+#define	R_386_TLS_LDO_32	32	/* 32 bit offset from start of TLS block */
+#define	R_386_TLS_IE_32		33	/* 32 bit offset to GOT static TLS offset entry */
+#define	R_386_TLS_LE_32		34	/* 32 bit offset within static TLS block */
+#define	R_386_TLS_DTPMOD32	35	/* GOT entry containing TLS index */
+#define	R_386_TLS_DTPOFF32	36	/* GOT entry containing TLS offset */
+#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_RELATIVE 	1027
+
+#define	R_ARM_NONE		0	/* No relocation. */
+#define	R_ARM_PC24		1
+#define	R_ARM_ABS32		2
+#define	R_ARM_REL32		3
+#define	R_ARM_PC13		4
+#define	R_ARM_ABS16		5
+#define	R_ARM_ABS12		6
+#define	R_ARM_THM_ABS5		7
+#define	R_ARM_ABS8		8
+#define	R_ARM_SBREL32		9
+#define	R_ARM_THM_PC22		10
+#define	R_ARM_THM_PC8		11
+#define	R_ARM_AMP_VCALL9	12
+#define	R_ARM_SWI24		13
+#define	R_ARM_THM_SWI8		14
+#define	R_ARM_XPC25		15
+#define	R_ARM_THM_XPC22		16
+/* TLS relocations */
+#define	R_ARM_TLS_DTPMOD32	17	/* ID of module containing symbol */
+#define	R_ARM_TLS_DTPOFF32	18	/* Offset in TLS block */
+#define	R_ARM_TLS_TPOFF32	19	/* Offset in static TLS block */
+#define	R_ARM_COPY		20	/* Copy data from shared object. */
+#define	R_ARM_GLOB_DAT		21	/* Set GOT entry to data address. */
+#define	R_ARM_JUMP_SLOT		22	/* Set GOT entry to code address. */
+#define	R_ARM_RELATIVE		23	/* Add load address of shared object. */
+#define	R_ARM_GOTOFF		24	/* Add GOT-relative symbol address. */
+#define	R_ARM_GOTPC		25	/* Add PC-relative GOT table address. */
+#define	R_ARM_GOT32		26	/* Add PC-relative GOT offset. */
+#define	R_ARM_PLT32		27	/* Add PC-relative PLT offset. */
+#define	R_ARM_GNU_VTENTRY	100
+#define	R_ARM_GNU_VTINHERIT	101
+#define	R_ARM_RSBREL32		250
+#define	R_ARM_THM_RPC22		251
+#define	R_ARM_RREL32		252
+#define	R_ARM_RABS32		253
+#define	R_ARM_RPC24		254
+#define	R_ARM_RBASE		255
+
+/*	Name			Value	   Field	Calculation */
+#define	R_IA_64_NONE		0	/* None */
+#define	R_IA_64_IMM14		0x21	/* immediate14	S + A */
+#define	R_IA_64_IMM22		0x22	/* immediate22	S + A */
+#define	R_IA_64_IMM64		0x23	/* immediate64	S + A */
+#define	R_IA_64_DIR32MSB	0x24	/* word32 MSB	S + A */
+#define	R_IA_64_DIR32LSB	0x25	/* word32 LSB	S + A */
+#define	R_IA_64_DIR64MSB	0x26	/* word64 MSB	S + A */
+#define	R_IA_64_DIR64LSB	0x27	/* word64 LSB	S + A */
+#define	R_IA_64_GPREL22		0x2a	/* immediate22	@gprel(S + A) */
+#define	R_IA_64_GPREL64I	0x2b	/* immediate64	@gprel(S + A) */
+#define	R_IA_64_GPREL32MSB	0x2c	/* word32 MSB	@gprel(S + A) */
+#define	R_IA_64_GPREL32LSB	0x2d	/* word32 LSB	@gprel(S + A) */
+#define	R_IA_64_GPREL64MSB	0x2e	/* word64 MSB	@gprel(S + A) */
+#define	R_IA_64_GPREL64LSB	0x2f	/* word64 LSB	@gprel(S + A) */
+#define	R_IA_64_LTOFF22		0x32	/* immediate22	@ltoff(S + A) */
+#define	R_IA_64_LTOFF64I	0x33	/* immediate64	@ltoff(S + A) */
+#define	R_IA_64_PLTOFF22	0x3a	/* immediate22	@pltoff(S + A) */
+#define	R_IA_64_PLTOFF64I	0x3b	/* immediate64	@pltoff(S + A) */
+#define	R_IA_64_PLTOFF64MSB	0x3e	/* word64 MSB	@pltoff(S + A) */
+#define	R_IA_64_PLTOFF64LSB	0x3f	/* word64 LSB	@pltoff(S + A) */
+#define	R_IA_64_FPTR64I		0x43	/* immediate64	@fptr(S + A) */
+#define	R_IA_64_FPTR32MSB	0x44	/* word32 MSB	@fptr(S + A) */
+#define	R_IA_64_FPTR32LSB	0x45	/* word32 LSB	@fptr(S + A) */
+#define	R_IA_64_FPTR64MSB	0x46	/* word64 MSB	@fptr(S + A) */
+#define	R_IA_64_FPTR64LSB	0x47	/* word64 LSB	@fptr(S + A) */
+#define	R_IA_64_PCREL60B	0x48	/* immediate60 form1 S + A - P */
+#define	R_IA_64_PCREL21B	0x49	/* immediate21 form1 S + A - P */
+#define	R_IA_64_PCREL21M	0x4a	/* immediate21 form2 S + A - P */
+#define	R_IA_64_PCREL21F	0x4b	/* immediate21 form3 S + A - P */
+#define	R_IA_64_PCREL32MSB	0x4c	/* word32 MSB	S + A - P */
+#define	R_IA_64_PCREL32LSB	0x4d	/* word32 LSB	S + A - P */
+#define	R_IA_64_PCREL64MSB	0x4e	/* word64 MSB	S + A - P */
+#define	R_IA_64_PCREL64LSB	0x4f	/* word64 LSB	S + A - P */
+#define	R_IA_64_LTOFF_FPTR22	0x52	/* immediate22	@ltoff(@fptr(S + A)) */
+#define	R_IA_64_LTOFF_FPTR64I	0x53	/* immediate64	@ltoff(@fptr(S + A)) */
+#define	R_IA_64_LTOFF_FPTR32MSB	0x54	/* word32 MSB	@ltoff(@fptr(S + A)) */
+#define	R_IA_64_LTOFF_FPTR32LSB	0x55	/* word32 LSB	@ltoff(@fptr(S + A)) */
+#define	R_IA_64_LTOFF_FPTR64MSB	0x56	/* word64 MSB	@ltoff(@fptr(S + A)) */
+#define	R_IA_64_LTOFF_FPTR64LSB	0x57	/* word64 LSB	@ltoff(@fptr(S + A)) */
+#define	R_IA_64_SEGREL32MSB	0x5c	/* word32 MSB	@segrel(S + A) */
+#define	R_IA_64_SEGREL32LSB	0x5d	/* word32 LSB	@segrel(S + A) */
+#define	R_IA_64_SEGREL64MSB	0x5e	/* word64 MSB	@segrel(S + A) */
+#define	R_IA_64_SEGREL64LSB	0x5f	/* word64 LSB	@segrel(S + A) */
+#define	R_IA_64_SECREL32MSB	0x64	/* word32 MSB	@secrel(S + A) */
+#define	R_IA_64_SECREL32LSB	0x65	/* word32 LSB	@secrel(S + A) */
+#define	R_IA_64_SECREL64MSB	0x66	/* word64 MSB	@secrel(S + A) */
+#define	R_IA_64_SECREL64LSB	0x67	/* word64 LSB	@secrel(S + A) */
+#define	R_IA_64_REL32MSB	0x6c	/* word32 MSB	BD + A */
+#define	R_IA_64_REL32LSB	0x6d	/* word32 LSB	BD + A */
+#define	R_IA_64_REL64MSB	0x6e	/* word64 MSB	BD + A */
+#define	R_IA_64_REL64LSB	0x6f	/* word64 LSB	BD + A */
+#define	R_IA_64_LTV32MSB	0x74	/* word32 MSB	S + A */
+#define	R_IA_64_LTV32LSB	0x75	/* word32 LSB	S + A */
+#define	R_IA_64_LTV64MSB	0x76	/* word64 MSB	S + A */
+#define	R_IA_64_LTV64LSB	0x77	/* word64 LSB	S + A */
+#define	R_IA_64_PCREL21BI	0x79	/* immediate21 form1 S + A - P */
+#define	R_IA_64_PCREL22		0x7a	/* immediate22	S + A - P */
+#define	R_IA_64_PCREL64I	0x7b	/* immediate64	S + A - P */
+#define	R_IA_64_IPLTMSB		0x80	/* function descriptor MSB special */
+#define	R_IA_64_IPLTLSB		0x81	/* function descriptor LSB speciaal */
+#define	R_IA_64_SUB		0x85	/* immediate64	A - S */
+#define	R_IA_64_LTOFF22X	0x86	/* immediate22	special */
+#define	R_IA_64_LDXMOV		0x87	/* immediate22	special */
+#define	R_IA_64_TPREL14		0x91	/* imm14	@tprel(S + A) */
+#define	R_IA_64_TPREL22		0x92	/* imm22	@tprel(S + A) */
+#define	R_IA_64_TPREL64I	0x93	/* imm64	@tprel(S + A) */
+#define	R_IA_64_TPREL64MSB	0x96	/* word64 MSB	@tprel(S + A) */
+#define	R_IA_64_TPREL64LSB	0x97	/* word64 LSB	@tprel(S + A) */
+#define	R_IA_64_LTOFF_TPREL22	0x9a	/* imm22	@ltoff(@tprel(S+A)) */
+#define	R_IA_64_DTPMOD64MSB	0xa6	/* word64 MSB	@dtpmod(S + A) */
+#define	R_IA_64_DTPMOD64LSB	0xa7	/* word64 LSB	@dtpmod(S + A) */
+#define	R_IA_64_LTOFF_DTPMOD22	0xaa	/* imm22	@ltoff(@dtpmod(S+A)) */
+#define	R_IA_64_DTPREL14	0xb1	/* imm14	@dtprel(S + A) */
+#define	R_IA_64_DTPREL22	0xb2	/* imm22	@dtprel(S + A) */
+#define	R_IA_64_DTPREL64I	0xb3	/* imm64	@dtprel(S + A) */
+#define	R_IA_64_DTPREL32MSB	0xb4	/* word32 MSB	@dtprel(S + A) */
+#define	R_IA_64_DTPREL32LSB	0xb5	/* word32 LSB	@dtprel(S + A) */
+#define	R_IA_64_DTPREL64MSB	0xb6	/* word64 MSB	@dtprel(S + A) */
+#define	R_IA_64_DTPREL64LSB	0xb7	/* word64 LSB	@dtprel(S + A) */
+#define	R_IA_64_LTOFF_DTPREL22	0xba	/* imm22	@ltoff(@dtprel(S+A)) */
+
+#define	R_MIPS_NONE	0	/* No reloc */
+#define	R_MIPS_16	1	/* Direct 16 bit */
+#define	R_MIPS_32	2	/* Direct 32 bit */
+#define	R_MIPS_REL32	3	/* PC relative 32 bit */
+#define	R_MIPS_26	4	/* Direct 26 bit shifted */
+#define	R_MIPS_HI16	5	/* High 16 bit */
+#define	R_MIPS_LO16	6	/* Low 16 bit */
+#define	R_MIPS_GPREL16	7	/* GP relative 16 bit */
+#define	R_MIPS_LITERAL	8	/* 16 bit literal entry */
+#define	R_MIPS_GOT16	9	/* 16 bit GOT entry */
+#define	R_MIPS_PC16	10	/* PC relative 16 bit */
+#define	R_MIPS_CALL16	11	/* 16 bit GOT entry for function */
+#define	R_MIPS_GPREL32	12	/* GP relative 32 bit */
+#define	R_MIPS_64	18	/* Direct 64 bit */
+#define	R_MIPS_GOTHI16	21	/* GOT HI 16 bit */
+#define	R_MIPS_GOTLO16	22	/* GOT LO 16 bit */
+#define	R_MIPS_CALLHI16 30	/* upper 16 bit GOT entry for function */
+#define	R_MIPS_CALLLO16 31	/* lower 16 bit GOT entry for function */
+
+#define	R_PPC_NONE		0	/* No relocation. */
+#define	R_PPC_ADDR32		1
+#define	R_PPC_ADDR24		2
+#define	R_PPC_ADDR16		3
+#define	R_PPC_ADDR16_LO		4
+#define	R_PPC_ADDR16_HI		5
+#define	R_PPC_ADDR16_HA		6
+#define	R_PPC_ADDR14		7
+#define	R_PPC_ADDR14_BRTAKEN	8
+#define	R_PPC_ADDR14_BRNTAKEN	9
+#define	R_PPC_REL24		10
+#define	R_PPC_REL14		11
+#define	R_PPC_REL14_BRTAKEN	12
+#define	R_PPC_REL14_BRNTAKEN	13
+#define	R_PPC_GOT16		14
+#define	R_PPC_GOT16_LO		15
+#define	R_PPC_GOT16_HI		16
+#define	R_PPC_GOT16_HA		17
+#define	R_PPC_PLTREL24		18
+#define	R_PPC_COPY		19
+#define	R_PPC_GLOB_DAT		20
+#define	R_PPC_JMP_SLOT		21
+#define	R_PPC_RELATIVE		22
+#define	R_PPC_LOCAL24PC		23
+#define	R_PPC_UADDR32		24
+#define	R_PPC_UADDR16		25
+#define	R_PPC_REL32		26
+#define	R_PPC_PLT32		27
+#define	R_PPC_PLTREL32		28
+#define	R_PPC_PLT16_LO		29
+#define	R_PPC_PLT16_HI		30
+#define	R_PPC_PLT16_HA		31
+#define	R_PPC_SDAREL16		32
+#define	R_PPC_SECTOFF		33
+#define	R_PPC_SECTOFF_LO	34
+#define	R_PPC_SECTOFF_HI	35
+#define	R_PPC_SECTOFF_HA	36
+
+/*
+ * 64-bit relocations
+ */
+#define	R_PPC64_ADDR64		38
+#define	R_PPC64_ADDR16_HIGHER	39
+#define	R_PPC64_ADDR16_HIGHERA	40
+#define	R_PPC64_ADDR16_HIGHEST	41
+#define	R_PPC64_ADDR16_HIGHESTA	42
+#define	R_PPC64_UADDR64		43
+#define	R_PPC64_REL64		44
+#define	R_PPC64_PLT64		45
+#define	R_PPC64_PLTREL64	46
+#define	R_PPC64_TOC16		47
+#define	R_PPC64_TOC16_LO	48
+#define	R_PPC64_TOC16_HI	49
+#define	R_PPC64_TOC16_HA	50
+#define	R_PPC64_TOC		51
+#define	R_PPC64_DTPMOD64	68
+#define	R_PPC64_TPREL64		73
+#define	R_PPC64_DTPREL64	78
+
+/*
+ * TLS relocations
+ */
+#define	R_PPC_TLS		67
+#define	R_PPC_DTPMOD32		68
+#define	R_PPC_TPREL16		69
+#define	R_PPC_TPREL16_LO	70
+#define	R_PPC_TPREL16_HI	71
+#define	R_PPC_TPREL16_HA	72
+#define	R_PPC_TPREL32		73
+#define	R_PPC_DTPREL16		74
+#define	R_PPC_DTPREL16_LO	75
+#define	R_PPC_DTPREL16_HI	76
+#define	R_PPC_DTPREL16_HA	77
+#define	R_PPC_DTPREL32		78
+#define	R_PPC_GOT_TLSGD16	79
+#define	R_PPC_GOT_TLSGD16_LO	80
+#define	R_PPC_GOT_TLSGD16_HI	81
+#define	R_PPC_GOT_TLSGD16_HA	82
+#define	R_PPC_GOT_TLSLD16	83
+#define	R_PPC_GOT_TLSLD16_LO	84
+#define	R_PPC_GOT_TLSLD16_HI	85
+#define	R_PPC_GOT_TLSLD16_HA	86
+#define	R_PPC_GOT_TPREL16	87
+#define	R_PPC_GOT_TPREL16_LO	88
+#define	R_PPC_GOT_TPREL16_HI	89
+#define	R_PPC_GOT_TPREL16_HA	90
+
+/*
+ * The remaining relocs are from the Embedded ELF ABI, and are not in the
+ *  SVR4 ELF ABI.
+ */
+
+#define	R_PPC_EMB_NADDR32	101
+#define	R_PPC_EMB_NADDR16	102
+#define	R_PPC_EMB_NADDR16_LO	103
+#define	R_PPC_EMB_NADDR16_HI	104
+#define	R_PPC_EMB_NADDR16_HA	105
+#define	R_PPC_EMB_SDAI16	106
+#define	R_PPC_EMB_SDA2I16	107
+#define	R_PPC_EMB_SDA2REL	108
+#define	R_PPC_EMB_SDA21		109
+#define	R_PPC_EMB_MRKREF	110
+#define	R_PPC_EMB_RELSEC16	111
+#define	R_PPC_EMB_RELST_LO	112
+#define	R_PPC_EMB_RELST_HI	113
+#define	R_PPC_EMB_RELST_HA	114
+#define	R_PPC_EMB_BIT_FLD	115
+#define	R_PPC_EMB_RELSDA	116
+
+#define	R_SPARC_NONE		0
+#define	R_SPARC_8		1
+#define	R_SPARC_16		2
+#define	R_SPARC_32		3
+#define	R_SPARC_DISP8		4
+#define	R_SPARC_DISP16		5
+#define	R_SPARC_DISP32		6
+#define	R_SPARC_WDISP30		7
+#define	R_SPARC_WDISP22		8
+#define	R_SPARC_HI22		9
+#define	R_SPARC_22		10
+#define	R_SPARC_13		11
+#define	R_SPARC_LO10		12
+#define	R_SPARC_GOT10		13
+#define	R_SPARC_GOT13		14
+#define	R_SPARC_GOT22		15
+#define	R_SPARC_PC10		16
+#define	R_SPARC_PC22		17
+#define	R_SPARC_WPLT30		18
+#define	R_SPARC_COPY		19
+#define	R_SPARC_GLOB_DAT	20
+#define	R_SPARC_JMP_SLOT	21
+#define	R_SPARC_RELATIVE	22
+#define	R_SPARC_UA32		23
+#define	R_SPARC_PLT32		24
+#define	R_SPARC_HIPLT22		25
+#define	R_SPARC_LOPLT10		26
+#define	R_SPARC_PCPLT32		27
+#define	R_SPARC_PCPLT22		28
+#define	R_SPARC_PCPLT10		29
+#define	R_SPARC_10		30
+#define	R_SPARC_11		31
+#define	R_SPARC_64		32
+#define	R_SPARC_OLO10		33
+#define	R_SPARC_HH22		34
+#define	R_SPARC_HM10		35
+#define	R_SPARC_LM22		36
+#define	R_SPARC_PC_HH22		37
+#define	R_SPARC_PC_HM10		38
+#define	R_SPARC_PC_LM22		39
+#define	R_SPARC_WDISP16		40
+#define	R_SPARC_WDISP19		41
+#define	R_SPARC_GLOB_JMP	42
+#define	R_SPARC_7		43
+#define	R_SPARC_5		44
+#define	R_SPARC_6		45
+#define	R_SPARC_DISP64		46
+#define	R_SPARC_PLT64		47
+#define	R_SPARC_HIX22		48
+#define	R_SPARC_LOX10		49
+#define	R_SPARC_H44		50
+#define	R_SPARC_M44		51
+#define	R_SPARC_L44		52
+#define	R_SPARC_REGISTER	53
+#define	R_SPARC_UA64		54
+#define	R_SPARC_UA16		55
+#define	R_SPARC_TLS_GD_HI22	56
+#define	R_SPARC_TLS_GD_LO10	57
+#define	R_SPARC_TLS_GD_ADD	58
+#define	R_SPARC_TLS_GD_CALL	59
+#define	R_SPARC_TLS_LDM_HI22	60
+#define	R_SPARC_TLS_LDM_LO10	61
+#define	R_SPARC_TLS_LDM_ADD	62
+#define	R_SPARC_TLS_LDM_CALL	63
+#define	R_SPARC_TLS_LDO_HIX22	64
+#define	R_SPARC_TLS_LDO_LOX10	65
+#define	R_SPARC_TLS_LDO_ADD	66
+#define	R_SPARC_TLS_IE_HI22	67
+#define	R_SPARC_TLS_IE_LO10	68
+#define	R_SPARC_TLS_IE_LD	69
+#define	R_SPARC_TLS_IE_LDX	70
+#define	R_SPARC_TLS_IE_ADD	71
+#define	R_SPARC_TLS_LE_HIX22	72
+#define	R_SPARC_TLS_LE_LOX10	73
+#define	R_SPARC_TLS_DTPMOD32	74
+#define	R_SPARC_TLS_DTPMOD64	75
+#define	R_SPARC_TLS_DTPOFF32	76
+#define	R_SPARC_TLS_DTPOFF64	77
+#define	R_SPARC_TLS_TPOFF32	78
+#define	R_SPARC_TLS_TPOFF64	79
+
+#define	R_X86_64_NONE		0	/* No relocation. */
+#define	R_X86_64_64		1	/* Add 64 bit symbol value. */
+#define	R_X86_64_PC32		2	/* PC-relative 32 bit signed sym value. */
+#define	R_X86_64_GOT32		3	/* PC-relative 32 bit GOT offset. */
+#define	R_X86_64_PLT32		4	/* PC-relative 32 bit PLT offset. */
+#define	R_X86_64_COPY		5	/* Copy data from shared object. */
+#define	R_X86_64_GLOB_DAT	6	/* Set GOT entry to data address. */
+#define	R_X86_64_JMP_SLOT	7	/* Set GOT entry to code address. */
+#define	R_X86_64_RELATIVE	8	/* Add load address of shared object. */
+#define	R_X86_64_GOTPCREL	9	/* Add 32 bit signed pcrel offset to GOT. */
+#define	R_X86_64_32		10	/* Add 32 bit zero extended symbol value */
+#define	R_X86_64_32S		11	/* Add 32 bit sign extended symbol value */
+#define	R_X86_64_16		12	/* Add 16 bit zero extended symbol value */
+#define	R_X86_64_PC16		13	/* Add 16 bit signed extended pc relative symbol value */
+#define	R_X86_64_8		14	/* Add 8 bit zero extended symbol value */
+#define	R_X86_64_PC8		15	/* Add 8 bit signed extended pc relative symbol value */
+#define	R_X86_64_DTPMOD64	16	/* ID of module containing symbol */
+#define	R_X86_64_DTPOFF64	17	/* Offset in TLS block */
+#define	R_X86_64_TPOFF64	18	/* Offset in static TLS block */
+#define	R_X86_64_TLSGD		19	/* PC relative offset to GD GOT entry */
+#define	R_X86_64_TLSLD		20	/* PC relative offset to LD GOT entry */
+#define	R_X86_64_DTPOFF32	21	/* Offset in TLS block */
+#define	R_X86_64_GOTTPOFF	22	/* PC relative offset to IE GOT entry */
+#define	R_X86_64_TPOFF32	23	/* Offset in static TLS block */
+#define	R_X86_64_IRELATIVE	37
+
+
+#endif /* !_SYS_ELF_COMMON_H_ */
diff --git a/core/arch/arm/kernel/elf_load.c b/core/arch/arm/kernel/elf_load.c
new file mode 100644
index 0000000..420ba59
--- /dev/null
+++ b/core/arch/arm/kernel/elf_load.c
@@ -0,0 +1,646 @@
+/*
+ * 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 <types_ext.h>
+#include <tee_api_types.h>
+#include <tee_api_defines.h>
+#include <kernel/tee_misc.h>
+#include <tee/tee_cryp_provider.h>
+#include <stdlib.h>
+#include <string.h>
+#include <util.h>
+#include <trace.h>
+#include "elf_load.h"
+#include "elf_common.h"
+#include "elf32.h"
+#include "elf64.h"
+
+struct elf_load_state {
+	bool is_32bit;
+
+	uint8_t *nwdata;
+	size_t nwdata_len;
+
+	void *hash_ctx;
+	uint32_t hash_algo;
+
+	size_t next_offs;
+
+	void *ta_head;
+	size_t ta_head_size;
+
+	void *ehdr;
+	void *phdr;
+
+	size_t vasize;
+	void *shdr;
+};
+
+/* Replicates the fields we need from Elf{32,64}_Ehdr */
+struct elf_ehdr {
+	size_t e_phoff;
+	size_t e_shoff;
+	uint32_t e_phentsize;
+	uint32_t e_phnum;
+	uint32_t e_shentsize;
+	uint32_t e_shnum;
+};
+
+/* Replicates the fields we need from Elf{32,64}_Phdr */
+struct elf_phdr {
+	uint32_t p_type;
+	uint32_t p_flags;
+	uintptr_t p_vaddr;
+	size_t p_filesz;
+	size_t p_memsz;
+	size_t p_offset;
+};
+
+#ifdef ARM64
+#define DO_ACTION(state, is_32bit_action, is_64bit_action) \
+	do { \
+		if ((state)->is_32bit) { \
+			is_32bit_action; \
+		} else { \
+			is_64bit_action; \
+		} \
+	} while (0)
+#else
+/* No need to assert state->is_32bit since that is caught before this is used */
+#define DO_ACTION(state, is_32bit_action, is_64bit_action) is_32bit_action
+#endif
+
+#define COPY_EHDR(dst, src) \
+	do { \
+		(dst)->e_phoff = (src)->e_phoff; \
+		(dst)->e_shoff = (src)->e_shoff; \
+		(dst)->e_phentsize = (src)->e_phentsize; \
+		(dst)->e_phnum = (src)->e_phnum; \
+		(dst)->e_shentsize = (src)->e_shentsize; \
+		(dst)->e_shnum = (src)->e_shnum; \
+	} while (0)
+static void copy_ehdr(struct elf_ehdr *ehdr, struct elf_load_state *state)
+{
+	DO_ACTION(state, COPY_EHDR(ehdr, ((Elf32_Ehdr *)state->ehdr)),
+			 COPY_EHDR(ehdr, ((Elf64_Ehdr *)state->ehdr)));
+}
+
+static uint32_t get_shdr_type(struct elf_load_state *state, size_t idx)
+{
+	DO_ACTION(state, return ((Elf32_Shdr *)state->shdr + idx)->sh_type,
+			 return ((Elf64_Shdr *)state->shdr + idx)->sh_type);
+}
+
+#define COPY_PHDR(dst, src) \
+	do { \
+		(dst)->p_type = (src)->p_type; \
+		(dst)->p_vaddr = (src)->p_vaddr; \
+		(dst)->p_filesz = (src)->p_filesz; \
+		(dst)->p_memsz = (src)->p_memsz; \
+		(dst)->p_offset = (src)->p_offset; \
+		(dst)->p_flags = (src)->p_flags; \
+	} while (0)
+static void copy_phdr(struct elf_phdr *phdr, struct elf_load_state *state,
+			size_t idx)
+{
+	DO_ACTION(state, COPY_PHDR(phdr, ((Elf32_Phdr *)state->phdr + idx)),
+			 COPY_PHDR(phdr, ((Elf64_Phdr *)state->phdr + idx)));
+}
+
+static TEE_Result advance_to(struct elf_load_state *state, size_t offs)
+{
+	TEE_Result res;
+
+	if (offs < state->next_offs)
+		return TEE_ERROR_BAD_STATE;
+	if (offs == state->next_offs)
+		return TEE_SUCCESS;
+
+	if (offs > state->nwdata_len)
+		return TEE_ERROR_SECURITY;
+
+	res = crypto_ops.hash.update(state->hash_ctx, state->hash_algo,
+			state->nwdata + state->next_offs,
+			offs - state->next_offs);
+	if (res != TEE_SUCCESS)
+		return res;
+	state->next_offs = offs;
+	return res;
+}
+
+static TEE_Result copy_to(struct elf_load_state *state,
+			void *dst, size_t dst_size, size_t dst_offs,
+			size_t offs, size_t len)
+{
+	TEE_Result res;
+
+	res = advance_to(state, offs);
+	if (res != TEE_SUCCESS)
+		return res;
+	if (!len)
+		return TEE_SUCCESS;
+
+	/* Check for integer overflow */
+	if ((len + dst_offs) < dst_offs || (len + dst_offs) > dst_size ||
+	    (len + offs) < offs || (len + offs) > state->nwdata_len)
+		return TEE_ERROR_SECURITY;
+
+	memcpy((uint8_t *)dst + dst_offs, state->nwdata + offs, len);
+	res = crypto_ops.hash.update(state->hash_ctx, state->hash_algo,
+				      (uint8_t *)dst + dst_offs, len);
+	if (res != TEE_SUCCESS)
+		return res;
+	state->next_offs = offs + len;
+	return res;
+}
+
+static TEE_Result alloc_and_copy_to(void **p, struct elf_load_state *state,
+			size_t offs, size_t len)
+{
+	TEE_Result res;
+	void *buf;
+
+	buf = malloc(len);
+	if (!buf)
+		return TEE_ERROR_OUT_OF_MEMORY;
+	res = copy_to(state, buf, len, 0, offs, len);
+	if (res == TEE_SUCCESS)
+		*p = buf;
+	else
+		free(buf);
+	return res;
+}
+
+TEE_Result elf_load_init(void *hash_ctx, uint32_t hash_algo, uint8_t *nwdata,
+			size_t nwdata_len, struct elf_load_state **ret_state)
+{
+	struct elf_load_state *state;
+
+	state = calloc(1, sizeof(*state));
+	if (!state)
+		return TEE_ERROR_OUT_OF_MEMORY;
+	state->hash_ctx = hash_ctx;
+	state->hash_algo = hash_algo;
+	state->nwdata = nwdata;
+	state->nwdata_len = nwdata_len;
+	*ret_state = state;
+	return TEE_SUCCESS;
+}
+
+static TEE_Result e32_load_ehdr(struct elf_load_state *state, Elf32_Ehdr *ehdr)
+{
+	if (ehdr->e_ident[EI_VERSION] != EV_CURRENT ||
+	    ehdr->e_ident[EI_CLASS] != ELFCLASS32 ||
+	    ehdr->e_ident[EI_DATA] != ELFDATA2LSB ||
+	    ehdr->e_ident[EI_OSABI] != ELFOSABI_NONE ||
+	    ehdr->e_type != ET_DYN || ehdr->e_machine != EM_ARM ||
+	    (ehdr->e_flags & EF_ARM_ABIMASK) != EF_ARM_ABI_VERSION ||
+#ifndef CFG_WITH_VFP
+	    (ehdr->e_flags & EF_ARM_ABI_FLOAT_HARD) ||
+#endif
+	    ehdr->e_phentsize != sizeof(Elf32_Phdr) ||
+	    ehdr->e_shentsize != sizeof(Elf32_Shdr))
+		return TEE_ERROR_BAD_FORMAT;
+
+	state->ehdr = malloc(sizeof(*ehdr));
+	if (!state->ehdr)
+		return TEE_ERROR_OUT_OF_MEMORY;
+	memcpy(state->ehdr, ehdr, sizeof(*ehdr));
+	state->is_32bit = true;
+	return TEE_SUCCESS;
+}
+
+#ifdef ARM64
+static TEE_Result e64_load_ehdr(struct elf_load_state *state, Elf32_Ehdr *eh32)
+{
+	TEE_Result res;
+	Elf64_Ehdr *ehdr = NULL;
+
+	if (eh32->e_ident[EI_VERSION] != EV_CURRENT ||
+	    eh32->e_ident[EI_CLASS] != ELFCLASS64 ||
+	    eh32->e_ident[EI_DATA] != ELFDATA2LSB ||
+	    eh32->e_ident[EI_OSABI] != ELFOSABI_NONE ||
+	    eh32->e_type != ET_DYN || eh32->e_machine != EM_AARCH64)
+		return TEE_ERROR_BAD_FORMAT;
+
+	ehdr = malloc(sizeof(*ehdr));
+	if (!ehdr)
+		return TEE_ERROR_OUT_OF_MEMORY;
+	state->ehdr = ehdr;
+	memcpy(ehdr, eh32, sizeof(*eh32));
+	res = copy_to(state, ehdr, sizeof(*ehdr), sizeof(*eh32),
+		      sizeof(*eh32), sizeof(*ehdr) - sizeof(*eh32));
+	if (res != TEE_SUCCESS)
+		return res;
+
+	if (ehdr->e_flags || ehdr->e_phentsize != sizeof(Elf64_Phdr) ||
+	    ehdr->e_shentsize != sizeof(Elf64_Shdr))
+		return TEE_ERROR_BAD_FORMAT;
+
+	state->ehdr = ehdr;
+	state->is_32bit = false;
+	return TEE_SUCCESS;
+}
+#else /*ARM64*/
+static TEE_Result e64_load_ehdr(struct elf_load_state *state __unused,
+			Elf32_Ehdr *eh32 __unused)
+{
+	return TEE_ERROR_NOT_SUPPORTED;
+}
+#endif /*ARM64*/
+
+static TEE_Result load_head(struct elf_load_state *state, size_t head_size)
+{
+	TEE_Result res;
+	size_t n;
+	void *p;
+	struct elf_ehdr ehdr;
+	struct elf_phdr phdr;
+	struct elf_phdr phdr0;
+
+	copy_ehdr(&ehdr, state);
+	/*
+	 * Program headers are supposed to be arranged as:
+	 * PT_LOAD [0] : .ta_head ...
+	 * ...
+	 * PT_LOAD [n]
+	 *
+	 * .ta_head must be located first in the first program header,
+	 * which also has to be of PT_LOAD type.
+	 *
+	 * A PT_DYNAMIC segment may appear, but is ignored. Any other
+	 * segment except PT_LOAD and PT_DYNAMIC will cause an error. All
+	 * sections not included by a PT_LOAD segment are ignored.
+	 */
+	if (ehdr.e_phnum < 1)
+		return TEE_ERROR_BAD_FORMAT;
+
+	/* Check for integer overflow */
+	if (((uint64_t)ehdr.e_phnum * ehdr.e_phentsize) > SIZE_MAX)
+		return TEE_ERROR_SECURITY;
+
+	res = alloc_and_copy_to(&p, state, ehdr.e_phoff,
+				ehdr.e_phnum * ehdr.e_phentsize);
+	if (res != TEE_SUCCESS)
+		return res;
+	state->phdr = p;
+
+	/*
+	 * Check that the first program header is a PT_LOAD (not strictly
+	 * needed but our link script is supposed to arrange it that way)
+	 * and that it starts at virtual address 0.
+	 */
+	copy_phdr(&phdr0, state, 0);
+	if (phdr0.p_type != PT_LOAD || phdr0.p_vaddr != 0)
+		return TEE_ERROR_BAD_FORMAT;
+
+	/*
+	 * Calculate amount of required virtual memory for TA. Find the max
+	 * address used by a PT_LOAD type. Note that last PT_LOAD type
+	 * dictates the total amount of needed memory. Eventual holes in
+	 * the memory will also be allocated.
+	 *
+	 * Note that this loop will terminate at n = 0 if not earlier
+	 * as we already know from above that state->phdr[0].p_type == PT_LOAD
+	 */
+	n = ehdr.e_phnum;
+	do {
+		n--;
+		copy_phdr(&phdr, state, n);
+	} while (phdr.p_type != PT_LOAD);
+	state->vasize = phdr.p_vaddr + phdr.p_memsz;
+
+	/* Check for integer overflow */
+	if (state->vasize < phdr.p_vaddr)
+		return TEE_ERROR_SECURITY;
+
+	/*
+	 * Read .ta_head from first segment, make sure the segment is large
+	 * enough. We're only interested in seeing that the
+	 * TA_FLAG_EXEC_DDR flag is set. If that's true we set that flag in
+	 * the TA context to enable mapping the TA. Later when this
+	 * function has returned and the hash has been verified the flags
+	 * field will be updated with eventual other flags.
+	 */
+	if (phdr0.p_filesz < head_size)
+		return TEE_ERROR_BAD_FORMAT;
+	res = alloc_and_copy_to(&p, state, phdr0.p_offset, head_size);
+	if (res == TEE_SUCCESS) {
+		state->ta_head = p;
+		state->ta_head_size = head_size;
+	}
+	return res;
+}
+
+TEE_Result elf_load_head(struct elf_load_state *state, size_t head_size,
+			void **head, size_t *vasize, bool *is_32bit)
+{
+	TEE_Result res;
+	Elf32_Ehdr ehdr;
+
+	/*
+	 * The ELF resides in shared memory, to avoid attacks based on
+	 * modifying the ELF while we're parsing it here we only read each
+	 * byte from the ELF once. We're also hashing the ELF while reading
+	 * so we're limited to only read the ELF sequentially from start to
+	 * end.
+	 */
+
+	res = copy_to(state, &ehdr, sizeof(ehdr), 0, 0, sizeof(Elf32_Ehdr));
+	if (res != TEE_SUCCESS)
+		return res;
+
+	if (!IS_ELF(ehdr))
+		return TEE_ERROR_BAD_FORMAT;
+	res = e32_load_ehdr(state, &ehdr);
+	if (res == TEE_ERROR_BAD_FORMAT)
+		res = e64_load_ehdr(state, &ehdr);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = load_head(state, head_size);
+	if (res == TEE_SUCCESS) {
+		*head = state->ta_head;
+		*vasize = state->vasize;
+		*is_32bit = state->is_32bit;
+	}
+	return res;
+}
+
+TEE_Result elf_load_get_next_segment(struct elf_load_state *state, size_t *idx,
+			vaddr_t *vaddr, size_t *size, uint32_t *flags)
+{
+	struct elf_ehdr ehdr;
+
+	copy_ehdr(&ehdr, state);
+	while (*idx < ehdr.e_phnum) {
+		struct elf_phdr phdr;
+
+		copy_phdr(&phdr, state, *idx);
+		(*idx)++;
+		if (phdr.p_type == PT_LOAD) {
+			if (vaddr)
+				*vaddr = phdr.p_vaddr;
+			if (size)
+				*size = phdr.p_memsz;
+			if (flags)
+				*flags = phdr.p_flags;
+			return TEE_SUCCESS;
+		}
+	}
+	return TEE_ERROR_ITEM_NOT_FOUND;
+}
+
+static TEE_Result e32_process_rel(struct elf_load_state *state, size_t rel_sidx,
+			vaddr_t vabase)
+{
+	Elf32_Ehdr *ehdr = state->ehdr;
+	Elf32_Shdr *shdr = state->shdr;
+	Elf32_Rel *rel;
+	Elf32_Rel *rel_end;
+	size_t sym_tab_idx;
+	Elf32_Sym *sym_tab = NULL;
+	size_t num_syms = 0;
+
+	if (shdr[rel_sidx].sh_type != SHT_REL)
+		return TEE_ERROR_NOT_IMPLEMENTED;
+
+	if (shdr[rel_sidx].sh_entsize != sizeof(Elf32_Rel))
+		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(Elf32_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 = (Elf32_Sym *)(vabase + shdr[sym_tab_idx].sh_addr);
+		if (!ALIGNMENT_IS_OK(sym_tab, Elf32_Sym))
+			return TEE_ERROR_BAD_FORMAT;
+
+		num_syms = shdr[sym_tab_idx].sh_size / sizeof(Elf32_Sym);
+	}
+
+	/* Check the address is inside TA memory */
+	if (shdr[rel_sidx].sh_addr >= state->vasize)
+		return TEE_ERROR_BAD_FORMAT;
+	rel = (Elf32_Rel *)(vabase + shdr[rel_sidx].sh_addr);
+	if (!ALIGNMENT_IS_OK(rel, Elf32_Rel))
+		return TEE_ERROR_BAD_FORMAT;
+
+	/* Check the address is inside TA memory */
+	if ((shdr[rel_sidx].sh_addr + shdr[rel_sidx].sh_size) >= state->vasize)
+		return TEE_ERROR_BAD_FORMAT;
+	rel_end = rel + shdr[rel_sidx].sh_size / sizeof(Elf32_Rel);
+	for (; rel < rel_end; rel++) {
+		Elf32_Addr *where;
+		size_t sym_idx;
+
+		/* Check the address is inside TA memory */
+		if (rel->r_offset >= state->vasize)
+			return TEE_ERROR_BAD_FORMAT;
+
+		where = (Elf32_Addr *)(vabase + rel->r_offset);
+		if (!ALIGNMENT_IS_OK(where, Elf32_Addr))
+			return TEE_ERROR_BAD_FORMAT;
+
+		switch (ELF32_R_TYPE(rel->r_info)) {
+		case R_ARM_ABS32:
+			sym_idx = ELF32_R_SYM(rel->r_info);
+			if (sym_idx >= num_syms)
+				return TEE_ERROR_BAD_FORMAT;
+
+			*where += vabase + sym_tab[sym_idx].st_value;
+			break;
+		case R_ARM_RELATIVE:
+			*where += vabase;
+			break;
+		default:
+			EMSG("Unknown relocation type %d",
+			     ELF32_R_TYPE(rel->r_info));
+			return TEE_ERROR_BAD_FORMAT;
+		}
+	}
+	return TEE_SUCCESS;
+}
+
+#ifdef ARM64
+static TEE_Result e64_process_rel(struct elf_load_state *state,
+			size_t rel_sidx, vaddr_t vabase)
+{
+	Elf64_Shdr *shdr = state->shdr;
+	Elf64_Rela *rela;
+	Elf64_Rela *rela_end;
+
+	if (shdr[rel_sidx].sh_type != SHT_RELA)
+		return TEE_ERROR_NOT_IMPLEMENTED;
+
+	if (shdr[rel_sidx].sh_entsize != sizeof(Elf64_Rela))
+		return TEE_ERROR_BAD_FORMAT;
+
+	/* Check the address is inside TA memory */
+	if (shdr[rel_sidx].sh_addr >= state->vasize)
+		return TEE_ERROR_BAD_FORMAT;
+	rela = (Elf64_Rela *)(vabase + shdr[rel_sidx].sh_addr);
+	if (!ALIGNMENT_IS_OK(rela, Elf64_Rela))
+		return TEE_ERROR_BAD_FORMAT;
+
+	/* Check the address is inside TA memory */
+	if ((shdr[rel_sidx].sh_addr + shdr[rel_sidx].sh_size) >= state->vasize)
+		return TEE_ERROR_BAD_FORMAT;
+	rela_end = rela + shdr[rel_sidx].sh_size / sizeof(Elf64_Rela);
+	for (; rela < rela_end; rela++) {
+		Elf64_Addr *where;
+
+		/* Check the address is inside TA memory */
+		if (rela->r_offset >= state->vasize)
+			return TEE_ERROR_BAD_FORMAT;
+
+		where = (Elf64_Addr *)(vabase + rela->r_offset);
+		if (!ALIGNMENT_IS_OK(where, Elf64_Addr))
+			return TEE_ERROR_BAD_FORMAT;
+
+		switch (ELF64_R_TYPE(rela->r_info)) {
+		case R_AARCH64_RELATIVE:
+			*where = rela->r_addend + vabase;
+			break;
+		default:
+			EMSG("Unknown relocation type %zd",
+			     ELF64_R_TYPE(rela->r_info));
+			return TEE_ERROR_BAD_FORMAT;
+		}
+	}
+	return TEE_SUCCESS;
+}
+#else /*ARM64*/
+static TEE_Result e64_process_rel(struct elf_load_state *state __unused,
+			size_t rel_sidx __unused, vaddr_t vabase __unused)
+{
+	return TEE_ERROR_NOT_SUPPORTED;
+}
+#endif /*ARM64*/
+
+TEE_Result elf_load_body(struct elf_load_state *state, vaddr_t vabase)
+{
+	TEE_Result res;
+	size_t n;
+	void *p;
+	uint8_t *dst = (uint8_t *)vabase;
+	struct elf_ehdr ehdr;
+	size_t offs;
+
+	copy_ehdr(&ehdr, state);
+
+	/*
+	 * Zero initialize everything to make sure that all memory not
+	 * updated from the ELF is zero (covering .bss and eventual gaps).
+	 */
+	memset(dst, 0, state->vasize);
+
+	/*
+	 * Copy the segments
+	 */
+	memcpy(dst, state->ta_head, state->ta_head_size);
+	offs = state->ta_head_size;
+	for (n = 0; n < ehdr.e_phnum; n++) {
+		struct elf_phdr phdr;
+
+		copy_phdr(&phdr, state, n);
+		if (phdr.p_type != PT_LOAD)
+			continue;
+
+		res = copy_to(state, dst, state->vasize,
+			      phdr.p_vaddr + offs,
+			      phdr.p_offset + offs,
+			      phdr.p_filesz - offs);
+		if (res != TEE_SUCCESS)
+			return res;
+		offs = 0;
+	}
+
+	/*
+	 * We have now loaded all segments into TA memory, now we need to
+	 * process relocation information. To find relocation information
+	 * we need to locate the section headers. The section headers are
+	 * located somewhere between the last segment and the end of the
+	 * ELF.
+	 */
+	if (ehdr.e_shoff) {
+		/* We have section headers */
+		res = alloc_and_copy_to(&p, state, ehdr.e_shoff,
+					ehdr.e_shnum * ehdr.e_shentsize);
+		if (res != TEE_SUCCESS)
+			return res;
+		state->shdr = p;
+	}
+
+	/* Hash until end of ELF */
+	res = advance_to(state, state->nwdata_len);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	if (state->shdr) {
+		TEE_Result (*process_rel)(struct elf_load_state *state,
+					size_t rel_sidx, vaddr_t vabase);
+
+		if (state->is_32bit)
+			process_rel = e32_process_rel;
+		else
+			process_rel = e64_process_rel;
+
+		/* Process relocation */
+		for (n = 0; n < ehdr.e_shnum; n++) {
+			uint32_t sh_type = get_shdr_type(state, n);
+
+			if (sh_type == SHT_REL || sh_type == SHT_RELA) {
+				res = process_rel(state, n, vabase);
+				if (res != TEE_SUCCESS)
+					return res;
+			}
+		}
+	}
+
+	return TEE_SUCCESS;
+}
+
+void elf_load_final(struct elf_load_state *state)
+{
+	if (state) {
+		free(state->ta_head);
+		free(state->ehdr);
+		free(state->phdr);
+		free(state->shdr);
+		free(state);
+	}
+}
diff --git a/core/arch/arm/kernel/elf_load.h b/core/arch/arm/kernel/elf_load.h
new file mode 100644
index 0000000..4944e3a
--- /dev/null
+++ b/core/arch/arm/kernel/elf_load.h
@@ -0,0 +1,44 @@
+/*
+ * 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.
+ */
+#ifndef ELF_LOAD_H
+#define ELF_LOAD_H
+
+#include <types_ext.h>
+#include <tee_api_types.h>
+
+struct elf_load_state;
+
+TEE_Result elf_load_init(void *hash_ctx, uint32_t hash_algo, uint8_t *nwdata,
+			size_t nwdata_len, struct elf_load_state **state);
+TEE_Result elf_load_head(struct elf_load_state *state, size_t head_size,
+			void **head, size_t *vasize, bool *is_32bit);
+TEE_Result elf_load_body(struct elf_load_state *state, vaddr_t vabase);
+TEE_Result elf_load_get_next_segment(struct elf_load_state *state, size_t *idx,
+			vaddr_t *vaddr, size_t *size, uint32_t *flags);
+void elf_load_final(struct elf_load_state *state);
+
+#endif /*ELF_LOAD_H*/
diff --git a/core/arch/arm/kernel/generic_boot.c b/core/arch/arm/kernel/generic_boot.c
new file mode 100644
index 0000000..8f13c36
--- /dev/null
+++ b/core/arch/arm/kernel/generic_boot.c
@@ -0,0 +1,710 @@
+/*
+ * 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 <arm.h>
+#include <assert.h>
+#include <compiler.h>
+#include <inttypes.h>
+#include <keep.h>
+#include <kernel/generic_boot.h>
+#include <kernel/thread.h>
+#include <kernel/panic.h>
+#include <kernel/misc.h>
+#include <kernel/asan.h>
+#include <malloc.h>
+#include <mm/core_mmu.h>
+#include <mm/core_memprot.h>
+#include <mm/tee_mm.h>
+#include <mm/tee_mmu.h>
+#include <mm/tee_pager.h>
+#include <sm/tee_mon.h>
+#include <trace.h>
+#include <tee/tee_cryp_provider.h>
+#include <utee_defines.h>
+#include <util.h>
+#include <stdio.h>
+
+#include <platform_config.h>
+
+#if !defined(CFG_WITH_ARM_TRUSTED_FW)
+#include <sm/sm.h>
+#endif
+
+#if defined(CFG_WITH_VFP)
+#include <kernel/vfp.h>
+#endif
+
+#if defined(CFG_DT)
+#include <libfdt.h>
+#endif
+
+/*
+ * In this file we're using unsigned long to represent physical pointers as
+ * they are received in a single register when OP-TEE is initially entered.
+ * This limits 32-bit systems to only use make use of the lower 32 bits
+ * of a physical address for initial parameters.
+ *
+ * 64-bit systems on the other hand can use full 64-bit physical pointers.
+ */
+#define PADDR_INVALID		ULONG_MAX
+
+#if defined(CFG_BOOT_SECONDARY_REQUEST)
+paddr_t ns_entry_addrs[CFG_TEE_CORE_NB_CORE] __early_bss;
+static uint32_t spin_table[CFG_TEE_CORE_NB_CORE] __early_bss;
+#endif
+
+#ifdef CFG_BOOT_SYNC_CPU
+/*
+ * Array used when booting, to synchronize cpu.
+ * When 0, the cpu has not started.
+ * When 1, it has started
+ */
+uint32_t sem_cpu_sync[CFG_TEE_CORE_NB_CORE] __early_bss;
+#endif
+
+/* May be overridden in plat-$(PLATFORM)/main.c */
+__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)
+{
+}
+
+/* May be overridden in plat-$(PLATFORM)/main.c */
+__weak void main_secondary_init_gic(void)
+{
+}
+
+#if defined(CFG_WITH_ARM_TRUSTED_FW)
+void init_sec_mon(unsigned long nsec_entry __maybe_unused)
+{
+	assert(nsec_entry == PADDR_INVALID);
+	/* Do nothing as we don't have a secure monitor */
+}
+#else
+/* May be overridden in plat-$(PLATFORM)/main.c */
+__weak void init_sec_mon(unsigned long nsec_entry)
+{
+	struct sm_nsec_ctx *nsec_ctx;
+
+	assert(nsec_entry != PADDR_INVALID);
+
+	/* Initialize secure monitor */
+	nsec_ctx = sm_get_nsec_ctx();
+	nsec_ctx->mon_lr = nsec_entry;
+	nsec_ctx->mon_spsr = CPSR_MODE_SVC | CPSR_I;
+
+}
+#endif
+
+#if defined(CFG_WITH_ARM_TRUSTED_FW)
+static void init_vfp_nsec(void)
+{
+}
+#else
+static void init_vfp_nsec(void)
+{
+	/* Normal world can use CP10 and CP11 (SIMD/VFP) */
+	write_nsacr(read_nsacr() | NSACR_CP10 | NSACR_CP11);
+}
+#endif
+
+#if defined(CFG_WITH_VFP)
+
+#ifdef ARM32
+static void init_vfp_sec(void)
+{
+	uint32_t cpacr = read_cpacr();
+
+	/*
+	 * Enable Advanced SIMD functionality.
+	 * Enable use of D16-D31 of the Floating-point Extension register
+	 * file.
+	 */
+	cpacr &= ~(CPACR_ASEDIS | CPACR_D32DIS);
+	/*
+	 * Enable usage of CP10 and CP11 (SIMD/VFP) (both kernel and user
+	 * mode.
+	 */
+	cpacr |= CPACR_CP(10, CPACR_CP_ACCESS_FULL);
+	cpacr |= CPACR_CP(11, CPACR_CP_ACCESS_FULL);
+	write_cpacr(cpacr);
+}
+#endif /* ARM32 */
+
+#ifdef ARM64
+static void init_vfp_sec(void)
+{
+	/* Not using VFP until thread_kernel_enable_vfp() */
+	vfp_disable();
+}
+#endif /* ARM64 */
+
+#else /* CFG_WITH_VFP */
+
+static void init_vfp_sec(void)
+{
+	/* Not using VFP */
+}
+#endif
+
+#ifdef CFG_WITH_PAGER
+
+static size_t get_block_size(void)
+{
+	struct core_mmu_table_info tbl_info;
+	unsigned l;
+
+	if (!core_mmu_find_table(CFG_TEE_RAM_START, UINT_MAX, &tbl_info))
+		panic("can't find mmu tables");
+
+	l = tbl_info.level - 1;
+	if (!core_mmu_find_table(CFG_TEE_RAM_START, l, &tbl_info))
+		panic("can't find mmu table upper level");
+
+	return 1 << tbl_info.shift;
+}
+
+static void init_runtime(unsigned long pageable_part)
+{
+	size_t n;
+	size_t init_size = (size_t)__init_size;
+	size_t pageable_size = __pageable_end - __pageable_start;
+	size_t hash_size = (pageable_size / SMALL_PAGE_SIZE) *
+			   TEE_SHA256_HASH_SIZE;
+	tee_mm_entry_t *mm;
+	uint8_t *paged_store;
+	uint8_t *hashes;
+	size_t block_size;
+
+	assert(pageable_size % SMALL_PAGE_SIZE == 0);
+	assert(hash_size == (size_t)__tmp_hashes_size);
+
+	/*
+	 * Zero BSS area. Note that globals that would normally would go
+	 * into BSS which are used before this has to be put into .nozi.*
+	 * to avoid getting overwritten.
+	 */
+	memset(__bss_start, 0, __bss_end - __bss_start);
+
+	/*
+	 * This needs to be initialized early to support address lookup
+	 * in MEM_AREA_TEE_RAM
+	 */
+	if (!core_mmu_find_table(CFG_TEE_RAM_START, UINT_MAX,
+				 &tee_pager_tbl_info))
+		panic("can't find mmu tables");
+
+	if (tee_pager_tbl_info.shift != SMALL_PAGE_SHIFT)
+		panic("Unsupported page size in translation table");
+
+	thread_init_boot_thread();
+
+	malloc_add_pool(__heap1_start, __heap1_end - __heap1_start);
+	malloc_add_pool(__heap2_start, __heap2_end - __heap2_start);
+
+	hashes = malloc(hash_size);
+	IMSG("Pager is enabled. Hashes: %zu bytes", hash_size);
+	assert(hashes);
+	memcpy(hashes, __tmp_hashes_start, hash_size);
+
+	/*
+	 * Need tee_mm_sec_ddr initialized to be able to allocate secure
+	 * DDR below.
+	 */
+	teecore_init_ta_ram();
+
+	mm = tee_mm_alloc(&tee_mm_sec_ddr, pageable_size);
+	assert(mm);
+	paged_store = phys_to_virt(tee_mm_get_smem(mm), MEM_AREA_TA_RAM);
+	/* Copy init part into pageable area */
+	memcpy(paged_store, __init_start, init_size);
+	/* Copy pageable part after init part into pageable area */
+	memcpy(paged_store + init_size,
+	       phys_to_virt(pageable_part,
+			    core_mmu_get_type_by_pa(pageable_part)),
+		__pageable_part_end - __pageable_part_start);
+
+	/* Check that hashes of what's in pageable area is OK */
+	DMSG("Checking hashes of pageable area");
+	for (n = 0; (n * SMALL_PAGE_SIZE) < pageable_size; n++) {
+		const uint8_t *hash = hashes + n * TEE_SHA256_HASH_SIZE;
+		const uint8_t *page = paged_store + n * SMALL_PAGE_SIZE;
+		TEE_Result res;
+
+		DMSG("hash pg_idx %zu hash %p page %p", n, hash, page);
+		res = hash_sha256_check(hash, page, SMALL_PAGE_SIZE);
+		if (res != TEE_SUCCESS) {
+			EMSG("Hash failed for page %zu at %p: res 0x%x",
+			     n, page, res);
+			panic();
+		}
+	}
+
+	/*
+	 * Copy what's not initialized in the last init page. Needed
+	 * because we're not going fault in the init pages again. We can't
+	 * fault in pages until we've switched to the new vector by calling
+	 * thread_init_handlers() below.
+	 */
+	if (init_size % SMALL_PAGE_SIZE) {
+		uint8_t *p;
+
+		memcpy(__init_start + init_size, paged_store + init_size,
+			SMALL_PAGE_SIZE - (init_size % SMALL_PAGE_SIZE));
+
+		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);
+	}
+
+	/*
+	 * Initialize the virtual memory pool used for main_mmu_l2_ttb which
+	 * is supplied to tee_pager_init() below.
+	 */
+	block_size = get_block_size();
+	if (!tee_mm_init(&tee_mm_vcore,
+			ROUNDDOWN(CFG_TEE_RAM_START, block_size),
+			ROUNDUP(CFG_TEE_RAM_START + CFG_TEE_RAM_VA_SIZE,
+				block_size),
+			SMALL_PAGE_SHIFT, 0))
+		panic("tee_mm_vcore init failed");
+
+	/*
+	 * Assign alias area for pager end of the small page block the rest
+	 * of the binary is loaded into. We're taking more than needed, but
+	 * we're guaranteed to not need more than the physical amount of
+	 * TZSRAM.
+	 */
+	mm = tee_mm_alloc2(&tee_mm_vcore,
+		(vaddr_t)tee_mm_vcore.hi - TZSRAM_SIZE, TZSRAM_SIZE);
+	assert(mm);
+	tee_pager_init(mm);
+
+	/*
+	 * Claim virtual memory which isn't paged, note that there migth be
+	 * a gap between tee_mm_vcore.lo and TEE_RAM_START which is also
+	 * claimed to avoid later allocations to get that memory.
+	 * Linear memory (flat map core memory) ends there.
+	 */
+	mm = tee_mm_alloc2(&tee_mm_vcore, tee_mm_vcore.lo,
+			(vaddr_t)(__pageable_start - tee_mm_vcore.lo));
+	assert(mm);
+
+	/*
+	 * Allocate virtual memory for the pageable area and let the pager
+	 * take charge of all the pages already assigned to that memory.
+	 */
+	mm = tee_mm_alloc2(&tee_mm_vcore, (vaddr_t)__pageable_start,
+			   pageable_size);
+	assert(mm);
+	if (!tee_pager_add_core_area(tee_mm_get_smem(mm), tee_mm_get_bytes(mm),
+				     TEE_MATTR_PRX, paged_store, hashes))
+		panic("failed to add pageable to vcore");
+
+	tee_pager_add_pages((vaddr_t)__pageable_start,
+		ROUNDUP(init_size, SMALL_PAGE_SIZE) / SMALL_PAGE_SIZE, false);
+	tee_pager_add_pages((vaddr_t)__pageable_start +
+				ROUNDUP(init_size, SMALL_PAGE_SIZE),
+			(pageable_size - ROUNDUP(init_size, SMALL_PAGE_SIZE)) /
+				SMALL_PAGE_SIZE, true);
+
+}
+#else
+
+#ifdef CFG_CORE_SANITIZE_KADDRESS
+static void init_run_constructors(void)
+{
+	vaddr_t *ctor;
+
+	for (ctor = &__ctor_list; ctor < &__ctor_end; ctor++)
+		((void (*)(void))(*ctor))();
+}
+
+static void init_asan(void)
+{
+
+	/*
+	 * CFG_ASAN_SHADOW_OFFSET is also supplied as
+	 * -fasan-shadow-offset=$(CFG_ASAN_SHADOW_OFFSET) to the compiler.
+	 * Since all the needed values to calculate the value of
+	 * CFG_ASAN_SHADOW_OFFSET isn't available in to make we need to
+	 * calculate it in advance and hard code it into the platform
+	 * conf.mk. Here where we have all the needed values we double
+	 * check that the compiler is supplied the correct value.
+	 */
+
+#define __ASAN_SHADOW_START \
+	ROUNDUP(CFG_TEE_RAM_START + (CFG_TEE_RAM_VA_SIZE * 8) / 9 - 8, 8)
+	assert(__ASAN_SHADOW_START == (vaddr_t)&__asan_shadow_start);
+#define __CFG_ASAN_SHADOW_OFFSET \
+	(__ASAN_SHADOW_START - (CFG_TEE_RAM_START / 8))
+	COMPILE_TIME_ASSERT(CFG_ASAN_SHADOW_OFFSET == __CFG_ASAN_SHADOW_OFFSET);
+#undef __ASAN_SHADOW_START
+#undef __CFG_ASAN_SHADOW_OFFSET
+
+	/*
+	 * Assign area covered by the shadow area, everything from start up
+	 * to the beginning of the shadow area.
+	 */
+	asan_set_shadowed((void *)CFG_TEE_LOAD_ADDR, &__asan_shadow_start);
+
+	/*
+	 * Add access to areas that aren't opened automatically by a
+	 * constructor.
+	 */
+	asan_tag_access(&__initcall_start, &__initcall_end);
+	asan_tag_access(&__ctor_list, &__ctor_end);
+	asan_tag_access(__rodata_start, __rodata_end);
+	asan_tag_access(__early_bss_start, __early_bss_end);
+	asan_tag_access(__nozi_start, __nozi_end);
+
+	init_run_constructors();
+
+	/* Everything is tagged correctly, let's start address sanitizing. */
+	asan_start();
+}
+#else /*CFG_CORE_SANITIZE_KADDRESS*/
+static void init_asan(void)
+{
+}
+#endif /*CFG_CORE_SANITIZE_KADDRESS*/
+
+static void init_runtime(unsigned long pageable_part __unused)
+{
+	/*
+	 * Zero BSS area. Note that globals that would normally would go
+	 * into BSS which are used before this has to be put into .nozi.*
+	 * to avoid getting overwritten.
+	 */
+	memset(__bss_start, 0, __bss_end - __bss_start);
+
+	thread_init_boot_thread();
+
+	init_asan();
+	malloc_add_pool(__heap1_start, __heap1_end - __heap1_start);
+
+	/*
+	 * Initialized at this stage in the pager version of this function
+	 * above
+	 */
+	teecore_init_ta_ram();
+}
+#endif
+
+#ifdef CFG_DT
+static int add_optee_dt_node(void *fdt)
+{
+	int offs;
+	int ret;
+
+	if (fdt_path_offset(fdt, "/firmware/optee") >= 0) {
+		IMSG("OP-TEE Device Tree node already exists!\n");
+		return 0;
+	}
+
+	offs = fdt_path_offset(fdt, "/firmware");
+	if (offs < 0) {
+		offs = fdt_path_offset(fdt, "/");
+		if (offs < 0)
+			return -1;
+		offs = fdt_add_subnode(fdt, offs, "firmware");
+		if (offs < 0)
+			return -1;
+	}
+
+	offs = fdt_add_subnode(fdt, offs, "optee");
+	if (offs < 0)
+		return -1;
+
+	ret = fdt_setprop_string(fdt, offs, "compatible", "linaro,optee-tz");
+	if (ret < 0)
+		return -1;
+	ret = fdt_setprop_string(fdt, offs, "method", "smc");
+	if (ret < 0)
+		return -1;
+	return 0;
+}
+
+static int get_dt_cell_size(void *fdt, int offs, const char *cell_name,
+			    uint32_t *cell_size)
+{
+	int len;
+	const uint32_t *cell = fdt_getprop(fdt, offs, cell_name, &len);
+
+	if (len != sizeof(*cell))
+		return -1;
+	*cell_size = fdt32_to_cpu(*cell);
+	if (*cell_size != 1 && *cell_size != 2)
+		return -1;
+	return 0;
+}
+
+static void set_dt_val(void *data, uint32_t cell_size, uint64_t val)
+{
+	if (cell_size == 1) {
+		uint32_t v = cpu_to_fdt32((uint32_t)val);
+
+		memcpy(data, &v, sizeof(v));
+	} else {
+		uint64_t v = cpu_to_fdt64(val);
+
+		memcpy(data, &v, sizeof(v));
+	}
+}
+
+static int add_optee_res_mem_dt_node(void *fdt)
+{
+	int offs;
+	int ret;
+	uint32_t addr_size = 2;
+	uint32_t len_size = 2;
+	vaddr_t shm_va_start;
+	vaddr_t shm_va_end;
+	paddr_t shm_pa;
+	char subnode_name[80];
+
+	offs = fdt_path_offset(fdt, "/reserved-memory");
+	if (offs >= 0) {
+		ret = get_dt_cell_size(fdt, offs, "#address-cells", &addr_size);
+		if (ret < 0)
+			return -1;
+		ret = get_dt_cell_size(fdt, offs, "#size-cells", &len_size);
+		if (ret < 0)
+			return -1;
+	} else {
+		offs = fdt_path_offset(fdt, "/");
+		if (offs < 0)
+			return -1;
+		offs = fdt_add_subnode(fdt, offs, "reserved-memory");
+		if (offs < 0)
+			return -1;
+		ret = fdt_setprop_cell(fdt, offs, "#address-cells", addr_size);
+		if (ret < 0)
+			return -1;
+		ret = fdt_setprop_cell(fdt, offs, "#size-cells", len_size);
+		if (ret < 0)
+			return -1;
+		ret = fdt_setprop(fdt, offs, "ranges", NULL, 0);
+		if (ret < 0)
+			return -1;
+	}
+
+	core_mmu_get_mem_by_type(MEM_AREA_NSEC_SHM, &shm_va_start, &shm_va_end);
+	shm_pa = virt_to_phys((void *)shm_va_start);
+	snprintf(subnode_name, sizeof(subnode_name),
+		 "optee@0x%" PRIxPA, shm_pa);
+	offs = fdt_add_subnode(fdt, offs, subnode_name);
+	if (offs >= 0) {
+		uint32_t data[addr_size + len_size] ;
+
+		set_dt_val(data, addr_size, shm_pa);
+		set_dt_val(data + addr_size, len_size,
+			   shm_va_end - shm_va_start);
+		ret = fdt_setprop(fdt, offs, "reg", data, sizeof(data));
+		if (ret < 0)
+			return -1;
+		ret = fdt_setprop(fdt, offs, "no-map", NULL, 0);
+		if (ret < 0)
+			return -1;
+	} else {
+		return -1;
+	}
+	return 0;
+}
+
+static void init_fdt(unsigned long phys_fdt)
+{
+	void *fdt;
+	int ret;
+
+	if (!phys_fdt) {
+		EMSG("Device Tree missing");
+		/*
+		 * No need to panic as we're not using the DT in OP-TEE
+		 * yet, we're only adding some nodes for normal world use.
+		 * This makes the switch to using DT easier as we can boot
+		 * a newer OP-TEE with older boot loaders. Once we start to
+		 * initialize devices based on DT we'll likely panic
+		 * instead of returning here.
+		 */
+		return;
+	}
+
+	if (!core_mmu_add_mapping(MEM_AREA_IO_NSEC, phys_fdt, CFG_DTB_MAX_SIZE))
+		panic("failed to map fdt");
+
+	fdt = phys_to_virt(phys_fdt, MEM_AREA_IO_NSEC);
+	if (!fdt)
+		panic();
+
+	ret = fdt_open_into(fdt, fdt, CFG_DTB_MAX_SIZE);
+	if (ret < 0) {
+		EMSG("Invalid Device Tree at 0x%" PRIxPA ": error %d",
+		     phys_fdt, ret);
+		panic();
+	}
+
+	if (add_optee_dt_node(fdt))
+		panic("Failed to add OP-TEE Device Tree node");
+
+	if (add_optee_res_mem_dt_node(fdt))
+		panic("Failed to add OP-TEE reserved memory DT node");
+
+	ret = fdt_pack(fdt);
+	if (ret < 0) {
+		EMSG("Failed to pack Device Tree at 0x%" PRIxPA ": error %d",
+		     phys_fdt, ret);
+		panic();
+	}
+}
+#else
+static void init_fdt(unsigned long phys_fdt __unused)
+{
+}
+#endif /*!CFG_DT*/
+
+static void init_primary_helper(unsigned long pageable_part,
+				unsigned long nsec_entry, unsigned long fdt)
+{
+	/*
+	 * 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.
+	 */
+	thread_set_exceptions(THREAD_EXCP_ALL);
+	init_vfp_sec();
+
+	init_runtime(pageable_part);
+
+	IMSG("Initializing (%s)\n", core_v_str);
+
+	thread_init_primary(generic_boot_get_handlers());
+	thread_init_per_cpu();
+	init_sec_mon(nsec_entry);
+	init_fdt(fdt);
+	main_init_gic();
+	init_vfp_nsec();
+
+	if (init_teecore() != TEE_SUCCESS)
+		panic();
+	DMSG("Primary CPU switching to normal world boot\n");
+}
+
+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.
+	 */
+	thread_set_exceptions(THREAD_EXCP_ALL);
+
+	thread_init_per_cpu();
+	init_sec_mon(nsec_entry);
+	main_secondary_init_gic();
+	init_vfp_sec();
+	init_vfp_nsec();
+
+	DMSG("Secondary CPU Switching to normal world boot\n");
+}
+
+#if defined(CFG_WITH_ARM_TRUSTED_FW)
+struct thread_vector_table *
+generic_boot_init_primary(unsigned long pageable_part, unsigned long u __unused,
+			  unsigned long fdt)
+{
+	init_primary_helper(pageable_part, PADDR_INVALID, fdt);
+	return &thread_vector_table;
+}
+
+unsigned long generic_boot_cpu_on_handler(unsigned long a0 __maybe_unused,
+				     unsigned long a1 __unused)
+{
+	DMSG("cpu %zu: a0 0x%lx", get_core_pos(), a0);
+	init_secondary_helper(PADDR_INVALID);
+	return 0;
+}
+#else
+void generic_boot_init_primary(unsigned long pageable_part,
+			       unsigned long nsec_entry, unsigned long fdt)
+{
+	init_primary_helper(pageable_part, nsec_entry, fdt);
+}
+
+void generic_boot_init_secondary(unsigned long nsec_entry)
+{
+	init_secondary_helper(nsec_entry);
+}
+#endif
+
+#if defined(CFG_BOOT_SECONDARY_REQUEST)
+int generic_boot_core_release(size_t core_idx, paddr_t entry)
+{
+	if (!core_idx || core_idx >= CFG_TEE_CORE_NB_CORE)
+		return -1;
+
+	ns_entry_addrs[core_idx] = entry;
+	dmb();
+	spin_table[core_idx] = 1;
+	dsb();
+	sev();
+
+	return 0;
+}
+
+/*
+ * spin until secondary boot request, then returns with
+ * the secondary core entry address.
+ */
+paddr_t generic_boot_core_hpen(void)
+{
+#ifdef CFG_PSCI_ARM32
+	return ns_entry_addrs[get_core_pos()];
+#else
+	do {
+		wfe();
+	} while (!spin_table[get_core_pos()]);
+	dmb();
+	return ns_entry_addrs[get_core_pos()];
+#endif
+}
+#endif
diff --git a/core/arch/arm/kernel/generic_entry_a32.S b/core/arch/arm/kernel/generic_entry_a32.S
new file mode 100644
index 0000000..27717d5
--- /dev/null
+++ b/core/arch/arm/kernel/generic_entry_a32.S
@@ -0,0 +1,503 @@
+/*
+ * Copyright (c) 2014, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <platform_config.h>
+
+#include <asm.S>
+#include <arm.h>
+#include <arm32_macros.S>
+#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
+
+#ifdef CFG_BOOT_SYNC_CPU
+.equ SEM_CPU_READY, 1
+#endif
+
+#ifdef CFG_PL310
+.section .rodata.init
+panic_boot_file:
+	.asciz __FILE__
+
+/*
+ * void assert_flat_mapped_range(uint32_t vaddr, uint32_t line)
+ */
+.section .text.init
+LOCAL_FUNC __assert_flat_mapped_range , :
+UNWIND(	.fnstart)
+UNWIND(	.cantunwind)
+	push	{ r4-r6, lr }
+	mov	r4, r0
+	mov	r5, r1
+	bl	cpu_mmu_enabled
+	cmp	r0, #0
+	beq	1f
+	mov	r0, r4
+	bl	virt_to_phys
+	cmp	r0, r4
+	beq	1f
+	/*
+	 * this must be compliant with the panic generic routine:
+	 * __do_panic(__FILE__, __LINE__, __func__, str)
+	 */
+	ldr	r0, =panic_boot_file
+	mov	r1, r5
+	mov	r2, #0
+	mov	r3, #0
+	bl	__do_panic
+	b	.		/* should NOT return */
+1:	pop	{ r4-r6, pc }
+UNWIND(	.fnend)
+END_FUNC __assert_flat_mapped_range
+
+	/* panic if mmu is enable and vaddr != paddr (scratch lr) */
+	.macro assert_flat_mapped_range va, line
+		ldr	r0, =(\va)
+		ldr	r1, =\line
+		bl	__assert_flat_mapped_range
+	.endm
+#endif /* CFG_PL310 */
+
+.section .text.boot
+FUNC _start , :
+	b	reset
+	b	.	/* Undef */
+	b	.	/* Syscall */
+	b	.	/* Prefetch abort */
+	b	.	/* Data abort */
+	b	.	/* Reserved */
+	b	.	/* IRQ */
+	b	.	/* FIQ */
+END_FUNC _start
+
+	.macro cpu_is_ready
+#ifdef CFG_BOOT_SYNC_CPU
+	bl	get_core_pos
+	lsl	r0, r0, #2
+	ldr	r1,=sem_cpu_sync
+	ldr	r2, =SEM_CPU_READY
+	str	r2, [r1, r0]
+	dsb
+	sev
+#endif
+	.endm
+
+	.macro wait_primary
+#ifdef CFG_BOOT_SYNC_CPU
+	ldr	r0, =sem_cpu_sync
+	mov	r2, #SEM_CPU_READY
+	sev
+1:
+	ldr	r1, [r0]
+	cmp	r1, r2
+	wfene
+	bne	1b
+#endif
+	.endm
+
+	.macro wait_secondary
+#ifdef CFG_BOOT_SYNC_CPU
+	ldr	r0, =sem_cpu_sync
+	mov	r3, #CFG_TEE_CORE_NB_CORE
+	mov	r2, #SEM_CPU_READY
+	sev
+1:
+	subs	r3, r3, #1
+	beq	3f
+	add	r0, r0, #4
+2:
+	ldr	r1, [r0]
+	cmp	r1, r2
+	wfene
+	bne	2b
+	b	1b
+3:
+#endif
+	.endm
+
+	/*
+	 * Save boot arguments
+	 * entry r0, saved r4: pagestore
+	 * entry r1, saved r7: (ARMv7 standard bootarg #1)
+	 * entry r2, saved r6: device tree address, (ARMv7 standard bootarg #2)
+	 * entry lr, saved r5: non-secure entry address (ARMv7 bootarg #0)
+	 */
+	.macro bootargs_entry
+#if defined(CFG_NS_ENTRY_ADDR)
+	ldr	r5, =CFG_NS_ENTRY_ADDR
+#else
+	mov	r5, lr
+#endif
+#if defined(CFG_PAGEABLE_ADDR)
+	ldr	r4, =CFG_PAGEABLE_ADDR
+#else
+	mov	r4, r0
+#endif
+#if defined(CFG_DT_ADDR)
+	ldr	r6, =CFG_DT_ADDR
+#else
+	mov	r6, r2
+#endif
+	mov	r7, r1
+	.endm
+
+LOCAL_FUNC reset , :
+UNWIND(	.fnstart)
+UNWIND(	.cantunwind)
+
+	bootargs_entry
+
+	/* Enable alignment checks and disable data and instruction cache. */
+	read_sctlr r0
+	orr	r0, r0, #SCTLR_A
+	bic	r0, r0, #SCTLR_C
+	bic	r0, r0, #SCTLR_I
+	write_sctlr r0
+	isb
+
+	/* Early ARM secure MP specific configuration */
+	bl	plat_cpu_reset_early
+
+	ldr	r0, =_start
+	write_vbar r0
+
+#if defined(CFG_WITH_ARM_TRUSTED_FW)
+	b	reset_primary
+#else
+	bl	get_core_pos
+	cmp	r0, #0
+	beq	reset_primary
+	b	reset_secondary
+#endif
+UNWIND(	.fnend)
+END_FUNC reset
+
+	/*
+	 * Setup sp to point to the top of the tmp stack for the current CPU:
+	 * sp is assigned stack_tmp + (cpu_id + 1) * stack_tmp_stride -
+	 * stack_tmp_offset
+	 */
+	.macro set_sp
+		bl	get_core_pos
+		cmp	r0, #CFG_TEE_CORE_NB_CORE
+		/* Unsupported CPU, park it before it breaks something */
+		bge	unhandled_cpu
+		add	r0, r0, #1
+		ldr	r2, =stack_tmp_stride
+		ldr	r1, [r2]
+		mul	r2, r0, r1
+		ldr	r1, =stack_tmp
+		add	r1, r1, r2
+		ldr	r2, =stack_tmp_offset
+		ldr	r2, [r2]
+		sub	sp, r1, r2
+	.endm
+
+	/*
+	 * Cache maintenance during entry: handle outer cache.
+	 * End address is exclusive: first byte not to be changed.
+	 * Note however arm_clX_inv/cleanbyva operate on full cache lines.
+	 *
+	 * Use ANSI #define to trap source file line number for PL310 assertion
+	 */
+	.macro __inval_cache_vrange vbase, vend, line
+#ifdef CFG_PL310
+		assert_flat_mapped_range (\vbase), (\line)
+		bl	pl310_base
+		ldr	r1, =(\vbase)
+		ldr	r2, =(\vend)
+		bl	arm_cl2_invbypa
+#endif
+		ldr	r0, =(\vbase)
+		ldr	r1, =(\vend)
+		bl	arm_cl1_d_invbyva
+	.endm
+
+	.macro __flush_cache_vrange vbase, vend, line
+#ifdef CFG_PL310
+		assert_flat_mapped_range (\vbase), (\line)
+		ldr	r0, =(\vbase)
+		ldr	r1, =(\vend)
+		bl	arm_cl1_d_cleanbyva
+		bl	pl310_base
+		ldr	r1, =(\vbase)
+		ldr	r2, =(\vend)
+		bl	arm_cl2_cleaninvbypa
+#endif
+		ldr	r0, =(\vbase)
+		ldr	r1, =(\vend)
+		bl	arm_cl1_d_cleaninvbyva
+	.endm
+
+#define inval_cache_vrange(vbase, vend) \
+		__inval_cache_vrange (vbase), ((vend) - 1), __LINE__
+
+#define flush_cache_vrange(vbase, vend) \
+		__flush_cache_vrange (vbase), ((vend) - 1), __LINE__
+
+#ifdef CFG_BOOT_SYNC_CPU
+#define flush_cpu_semaphores \
+		flush_cache_vrange(sem_cpu_sync, \
+				   (sem_cpu_sync + (CFG_TEE_CORE_NB_CORE << 2)))
+#else
+#define flush_cpu_semaphores
+#endif
+
+LOCAL_FUNC reset_primary , :
+UNWIND(	.fnstart)
+UNWIND(	.cantunwind)
+
+	/* preserve r4-r7: bootargs */
+
+#ifdef CFG_WITH_PAGER
+	/*
+	 * Move init code into correct location and move hashes to a
+	 * temporary safe location until the heap is initialized.
+	 *
+	 * The binary is built as:
+	 * [Pager code, rodata and data] : In correct location
+	 * [Init code and rodata] : Should be copied to __text_init_start
+	 * [Hashes] : Should be saved before initializing pager
+	 *
+	 */
+	ldr	r0, =__text_init_start	/* dst */
+	ldr	r1, =__data_end 	/* src */
+	ldr	r2, =__tmp_hashes_end	/* dst limit */
+	/* Copy backwards (as memmove) in case we're overlapping */
+	sub	r2, r2, r0		/* len */
+	add	r0, r0, r2
+	add	r1, r1, r2
+	ldr	r2, =__text_init_start
+copy_init:
+	ldmdb	r1!, {r3, r8-r12, sp}
+	stmdb	r0!, {r3, r8-r12, sp}
+	cmp	r0, r2
+	bgt	copy_init
+#endif
+
+
+#ifdef CFG_CORE_SANITIZE_KADDRESS
+	/* First initialize the entire shadow area with no access */
+	ldr	r0, =__asan_shadow_start	/* start */
+	ldr	r1, =__asan_shadow_end	/* limit */
+	mov	r2, #ASAN_DATA_RED_ZONE
+shadow_no_access:
+	str	r2, [r0], #4
+	cmp	r0, r1
+	bls	shadow_no_access
+
+	/* Mark the entire stack area as OK */
+	ldr	r2, =CFG_ASAN_SHADOW_OFFSET
+	ldr	r0, =__nozi_stack_start	/* start */
+	lsr	r0, r0, #ASAN_BLOCK_SHIFT
+	add	r0, r0, r2
+	ldr	r1, =__nozi_stack_end	/* limit */
+	lsr	r1, r1, #ASAN_BLOCK_SHIFT
+	add	r1, r1, r2
+	mov	r2, #0
+shadow_stack_access_ok:
+	strb	r2, [r0], #1
+	cmp	r0, r1
+	bls	shadow_stack_access_ok
+#endif
+
+	set_sp
+
+	/* complete ARM secure MP common configuration */
+	bl	plat_cpu_reset_late
+
+	/* Enable Console */
+	bl	console_init
+
+#ifdef CFG_PL310
+	bl	pl310_base
+	bl	arm_cl2_config
+#endif
+
+	/*
+	 * Invalidate dcache for all memory used during initialization to
+	 * avoid nasty surprices when the cache is turned on. We must not
+	 * invalidate memory not used by OP-TEE since we may invalidate
+	 * entries used by for instance ARM Trusted Firmware.
+	 */
+#ifdef CFG_WITH_PAGER
+	inval_cache_vrange(__text_start, __tmp_hashes_end)
+#else
+	inval_cache_vrange(__text_start, __end)
+#endif
+
+#ifdef CFG_PL310
+	/* Enable PL310 if not yet enabled */
+	bl	pl310_base
+	bl	arm_cl2_enable
+#endif
+
+	bl	core_init_mmu_map
+	bl	core_init_mmu_regs
+	bl	cpu_mmu_enable
+	bl	cpu_mmu_enable_icache
+	bl	cpu_mmu_enable_dcache
+
+	mov	r0, r4		/* pageable part address */
+	mov	r1, r5		/* ns-entry address */
+	mov	r2, r6		/* DT address */
+	bl	generic_boot_init_primary
+	mov	r4, r0		/* save entry test vector */
+
+	/*
+	 * In case we've touched memory that secondary CPUs will use before
+	 * they have turned on their D-cache, clean and invalidate the
+	 * D-cache before exiting to normal world.
+	 */
+#ifdef CFG_WITH_PAGER
+	flush_cache_vrange(__text_start, __init_end)
+#else
+	flush_cache_vrange(__text_start, __end)
+#endif
+
+	/* release secondary boot cores and sync with them */
+	cpu_is_ready
+	flush_cpu_semaphores
+	wait_secondary
+
+#ifdef CFG_PL310_LOCKED
+	/* lock/invalidate all lines: pl310 behaves as if disable */
+	bl	pl310_base
+	bl	arm_cl2_lockallways
+	bl	pl310_base
+	bl	arm_cl2_cleaninvbyway
+#endif
+
+	/*
+	 * Clear current thread id now to allow the thread to be reused on
+	 * next entry. Matches the thread_init_boot_thread() in
+	 * generic_boot.c.
+	 */
+	bl 	thread_clr_boot_thread
+
+#if defined(CFG_WITH_ARM_TRUSTED_FW)
+	/* Pass the vector address returned from main_init */
+	mov	r1, r4
+#else
+	/* realy standard bootarg #1 and #2 to non secure entry */
+	mov	r4, #0
+	mov	r3, r6		/* std bootarg #2 for register R2 */
+	mov	r2, r7		/* std bootarg #1 for register R1 */
+	mov	r1, #0
+#endif /* CFG_WITH_ARM_TRUSTED_FW */
+
+	mov	r0, #TEESMC_OPTEED_RETURN_ENTRY_DONE
+	smc	#0
+	b	.	/* SMC should not return */
+UNWIND(	.fnend)
+END_FUNC reset_primary
+
+
+LOCAL_FUNC unhandled_cpu , :
+UNWIND(	.fnstart)
+	wfi
+	b	unhandled_cpu
+UNWIND(	.fnend)
+END_FUNC unhandled_cpu
+
+#if defined(CFG_WITH_ARM_TRUSTED_FW)
+FUNC cpu_on_handler , :
+UNWIND(	.fnstart)
+UNWIND(	.cantunwind)
+	mov	r4, r0
+	mov	r5, r1
+	mov	r6, lr
+	read_sctlr r0
+	orr	r0, r0, #SCTLR_A
+	write_sctlr r0
+
+	ldr	r0, =_start
+	write_vbar r0
+
+	mov	r4, lr
+	set_sp
+
+	bl	core_init_mmu_regs
+	bl	cpu_mmu_enable
+	bl	cpu_mmu_enable_icache
+	bl	cpu_mmu_enable_dcache
+
+	mov	r0, r4
+	mov	r1, r5
+	bl	generic_boot_cpu_on_handler
+
+	bx	r6
+UNWIND(	.fnend)
+END_FUNC cpu_on_handler
+
+#else /* defined(CFG_WITH_ARM_TRUSTED_FW) */
+
+LOCAL_FUNC reset_secondary , :
+UNWIND(	.fnstart)
+UNWIND(	.cantunwind)
+
+	wait_primary
+
+	set_sp
+
+	bl	plat_cpu_reset_late
+
+#if defined (CFG_BOOT_SECONDARY_REQUEST)
+	/* if L1 is not invalidated before, do it here */
+	bl	arm_cl1_d_invbysetway
+#endif
+
+	bl	core_init_mmu_regs
+	bl	cpu_mmu_enable
+	bl	cpu_mmu_enable_icache
+	bl	cpu_mmu_enable_dcache
+
+	cpu_is_ready
+
+#if defined (CFG_BOOT_SECONDARY_REQUEST)
+	/* generic_boot_core_hpen return value (r0) is ns entry point */
+	bl	generic_boot_core_hpen
+#else
+	mov	r0, r5		/* ns-entry address */
+#endif
+	bl	generic_boot_init_secondary
+
+	mov	r0, #TEESMC_OPTEED_RETURN_ENTRY_DONE
+	mov	r1, #0
+	mov	r2, #0
+	mov	r3, #0
+	mov	r4, #0
+	smc	#0
+	b	.	/* SMC should not return */
+UNWIND(	.fnend)
+END_FUNC reset_secondary
+#endif /* defined(CFG_WITH_ARM_TRUSTED_FW) */
diff --git a/core/arch/arm/kernel/generic_entry_a64.S b/core/arch/arm/kernel/generic_entry_a64.S
new file mode 100644
index 0000000..5a5dd53
--- /dev/null
+++ b/core/arch/arm/kernel/generic_entry_a64.S
@@ -0,0 +1,315 @@
+/*
+ * 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 <platform_config.h>
+
+#include <asm.S>
+#include <arm.h>
+#include <sm/optee_smc.h>
+#include <sm/teesmc_opteed_macros.h>
+#include <sm/teesmc_opteed.h>
+
+	/*
+	 * Setup SP_EL0 and SPEL1, SP will be set to SP_EL0.
+	 * SP_EL0 is assigned stack_tmp + (cpu_id + 1) * stack_tmp_stride -
+	 *		      stack_tmp_offset
+	 * SP_EL1 is assigned thread_core_local[cpu_id]
+	 */
+	.macro set_sp
+		bl	get_core_pos
+		cmp	x0, #CFG_TEE_CORE_NB_CORE
+		/* Unsupported CPU, park it before it breaks something */
+		bge	unhandled_cpu
+		add	x0, x0, #1
+		adr	x2, stack_tmp_stride
+		ldr	w1, [x2]
+		mul	x2, x0, x1
+		adrp	x1, stack_tmp
+		add	x1, x1, :lo12:stack_tmp
+		add	x1, x1, x2
+		adr	x2, stack_tmp_offset
+		ldr	w2, [x2]
+		sub	x1, x1, x2
+		msr	spsel, #0
+		mov	sp, x1
+		bl	thread_get_core_local
+		msr	spsel, #1
+		mov	sp, x0
+		msr	spsel, #0
+	.endm
+
+.section .text.boot
+FUNC _start , :
+	mov	x19, x0		/* Save pagable part address */
+	mov	x20, x2		/* Save DT address */
+
+	adr	x0, reset_vect_table
+	msr	vbar_el1, x0
+	isb
+
+	mrs	x0, sctlr_el1
+	mov	x1, #(SCTLR_I | SCTLR_A | SCTLR_SA)
+	orr	x0, x0, x1
+	msr	sctlr_el1, x0
+	isb
+
+#ifdef CFG_WITH_PAGER
+	/*
+	 * Move init code into correct location
+	 *
+	 * The binary is built as:
+	 * [Pager code, rodata and data] : In correct location
+	 * [Init code and rodata] : Should be copied to __text_init_start
+	 * [Hashes] : Should be saved before clearing bss
+	 *
+	 * When we copy init code and rodata into correct location we don't
+	 * need to worry about hashes being overwritten as size of .bss,
+	 * .heap, .nozi and .heap3 is much larger than the size of init
+	 * code and rodata and hashes.
+	 */
+	adr	x0, __text_init_start	/* dst */
+	adr	x1, __data_end 	/* src */
+	adr	x2, __rodata_init_end	/* dst limit */
+copy_init:
+	ldp	x3, x4, [x1], #16
+	stp	x3, x4, [x0], #16
+	cmp	x0, x2
+	b.lt	copy_init
+#endif
+
+	/* Setup SP_EL0 and SP_EL1, SP will be set to SP_EL0 */
+	set_sp
+
+	/* Enable aborts now that we can receive exceptions */
+	msr	daifclr, #DAIFBIT_ABT
+
+	adr	x0, __text_start
+#ifdef CFG_WITH_PAGER
+	adrp	x1, __init_end
+	add	x1, x1, :lo12:__init_end
+#else
+	adrp	x1, __end
+	add	x1, x1, :lo12:__end
+#endif
+	sub	x1, x1, x0
+	bl	inv_dcache_range
+
+	/* Enable Console */
+	bl	console_init
+
+	bl	core_init_mmu_map
+	bl	core_init_mmu_regs
+	bl	cpu_mmu_enable
+	bl	cpu_mmu_enable_icache
+	bl	cpu_mmu_enable_dcache
+
+	mov	x0, x19		/* pagable part address */
+	mov	x1, #-1
+	mov	x2, x20		/* DT address */
+	bl	generic_boot_init_primary
+
+	/*
+	 * In case we've touched memory that secondary CPUs will use before
+	 * they have turned on their D-cache, clean and invalidate the
+	 * D-cache before exiting to normal world.
+	 */
+	mov	x19, x0
+	adr	x0, __text_start
+#ifdef CFG_WITH_PAGER
+	adrp	x1, __init_end
+	add	x1, x1, :lo12:__init_end
+#else
+	adrp	x1, __end
+	add	x1, x1, :lo12:__end
+#endif
+	sub	x1, x1, x0
+	bl	flush_dcache_range
+
+
+	/*
+	 * Clear current thread id now to allow the thread to be reused on
+	 * next entry. Matches the thread_init_boot_thread in
+	 * generic_boot.c.
+	 */
+	bl 	thread_clr_boot_thread
+
+	/* Pass the vector address returned from main_init */
+	mov	x1, x19
+	mov	x0, #TEESMC_OPTEED_RETURN_ENTRY_DONE
+	smc	#0
+	b	.	/* SMC should not return */
+END_FUNC _start
+
+
+.section .text.cpu_on_handler
+FUNC cpu_on_handler , :
+	mov	x19, x0
+	mov	x20, x1
+	mov	x21, x30
+
+	adr	x0, reset_vect_table
+	msr	vbar_el1, x0
+	isb
+
+	mrs	x0, sctlr_el1
+	mov	x1, #(SCTLR_I | SCTLR_A | SCTLR_SA)
+	orr	x0, x0, x1
+	msr	sctlr_el1, x0
+	isb
+
+	/* Setup SP_EL0 and SP_EL1, SP will be set to SP_EL0 */
+	set_sp
+
+	/* Enable aborts now that we can receive exceptions */
+	msr	daifclr, #DAIFBIT_ABT
+
+	bl	core_init_mmu_regs
+	bl	cpu_mmu_enable
+	bl	cpu_mmu_enable_icache
+	bl	cpu_mmu_enable_dcache
+
+	mov	x0, x19
+	mov	x1, x20
+	mov	x30, x21
+	b	generic_boot_cpu_on_handler
+END_FUNC cpu_on_handler
+
+LOCAL_FUNC unhandled_cpu , :
+	wfi
+	b	unhandled_cpu
+END_FUNC unhandled_cpu
+
+	/*
+	 * This macro verifies that the a given vector doesn't exceed the
+	 * architectural limit of 32 instructions. This is meant to be placed
+	 * immedately after the last instruction in the vector. It takes the
+	 * vector entry as the parameter
+	 */
+	.macro check_vector_size since
+	  .if (. - \since) > (32 * 4)
+	    .error "Vector exceeds 32 instructions"
+	  .endif
+	.endm
+
+	.align	11
+LOCAL_FUNC reset_vect_table , :
+	/* -----------------------------------------------------
+	 * Current EL with SP0 : 0x0 - 0x180
+	 * -----------------------------------------------------
+	 */
+SynchronousExceptionSP0:
+	b	SynchronousExceptionSP0
+	check_vector_size SynchronousExceptionSP0
+
+	.align	7
+IrqSP0:
+	b	IrqSP0
+	check_vector_size IrqSP0
+
+	.align	7
+FiqSP0:
+	b	FiqSP0
+	check_vector_size FiqSP0
+
+	.align	7
+SErrorSP0:
+	b	SErrorSP0
+	check_vector_size SErrorSP0
+
+	/* -----------------------------------------------------
+	 * Current EL with SPx: 0x200 - 0x380
+	 * -----------------------------------------------------
+	 */
+	.align	7
+SynchronousExceptionSPx:
+	b	SynchronousExceptionSPx
+	check_vector_size SynchronousExceptionSPx
+
+	.align	7
+IrqSPx:
+	b	IrqSPx
+	check_vector_size IrqSPx
+
+	.align	7
+FiqSPx:
+	b	FiqSPx
+	check_vector_size FiqSPx
+
+	.align	7
+SErrorSPx:
+	b	SErrorSPx
+	check_vector_size SErrorSPx
+
+	/* -----------------------------------------------------
+	 * Lower EL using AArch64 : 0x400 - 0x580
+	 * -----------------------------------------------------
+	 */
+	.align	7
+SynchronousExceptionA64:
+	b	SynchronousExceptionA64
+	check_vector_size SynchronousExceptionA64
+
+	.align	7
+IrqA64:
+	b	IrqA64
+	check_vector_size IrqA64
+
+	.align	7
+FiqA64:
+	b	FiqA64
+	check_vector_size FiqA64
+
+	.align	7
+SErrorA64:
+	b   	SErrorA64
+	check_vector_size SErrorA64
+
+	/* -----------------------------------------------------
+	 * Lower EL using AArch32 : 0x0 - 0x180
+	 * -----------------------------------------------------
+	 */
+	.align	7
+SynchronousExceptionA32:
+	b	SynchronousExceptionA32
+	check_vector_size SynchronousExceptionA32
+
+	.align	7
+IrqA32:
+	b	IrqA32
+	check_vector_size IrqA32
+
+	.align	7
+FiqA32:
+	b	FiqA32
+	check_vector_size FiqA32
+
+	.align	7
+SErrorA32:
+	b	SErrorA32
+	check_vector_size SErrorA32
+
+END_FUNC reset_vect_table
diff --git a/core/arch/arm/kernel/kern.ld.S b/core/arch/arm/kernel/kern.ld.S
new file mode 100644
index 0000000..10dac6e
--- /dev/null
+++ b/core/arch/arm/kernel/kern.ld.S
@@ -0,0 +1,340 @@
+/*
+ * Copyright (c) 2014, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/*
+ * Copyright (c) 2008-2010 Travis Geiselbrecht
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining
+ * a copy of this software and associated documentation files
+ * (the "Software"), to deal in the Software without restriction,
+ * including without limitation the rights to use, copy, modify, merge,
+ * publish, distribute, sublicense, and/or sell copies of the Software,
+ * and to permit persons to whom the Software is furnished to do so,
+ * subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
+ * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
+ * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
+ * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
+ * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ */
+
+#include <platform_config.h>
+
+OUTPUT_FORMAT(CFG_KERN_LINKER_FORMAT)
+OUTPUT_ARCH(CFG_KERN_LINKER_ARCH)
+
+ENTRY(_start)
+SECTIONS
+{
+	. = CFG_TEE_LOAD_ADDR;
+
+	/* text/read-only data */
+	.text : {
+		__text_start = .;
+		KEEP(*(.text.boot.vectab1))
+		KEEP(*(.text.boot.vectab2))
+		KEEP(*(.text.boot))
+
+		. = ALIGN(8);
+		__initcall_start = .;
+		KEEP(*(.initcall1))
+		KEEP(*(.initcall2))
+		KEEP(*(.initcall3))
+		KEEP(*(.initcall4))
+		__initcall_end = .;
+
+#ifdef CFG_WITH_PAGER
+		*(.text)
+/* Include list of sections needed for paging */
+#include <text_unpaged.ld.S>
+#else
+		*(.text .text.*)
+#endif
+		*(.sram.text.glue_7* .gnu.linkonce.t.*)
+		. = ALIGN(8);
+		__text_end = .;
+	}
+
+	.rodata : ALIGN(8) {
+		__rodata_start = .;
+		*(.gnu.linkonce.r.*)
+#ifdef CFG_WITH_PAGER
+		*(.rodata .rodata.__unpaged)
+#include <rodata_unpaged.ld.S>
+#else
+		*(.rodata .rodata.*)
+
+		/*
+		 * 8 to avoid unwanted padding between __start_ta_head_section
+		 * and the first structure in ta_head_section, in 64-bit
+		 * builds
+		 */
+		. = ALIGN(8);
+		__start_ta_head_section = . ;
+		KEEP(*(ta_head_section))
+		__stop_ta_head_section = . ;
+		. = ALIGN(8);
+		__start_phys_mem_map_section = . ;
+		KEEP(*(phys_mem_map_section))
+		__end_phys_mem_map_section = . ;
+#endif
+		. = ALIGN(8);
+		__rodata_end = .;
+	}
+
+	.interp : { *(.interp) }
+	.hash : { *(.hash) }
+	.dynsym : { *(.dynsym) }
+	.dynstr : { *(.dynstr) }
+	.rel.text : { *(.rel.text) *(.rel.gnu.linkonce.t*) }
+	.rela.text : { *(.rela.text) *(.rela.gnu.linkonce.t*) }
+	.rel.data : { *(.rel.data) *(.rel.gnu.linkonce.d*) }
+	.rela.data : { *(.rela.data) *(.rela.gnu.linkonce.d*) }
+	.rel.rodata : { *(.rel.rodata) *(.rel.gnu.linkonce.r*) }
+	.rela.rodata : { *(.rela.rodata) *(.rela.gnu.linkonce.r*) }
+	.rel.got : { *(.rel.got) }
+	.rela.got : { *(.rela.got) }
+	.rel.ctors : { *(.rel.ctors) }
+	.rela.ctors : { *(.rela.ctors) }
+	.rel.dtors : { *(.rel.dtors) }
+	.rela.dtors : { *(.rela.dtors) }
+	.rel.init : { *(.rel.init) }
+	.rela.init : { *(.rela.init) }
+	.rel.fini : { *(.rel.fini) }
+	.rela.fini : { *(.rela.fini) }
+	.rel.bss : { *(.rel.bss) }
+	.rela.bss : { *(.rela.bss) }
+	.rel.plt : { *(.rel.plt) }
+	.rela.plt : { *(.rela.plt) }
+	.init : { *(.init) } =0x9090
+	.plt : { *(.plt) }
+
+	/* .ARM.exidx is sorted, so has to go in its own output section.  */
+	.ARM.exidx : {
+		__exidx_start = .;
+		*(.ARM.exidx* .gnu.linkonce.armexidx.*)
+		__exidx_end = .;
+	}
+
+	.ARM.extab : {
+		__extab_start = .;
+		*(.ARM.extab*)
+		__extab_end = .;
+	}
+
+	.data : ALIGN(8) {
+		/* writable data  */
+		__data_start_rom = .;
+		/* in one segment binaries, the rom data address is on top
+		   of the ram data address */
+		__early_bss_start = .;
+		*(.early_bss .early_bss.*)
+		. = ALIGN(8);
+		__early_bss_end = .;
+		__data_start = .;
+		*(.data .data.* .gnu.linkonce.d.*)
+		. = ALIGN(8);
+	}
+
+	.ctors : ALIGN(8) {
+		__ctor_list = .;
+		KEEP(*(.ctors .ctors.* .init_array .init_array.*))
+		__ctor_end = .;
+	}
+	.dtors : ALIGN(8) {
+		__dtor_list = .;
+		KEEP(*(.dtors .dtors.* .fini_array .fini_array.*))
+		__dtor_end = .;
+	}
+	.got : { *(.got.plt) *(.got) }
+	.dynamic : { *(.dynamic) }
+
+	__data_end = .;
+	/* unintialized data */
+	.bss : ALIGN(8) {
+		__bss_start = .;
+		*(.bss .bss.*)
+		*(.gnu.linkonce.b.*)
+		*(COMMON)
+		. = ALIGN(8);
+		__bss_end = .;
+	}
+
+	.heap1 (NOLOAD) : {
+		/*
+		 * We're keeping track of the padding added before the
+		 * .nozi section so we can do something useful with
+		 * this otherwise wasted memory.
+		 */
+		__heap1_start = .;
+#ifndef CFG_WITH_PAGER
+		. += CFG_CORE_HEAP_SIZE;
+#endif
+		. = ALIGN(16 * 1024);
+		__heap1_end = .;
+	}
+
+	/*
+	 * Uninitialized data that shouldn't be zero initialized at
+	 * runtime.
+	 *
+	 * L1 mmu table requires 16 KiB alignment
+	 */
+	.nozi (NOLOAD) : ALIGN(16 * 1024) {
+		__nozi_start = .;
+		KEEP(*(.nozi .nozi.*))
+		. = ALIGN(16);
+		__nozi_end = .;
+		__nozi_stack_start = .;
+		KEEP(*(.nozi_stack))
+		. = ALIGN(8);
+		__nozi_stack_end = .;
+	}
+
+#ifdef CFG_WITH_PAGER
+	.heap2 (NOLOAD) : {
+		__heap2_start = .;
+		/*
+		 * Reserve additional memory for heap, the total should be
+		 * at least CFG_CORE_HEAP_SIZE, but count what has already
+		 * been reserved in .heap1
+		 */
+		. += CFG_CORE_HEAP_SIZE - (__heap1_end - __heap1_start);
+		. = ALIGN(4 * 1024);
+		__heap2_end = .;
+	}
+
+	.text_init : ALIGN(4 * 1024) {
+		__text_init_start = .;
+/*
+ * Include list of sections needed for boot initialization, this list
+ * overlaps with unpaged.ld.S but since unpaged.ld.S is first all those
+ * sections will go into the unpaged area.
+ */
+#include <text_init.ld.S>
+		. = ALIGN(8);
+		__text_init_end = .;
+	}
+
+	.rodata_init : ALIGN(8) {
+		__rodata_init_start = .;
+#include <rodata_init.ld.S>
+		. = ALIGN(8);
+		__start_phys_mem_map_section = . ;
+		KEEP(*(phys_mem_map_section))
+		__end_phys_mem_map_section = . ;
+		. = ALIGN(8);
+		__rodata_init_end = .;
+	}
+	__init_start = __text_init_start;
+	__init_end = .;
+	__init_size = __init_end - __text_init_start;
+
+	.text_pageable : ALIGN(8) {
+		__text_pageable_start = .;
+		*(.text*)
+		. = ALIGN(8);
+		__text_pageable_end = .;
+	}
+
+	.rodata_pageable : ALIGN(8) {
+		__rodata_pageable_start = .;
+		*(.rodata*)
+		/*
+		 * 8 to avoid unwanted padding between __start_ta_head_section
+		 * and the first structure in ta_head_section, in 64-bit
+		 * builds
+		 */
+		. = ALIGN(8);
+		__start_ta_head_section = . ;
+		KEEP(*(ta_head_section))
+		__stop_ta_head_section = . ;
+		. = ALIGN(4 * 1024);
+		__rodata_pageable_end = .;
+	}
+
+	__pageable_part_start = __rodata_init_end;
+	__pageable_part_end = __rodata_pageable_end;
+	__pageable_start = __text_init_start;
+	__pageable_end = __pageable_part_end;
+
+	/*
+	 * Assign a safe spot to store the hashes of the pages before
+	 * heap is initialized.
+	 */
+	__tmp_hashes_start = __rodata_init_end;
+	__tmp_hashes_size = ((__pageable_end - __pageable_start) /
+				(4 * 1024)) * 32;
+	__tmp_hashes_end = __tmp_hashes_start + __tmp_hashes_size;
+
+	__init_mem_usage = __tmp_hashes_end - CFG_TEE_LOAD_ADDR;
+
+	ASSERT(CFG_TEE_LOAD_ADDR >= CFG_TEE_RAM_START,
+		"Load address before start of physical memory")
+	ASSERT(CFG_TEE_LOAD_ADDR < (CFG_TEE_RAM_START + CFG_TEE_RAM_PH_SIZE),
+		"Load address after end of physical memory")
+	ASSERT(__tmp_hashes_end < (CFG_TEE_RAM_START + CFG_TEE_RAM_PH_SIZE),
+		"OP-TEE can't fit init part into available physical memory")
+	ASSERT((CFG_TEE_RAM_START + CFG_TEE_RAM_PH_SIZE - __init_end) >
+		1 * 4096, "Too few free pages to initialize paging")
+
+
+#endif /*CFG_WITH_PAGER*/
+
+#ifdef CFG_CORE_SANITIZE_KADDRESS
+	. = CFG_TEE_RAM_START + (CFG_TEE_RAM_VA_SIZE * 8) / 9 - 8;
+	. = ALIGN(8);
+	.asan_shadow : {
+		__asan_shadow_start = .;
+		. += CFG_TEE_RAM_VA_SIZE / 9;
+		__asan_shadow_end = .;
+	}
+#endif /*CFG_CORE_SANITIZE_KADDRESS*/
+
+	__end = .;
+
+#ifndef CFG_WITH_PAGER
+	__init_size = __data_end - CFG_TEE_LOAD_ADDR;
+	__init_mem_usage = __end - CFG_TEE_LOAD_ADDR;
+#endif
+	. = CFG_TEE_RAM_START + CFG_TEE_RAM_VA_SIZE;
+	_end_of_ram = .;
+
+	/DISCARD/ : {
+		/* Strip unnecessary stuff */
+		*(.comment .note .eh_frame)
+		/* Strip meta variables */
+		*(__keep_meta_vars*)
+	}
+
+}
diff --git a/core/arch/arm/kernel/link.mk b/core/arch/arm/kernel/link.mk
new file mode 100644
index 0000000..4a7bd8e
--- /dev/null
+++ b/core/arch/arm/kernel/link.mk
@@ -0,0 +1,241 @@
+link-out-dir = $(out-dir)/core
+
+link-script = $(platform-dir)/kern.ld.S
+link-script-pp = $(link-out-dir)/kern.ld
+link-script-dep = $(link-out-dir)/.kern.ld.d
+
+AWK	 = awk
+
+
+link-ldflags  = $(LDFLAGS)
+link-ldflags += -T $(link-script-pp) -Map=$(link-out-dir)/tee.map
+link-ldflags += --sort-section=alignment
+link-ldflags += --fatal-warnings
+link-ldflags += --gc-sections
+
+link-ldadd  = $(LDADD)
+link-ldadd += $(addprefix -L,$(libdirs))
+link-ldadd += $(addprefix -l,$(libnames))
+ldargs-tee.elf := $(link-ldflags) $(objs) $(link-out-dir)/version.o \
+	$(link-ldadd) $(libgcccore)
+
+link-script-cppflags := -DASM=1 \
+	$(filter-out $(CPPFLAGS_REMOVE) $(cppflags-remove), \
+		$(nostdinccore) $(CPPFLAGS) \
+		$(addprefix -I,$(incdirscore) $(link-out-dir)) \
+		$(cppflagscore))
+
+entries-unpaged += thread_init_vbar
+entries-unpaged += sm_init
+entries-unpaged += core_init_mmu_regs
+entries-unpaged += sem_cpu_sync
+entries-unpaged += generic_boot_get_handlers
+
+ldargs-all_objs := -i $(objs) $(link-ldadd) $(libgcccore)
+cleanfiles += $(link-out-dir)/all_objs.o
+$(link-out-dir)/all_objs.o: $(objs) $(libdeps) $(MAKEFILE_LIST)
+	@$(cmd-echo-silent) '  LD      $@'
+	$(q)$(LDcore) $(ldargs-all_objs) -o $@
+
+cleanfiles += $(link-out-dir)/unpaged_entries.txt
+$(link-out-dir)/unpaged_entries.txt: $(link-out-dir)/all_objs.o
+	@$(cmd-echo-silent) '  GEN     $@'
+	$(q)$(NMcore) $< | \
+		$(AWK) '/ ____keep_pager/ { printf "-u%s ", $$3 }' > $@
+
+objs-unpaged-rem += core/arch/arm/tee/entry_std.o
+objs-unpaged-rem += core/arch/arm/tee/arch_svc.o
+objs-unpaged := \
+	$(filter-out $(addprefix $(out-dir)/, $(objs-unpaged-rem)), $(objs))
+ldargs-unpaged = -i --gc-sections $(addprefix -u, $(entries-unpaged))
+ldargs-unpaged-objs := $(objs-unpaged) $(link-ldadd) $(libgcccore)
+cleanfiles += $(link-out-dir)/unpaged.o
+$(link-out-dir)/unpaged.o: $(link-out-dir)/unpaged_entries.txt
+	@$(cmd-echo-silent) '  LD      $@'
+	$(q)$(LDcore) $(ldargs-unpaged) \
+		`cat $(link-out-dir)/unpaged_entries.txt` \
+		$(ldargs-unpaged-objs) -o $@
+
+cleanfiles += $(link-out-dir)/text_unpaged.ld.S
+$(link-out-dir)/text_unpaged.ld.S: $(link-out-dir)/unpaged.o
+	@$(cmd-echo-silent) '  GEN     $@'
+	$(q)$(READELFcore) -a -W $< | ${AWK} -f ./scripts/gen_ld_text_sects.awk > $@
+
+cleanfiles += $(link-out-dir)/rodata_unpaged.ld.S
+$(link-out-dir)/rodata_unpaged.ld.S: $(link-out-dir)/unpaged.o
+	@$(cmd-echo-silent) '  GEN     $@'
+	$(q)$(READELFcore) -a -W $< | \
+		${AWK} -f ./scripts/gen_ld_rodata_sects.awk > $@
+
+
+cleanfiles += $(link-out-dir)/init_entries.txt
+$(link-out-dir)/init_entries.txt: $(link-out-dir)/all_objs.o
+	@$(cmd-echo-silent) '  GEN     $@'
+	$(q)$(NMcore) $< | \
+		$(AWK) '/ ____keep_init/ { printf "-u%s", $$3 }' > $@
+
+objs-init-rem += core/arch/arm/tee/arch_svc.o
+objs-init-rem += core/arch/arm/tee/arch_svc_asm.o
+objs-init-rem += core/arch/arm/tee/init.o
+objs-init-rem += core/arch/arm/tee/entry_std.o
+entries-init += _start
+objs-init := \
+	$(filter-out $(addprefix $(out-dir)/, $(objs-init-rem)), $(objs) \
+		$(link-out-dir)/version.o)
+ldargs-init := -i --gc-sections $(addprefix -u, $(entries-init))
+
+ldargs-init-objs := $(objs-init) $(link-ldadd) $(libgcccore)
+cleanfiles += $(link-out-dir)/init.o
+$(link-out-dir)/init.o: $(link-out-dir)/init_entries.txt
+	$(call gen-version-o)
+	@$(cmd-echo-silent) '  LD      $@'
+	$(q)$(LDcore) $(ldargs-init) \
+		`cat $(link-out-dir)/init_entries.txt` \
+		$(ldargs-init-objs) -o $@
+
+cleanfiles += $(link-out-dir)/text_init.ld.S
+$(link-out-dir)/text_init.ld.S: $(link-out-dir)/init.o
+	@$(cmd-echo-silent) '  GEN     $@'
+	$(q)$(READELFcore) -a -W $< | ${AWK} -f ./scripts/gen_ld_text_sects.awk > $@
+
+cleanfiles += $(link-out-dir)/rodata_init.ld.S
+$(link-out-dir)/rodata_init.ld.S: $(link-out-dir)/init.o
+	@$(cmd-echo-silent) '  GEN     $@'
+	$(q)$(READELFcore) -a -W $< | \
+		${AWK} -f ./scripts/gen_ld_rodata_sects.awk > $@
+
+-include $(link-script-dep)
+
+link-script-extra-deps += $(link-out-dir)/text_unpaged.ld.S
+link-script-extra-deps += $(link-out-dir)/rodata_unpaged.ld.S
+link-script-extra-deps += $(link-out-dir)/text_init.ld.S
+link-script-extra-deps += $(link-out-dir)/rodata_init.ld.S
+link-script-extra-deps += $(conf-file)
+cleanfiles += $(link-script-pp) $(link-script-dep)
+$(link-script-pp): $(link-script) $(link-script-extra-deps)
+	@$(cmd-echo-silent) '  CPP     $@'
+	@mkdir -p $(dir $@)
+	$(q)$(CPPcore) -Wp,-P,-MT,$@,-MD,$(link-script-dep) \
+		$(link-script-cppflags) $< > $@
+
+define update-buildcount
+	@$(cmd-echo-silent) '  UPD     $(1)'
+	$(q)if [ ! -f $(1) ]; then \
+		mkdir -p $(dir $(1)); \
+		echo 1 >$(1); \
+	else \
+		expr 0`cat $(1)` + 1 >$(1); \
+	fi
+endef
+
+version-o-cflags = $(filter-out -g3,$(core-platform-cflags) \
+			$(platform-cflags)) # Workaround objdump warning
+DATE_STR = `date -u`
+BUILD_COUNT_STR = `cat $(link-out-dir)/.buildcount`
+define gen-version-o
+	$(call update-buildcount,$(link-out-dir)/.buildcount)
+	@$(cmd-echo-silent) '  GEN     $(link-out-dir)/version.o'
+	$(q)echo -e "const char core_v_str[] =" \
+		"\"$(TEE_IMPL_VERSION) \"" \
+		"\"#$(BUILD_COUNT_STR) \"" \
+		"\"$(DATE_STR) \"" \
+		"\"$(CFG_KERN_LINKER_ARCH)\";\n" \
+		| $(CCcore) $(version-o-cflags) \
+			-xc - -c -o $(link-out-dir)/version.o
+endef
+$(link-out-dir)/version.o:
+	$(call gen-version-o)
+
+all: $(link-out-dir)/tee.elf
+cleanfiles += $(link-out-dir)/tee.elf $(link-out-dir)/tee.map
+cleanfiles += $(link-out-dir)/version.o
+cleanfiles += $(link-out-dir)/.buildcount
+$(link-out-dir)/tee.elf: $(objs) $(libdeps) $(link-script-pp)
+	@$(cmd-echo-silent) '  LD      $@'
+	$(q)$(LDcore) $(ldargs-tee.elf) -o $@
+
+all: $(link-out-dir)/tee.dmp
+cleanfiles += $(link-out-dir)/tee.dmp
+$(link-out-dir)/tee.dmp: $(link-out-dir)/tee.elf
+	@$(cmd-echo-silent) '  OBJDUMP $@'
+	$(q)$(OBJDUMPcore) -l -x -d $< > $@
+
+pageable_sections := .*_pageable
+init_sections := .*_init
+cleanfiles += $(link-out-dir)/tee-pager.bin
+$(link-out-dir)/tee-pager.bin: $(link-out-dir)/tee.elf \
+		$(link-out-dir)/tee-data_end.txt
+	@$(cmd-echo-silent) '  OBJCOPY $@'
+	$(q)$(OBJCOPYcore) -O binary \
+		--remove-section="$(pageable_sections)" \
+		--remove-section="$(init_sections)" \
+		--pad-to `cat $(link-out-dir)/tee-data_end.txt` \
+		$< $@
+
+cleanfiles += $(link-out-dir)/tee-pageable.bin
+$(link-out-dir)/tee-pageable.bin: $(link-out-dir)/tee.elf
+	@$(cmd-echo-silent) '  OBJCOPY $@'
+	$(q)$(OBJCOPYcore) -O binary \
+		--only-section="$(init_sections)" \
+		--only-section="$(pageable_sections)" \
+		$< $@
+
+cleanfiles += $(link-out-dir)/tee-data_end.txt
+$(link-out-dir)/tee-data_end.txt: $(link-out-dir)/tee.elf
+	@$(cmd-echo-silent) '  GEN     $@'
+	@echo -n 0x > $@
+	$(q)$(NMcore) $< | grep __data_end | sed 's/ .*$$//' >> $@
+
+cleanfiles += $(link-out-dir)/tee-init_size.txt
+$(link-out-dir)/tee-init_size.txt: $(link-out-dir)/tee.elf
+	@$(cmd-echo-silent) '  GEN     $@'
+	@echo -n 0x > $@
+	$(q)$(NMcore) $< | grep __init_size | sed 's/ .*$$//' >> $@
+
+cleanfiles += $(link-out-dir)/tee-init_load_addr.txt
+$(link-out-dir)/tee-init_load_addr.txt: $(link-out-dir)/tee.elf
+	@$(cmd-echo-silent) '  GEN     $@'
+	@echo -n 0x > $@
+	$(q)$(NMcore) $< | grep ' _start' | sed 's/ .*$$//' >> $@
+
+cleanfiles += $(link-out-dir)/tee-init_mem_usage.txt
+$(link-out-dir)/tee-init_mem_usage.txt: $(link-out-dir)/tee.elf
+	@$(cmd-echo-silent) '  GEN     $@'
+	@echo -n 0x > $@
+	$(q)$(NMcore) $< | grep ' __init_mem_usage' | sed 's/ .*$$//' >> $@
+
+all: $(link-out-dir)/tee.bin
+cleanfiles += $(link-out-dir)/tee.bin
+$(link-out-dir)/tee.bin: $(link-out-dir)/tee-pager.bin \
+			 $(link-out-dir)/tee-pageable.bin \
+			 $(link-out-dir)/tee-init_size.txt \
+			 $(link-out-dir)/tee-init_load_addr.txt \
+			 $(link-out-dir)/tee-init_mem_usage.txt \
+			./scripts/gen_hashed_bin.py
+	@$(cmd-echo-silent) '  GEN     $@'
+	$(q)load_addr=`cat $(link-out-dir)/tee-init_load_addr.txt` && \
+	./scripts/gen_hashed_bin.py \
+		--arch $(if $(filter y,$(CFG_ARM64_core)),arm64,arm32) \
+		--init_size `cat $(link-out-dir)/tee-init_size.txt` \
+		--init_load_addr_hi $$(($$load_addr >> 32 & 0xffffffff)) \
+		--init_load_addr_lo $$(($$load_addr & 0xffffffff)) \
+		--init_mem_usage `cat $(link-out-dir)/tee-init_mem_usage.txt` \
+		--tee_pager_bin $(link-out-dir)/tee-pager.bin \
+		--tee_pageable_bin $(link-out-dir)/tee-pageable.bin \
+		--out $@
+
+
+all: $(link-out-dir)/tee.symb_sizes
+cleanfiles += $(link-out-dir)/tee.symb_sizes
+$(link-out-dir)/tee.symb_sizes: $(link-out-dir)/tee.elf
+	@$(cmd-echo-silent) '  GEN     $@'
+	$(q)$(NMcore) --print-size --reverse-sort --size-sort $< > $@
+
+cleanfiles += $(link-out-dir)/tee.mem_usage
+ifneq ($(filter mem_usage,$(MAKECMDGOALS)),)
+mem_usage: $(link-out-dir)/tee.mem_usage
+
+$(link-out-dir)/tee.mem_usage: $(link-out-dir)/tee.elf
+	@$(cmd-echo-silent) '  GEN     $@'
+	$(q)$(READELFcore) -a -W $< | ${AWK} -f ./scripts/mem_usage.awk > $@
+endif
diff --git a/core/arch/arm/kernel/misc_a32.S b/core/arch/arm/kernel/misc_a32.S
new file mode 100644
index 0000000..48fd8ba
--- /dev/null
+++ b/core/arch/arm/kernel/misc_a32.S
@@ -0,0 +1,90 @@
+/*
+ * 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 <asm.S>
+#include <arm.h>
+#include <arm32_macros.S>
+#include <kernel/unwind.h>
+
+/* Let platforms override this if needed */
+.weak get_core_pos
+
+FUNC get_core_pos , :
+UNWIND(	.fnstart)
+	read_mpidr r0
+	/* Calculate CorePos = (ClusterId * 4) + CoreId */
+	and	r1, r0, #MPIDR_CPU_MASK
+	and	r0, r0, #MPIDR_CLUSTER_MASK
+	add	r0, r1, r0, LSR #6
+	bx	lr
+UNWIND(	.fnend)
+END_FUNC get_core_pos
+
+/*
+ * uint32_t temp_set_mode(int cpu_mode)
+ *   returns cpsr to be set
+ */
+LOCAL_FUNC temp_set_mode , :
+UNWIND(	.fnstart)
+	mov	r1, r0
+	cmp	r1, #CPSR_MODE_USR	/* update mode: usr -> sys */
+	moveq	r1, #CPSR_MODE_SYS
+	cpsid	aif			/* disable interrupts */
+	mrs	r0, cpsr		/* get cpsr with disabled its*/
+	bic	r0, #CPSR_MODE_MASK	/* clear mode */
+	orr	r0, r1			/* set expected mode */
+	bx	lr
+UNWIND(	.fnend)
+END_FUNC temp_set_mode
+
+/* uint32_t read_mode_sp(int cpu_mode) */
+FUNC read_mode_sp , :
+UNWIND(	.fnstart)
+	push	{r4, lr}
+UNWIND(	.save	{r4, lr})
+	mrs	r4, cpsr		/* save cpsr */
+	bl	temp_set_mode
+	msr	cpsr, r0		/* set the new mode */
+	mov	r0, sp			/* get the function result */
+	msr	cpsr, r4		/* back to the old mode */
+	pop	{r4, pc}
+UNWIND(	.fnend)
+END_FUNC read_mode_sp
+
+/* uint32_t read_mode_lr(int cpu_mode) */
+FUNC read_mode_lr , :
+UNWIND(	.fnstart)
+	push	{r4, lr}
+UNWIND(	.save	{r4, lr})
+	mrs	r4, cpsr		/* save cpsr */
+	bl	temp_set_mode
+	msr	cpsr, r0		/* set the new mode */
+	mov	r0, lr			/* get the function result */
+	msr	cpsr, r4		/* back to the old mode */
+	pop	{r4, pc}
+UNWIND(	.fnend)
+END_FUNC read_mode_lr
diff --git a/core/arch/arm/kernel/misc_a64.S b/core/arch/arm/kernel/misc_a64.S
new file mode 100644
index 0000000..2b4da4a
--- /dev/null
+++ b/core/arch/arm/kernel/misc_a64.S
@@ -0,0 +1,41 @@
+/*
+ * 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 <asm.S>
+#include <arm.h>
+
+/* Let platforms override this if needed */
+.weak get_core_pos
+
+FUNC get_core_pos , :
+	mrs	x0, mpidr_el1
+	/* Calculate CorePos = (ClusterId * 4) + CoreId */
+	and	x1, x0, #MPIDR_CPU_MASK
+	and	x0, x0, #MPIDR_CLUSTER_MASK
+	add	x0, x1, x0, LSR #6
+	ret
+END_FUNC get_core_pos
diff --git a/core/arch/arm/kernel/mutex.c b/core/arch/arm/kernel/mutex.c
new file mode 100644
index 0000000..0e1b836
--- /dev/null
+++ b/core/arch/arm/kernel/mutex.c
@@ -0,0 +1,279 @@
+/*
+ * 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/mutex.h>
+#include <kernel/panic.h>
+#include <kernel/spinlock.h>
+#include <kernel/thread.h>
+#include <trace.h>
+
+void mutex_init(struct mutex *m)
+{
+	*m = (struct mutex)MUTEX_INITIALIZER;
+}
+
+static void __mutex_lock(struct mutex *m, const char *fname, int lineno)
+{
+	assert_have_no_spinlock();
+	assert(thread_get_id_may_fail() != -1);
+
+	while (true) {
+		uint32_t old_itr_status;
+		enum mutex_value old_value;
+		struct wait_queue_elem wqe;
+
+		/*
+		 * If the mutex is locked we need to initialize the wqe
+		 * before releasing the spinlock to guarantee that we don't
+		 * miss the wakeup from mutex_unlock().
+		 *
+		 * If the mutex is unlocked we don't need to use the wqe at
+		 * all.
+		 */
+
+		old_itr_status = thread_mask_exceptions(THREAD_EXCP_ALL);
+		cpu_spin_lock(&m->spin_lock);
+
+		old_value = m->value;
+		if (old_value == MUTEX_VALUE_LOCKED) {
+			wq_wait_init(&m->wq, &wqe);
+		} else {
+			m->value = MUTEX_VALUE_LOCKED;
+			thread_add_mutex(m);
+		}
+
+		cpu_spin_unlock(&m->spin_lock);
+		thread_unmask_exceptions(old_itr_status);
+
+		if (old_value == MUTEX_VALUE_LOCKED) {
+			/*
+			 * 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);
+		} else
+			return;
+	}
+}
+
+static void __mutex_unlock(struct mutex *m, const char *fname, int lineno)
+{
+	uint32_t old_itr_status;
+
+	assert_have_no_spinlock();
+	assert(thread_get_id_may_fail() != -1);
+
+	old_itr_status = thread_mask_exceptions(THREAD_EXCP_ALL);
+	cpu_spin_lock(&m->spin_lock);
+
+	if (m->value != MUTEX_VALUE_LOCKED)
+		panic();
+
+	thread_rem_mutex(m);
+	m->value = MUTEX_VALUE_UNLOCKED;
+
+	cpu_spin_unlock(&m->spin_lock);
+	thread_unmask_exceptions(old_itr_status);
+
+	wq_wake_one(&m->wq, m, fname, lineno);
+}
+
+static bool __mutex_trylock(struct mutex *m, const char *fname __unused,
+			int lineno __unused)
+{
+	uint32_t old_itr_status;
+	enum mutex_value old_value;
+
+	assert_have_no_spinlock();
+	assert(thread_get_id_may_fail() != -1);
+
+	old_itr_status = thread_mask_exceptions(THREAD_EXCP_ALL);
+	cpu_spin_lock(&m->spin_lock);
+
+	old_value = m->value;
+	if (old_value == MUTEX_VALUE_UNLOCKED) {
+		m->value = MUTEX_VALUE_LOCKED;
+		thread_add_mutex(m);
+	}
+
+	cpu_spin_unlock(&m->spin_lock);
+	thread_unmask_exceptions(old_itr_status);
+
+	return old_value == MUTEX_VALUE_UNLOCKED;
+}
+
+#ifdef CFG_MUTEX_DEBUG
+void mutex_unlock_debug(struct mutex *m, const char *fname, int lineno)
+{
+	__mutex_unlock(m, fname, lineno);
+}
+
+void mutex_lock_debug(struct mutex *m, const char *fname, int lineno)
+{
+	__mutex_lock(m, fname, lineno);
+}
+
+bool mutex_trylock_debug(struct mutex *m, const char *fname, int lineno)
+{
+	return __mutex_trylock(m, fname, lineno);
+}
+#else
+void mutex_unlock(struct mutex *m)
+{
+	__mutex_unlock(m, NULL, -1);
+}
+
+void mutex_lock(struct mutex *m)
+{
+	__mutex_lock(m, NULL, -1);
+}
+
+bool mutex_trylock(struct mutex *m)
+{
+	return __mutex_trylock(m, NULL, -1);
+}
+#endif
+
+
+
+void mutex_destroy(struct mutex *m)
+{
+	/*
+	 * Caller guarantees that no one will try to take the mutex so
+	 * there's no need to take the spinlock before accessing it.
+	 */
+	if (m->value != MUTEX_VALUE_UNLOCKED)
+		panic();
+	if (!wq_is_empty(&m->wq))
+		panic("waitqueue not empty");
+}
+
+void condvar_init(struct condvar *cv)
+{
+	*cv = (struct condvar)CONDVAR_INITIALIZER;
+}
+
+void condvar_destroy(struct condvar *cv)
+{
+	if (cv->m && wq_have_condvar(&cv->m->wq, cv))
+		panic();
+
+	condvar_init(cv);
+}
+
+static void cv_signal(struct condvar *cv, bool only_one, const char *fname,
+			int lineno)
+{
+	uint32_t old_itr_status;
+	struct mutex *m;
+
+	old_itr_status = thread_mask_exceptions(THREAD_EXCP_ALL);
+	cpu_spin_lock(&cv->spin_lock);
+	m = cv->m;
+	cpu_spin_unlock(&cv->spin_lock);
+	thread_unmask_exceptions(old_itr_status);
+
+	if (m)
+		wq_promote_condvar(&m->wq, cv, only_one, m, fname, lineno);
+
+}
+
+#ifdef CFG_MUTEX_DEBUG
+void condvar_signal_debug(struct condvar *cv, const char *fname, int lineno)
+{
+	cv_signal(cv, true /* only one */, fname, lineno);
+}
+
+void condvar_broadcast_debug(struct condvar *cv, const char *fname, int lineno)
+{
+	cv_signal(cv, false /* all */, fname, lineno);
+}
+
+#else
+void condvar_signal(struct condvar *cv)
+{
+	cv_signal(cv, true /* only one */, NULL, -1);
+}
+
+void condvar_broadcast(struct condvar *cv)
+{
+	cv_signal(cv, false /* all */, NULL, -1);
+}
+#endif /*CFG_MUTEX_DEBUG*/
+
+static void __condvar_wait(struct condvar *cv, struct mutex *m,
+			const char *fname, int lineno)
+{
+	uint32_t old_itr_status;
+	struct wait_queue_elem wqe;
+
+	old_itr_status = thread_mask_exceptions(THREAD_EXCP_ALL);
+
+	/* Link this condvar to this mutex until reinitialized */
+	cpu_spin_lock(&cv->spin_lock);
+	if (cv->m && cv->m != m)
+		panic("invalid mutex");
+
+	cv->m = m;
+	cpu_spin_unlock(&cv->spin_lock);
+
+	cpu_spin_lock(&m->spin_lock);
+
+	/* Add to mutex wait queue as a condvar waiter */
+	wq_wait_init_condvar(&m->wq, &wqe, cv);
+
+	/* Unlock the mutex */
+	if (m->value != MUTEX_VALUE_LOCKED)
+		panic();
+
+	thread_rem_mutex(m);
+	m->value = MUTEX_VALUE_UNLOCKED;
+
+	cpu_spin_unlock(&m->spin_lock);
+
+	thread_unmask_exceptions(old_itr_status);
+
+	/* Wake eventual waiters */
+	wq_wake_one(&m->wq, m, fname, lineno);
+
+	wq_wait_final(&m->wq, &wqe, m, fname, lineno);
+
+	mutex_lock(m);
+}
+
+#ifdef CFG_MUTEX_DEBUG
+void condvar_wait_debug(struct condvar *cv, struct mutex *m,
+			const char *fname, int lineno)
+{
+	__condvar_wait(cv, m, fname, lineno);
+}
+#else
+void condvar_wait(struct condvar *cv, struct mutex *m)
+{
+	__condvar_wait(cv, m, NULL, -1);
+}
+#endif
diff --git a/core/arch/arm/kernel/pm_stubs.c b/core/arch/arm/kernel/pm_stubs.c
new file mode 100644
index 0000000..db77e7c
--- /dev/null
+++ b/core/arch/arm/kernel/pm_stubs.c
@@ -0,0 +1,41 @@
+/*
+ * 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 <compiler.h>
+#include <kernel/panic.h>
+#include <kernel/pm_stubs.h>
+
+unsigned long pm_panic(unsigned long a0 __unused, unsigned long a1 __unused)
+{
+	panic();
+}
+
+unsigned long pm_do_nothing(unsigned long a0 __unused,
+			    unsigned long a1 __unused)
+{
+	return 0;
+}
diff --git a/core/arch/arm/kernel/proc_a32.S b/core/arch/arm/kernel/proc_a32.S
new file mode 100644
index 0000000..f0446a6
--- /dev/null
+++ b/core/arch/arm/kernel/proc_a32.S
@@ -0,0 +1,96 @@
+/*
+ * Copyright (c) 2016, ARM Limited and Contributors. All rights reserved.
+ * 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.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE 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 <arm.h>
+#include <arm32_macros.S>
+#include <asm.S>
+#include <keep.h>
+#include <kernel/unwind.h>
+
+/*
+ * void cpu_mmu_enable(void) - enable MMU
+ *
+ * TLBs are invalidated before MMU is enabled.
+ * An DSB and ISB insures MMUs is enabled before routine returns
+ */
+FUNC cpu_mmu_enable , :
+UNWIND(	.fnstart)
+	/* Invalidate TLB */
+	write_tlbiall
+
+	/* Enable the MMU */
+	read_sctlr r0
+	orr	r0, r0, #SCTLR_M
+	write_sctlr r0
+
+	dsb
+	isb
+
+	bx	lr
+UNWIND(	.fnend)
+END_FUNC cpu_mmu_enable
+KEEP_PAGER cpu_mmu_enable
+
+/* void cpu_mmu_enable_icache(void) - enable instruction cache */
+FUNC cpu_mmu_enable_icache , :
+UNWIND(	.fnstart)
+	/* Invalidate instruction cache and branch predictor */
+	write_iciallu
+	write_bpiall
+
+	/* Enable the instruction cache */
+	read_sctlr r1
+	orr	r1, r1, #SCTLR_I
+	write_sctlr r1
+
+	dsb
+	isb
+
+	bx	lr
+UNWIND(	.fnend)
+END_FUNC cpu_mmu_enable_icache
+KEEP_PAGER cpu_mmu_enable_icache
+
+/* void cpu_mmu_enable_dcache(void) - enable data cache */
+FUNC cpu_mmu_enable_dcache , :
+UNWIND(	.fnstart)
+	read_sctlr r0
+	orr	r0, r0, #SCTLR_C
+	write_sctlr r0
+
+	dsb
+	isb
+
+	bx	lr
+UNWIND(	.fnend)
+END_FUNC cpu_mmu_enable_dcache
+KEEP_PAGER cpu_mmu_enable_dcache
diff --git a/core/arch/arm/kernel/proc_a64.S b/core/arch/arm/kernel/proc_a64.S
new file mode 100644
index 0000000..5db895a
--- /dev/null
+++ b/core/arch/arm/kernel/proc_a64.S
@@ -0,0 +1,71 @@
+/*
+ * 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 <arm64.h>
+#include <asm.S>
+
+/* void cpu_mmu_enable(void) */
+FUNC cpu_mmu_enable , :
+	/* Invalidate TLB */
+	tlbi	vmalle1
+
+	/*
+	 * Make sure translation table writes have drained into memory and
+	 * the TLB invalidation is complete.
+	 */
+	dsb	sy
+	isb
+
+	/* Enable the MMU */
+	mrs	x0, sctlr_el1
+	orr	x0, x0, #SCTLR_M
+	msr	sctlr_el1, x0
+	isb
+
+	ret
+END_FUNC cpu_mmu_enable
+
+/* void cpu_mmu_enable_icache(void) */
+FUNC cpu_mmu_enable_icache , :
+	/* Invalidate instruction cache and branch predictor */
+	ic	iallu
+	isb
+	mrs	x0, sctlr_el1
+	orr	x0, x0, #SCTLR_I
+	msr	sctlr_el1, x0
+	isb
+	ret
+END_FUNC cpu_mmu_enable_icache
+
+
+/* void cpu_mmu_enable_dcache(void) */
+FUNC cpu_mmu_enable_dcache , :
+	mrs	x0, sctlr_el1
+	orr	x0, x0, #SCTLR_C
+	msr	sctlr_el1, x0
+	isb
+	ret
+END_FUNC cpu_mmu_enable_dcache
diff --git a/core/arch/arm/kernel/pseudo_ta.c b/core/arch/arm/kernel/pseudo_ta.c
new file mode 100644
index 0000000..6352a28
--- /dev/null
+++ b/core/arch/arm/kernel/pseudo_ta.c
@@ -0,0 +1,256 @@
+/*
+ * 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 <initcall.h>
+#include <kernel/panic.h>
+#include <kernel/pseudo_ta.h>
+#include <kernel/tee_ta_manager.h>
+#include <mm/core_memprot.h>
+#include <mm/mobj.h>
+#include <sm/tee_mon.h>
+#include <stdlib.h>
+#include <string.h>
+#include <trace.h>
+#include <types_ext.h>
+
+/* Maps static TA params */
+static TEE_Result copy_in_param(struct tee_ta_param *param,
+				     TEE_Param tee_param[TEE_NUM_PARAMS])
+{
+	size_t n;
+	void *va;
+
+	for (n = 0; n < TEE_NUM_PARAMS; n++) {
+		switch (TEE_PARAM_TYPE_GET(param->types, n)) {
+		case TEE_PARAM_TYPE_VALUE_INPUT:
+		case TEE_PARAM_TYPE_VALUE_OUTPUT:
+		case TEE_PARAM_TYPE_VALUE_INOUT:
+			tee_param[n].value.a = param->u[n].val.a;
+			tee_param[n].value.b = param->u[n].val.b;
+			break;
+		case TEE_PARAM_TYPE_MEMREF_INPUT:
+		case TEE_PARAM_TYPE_MEMREF_OUTPUT:
+		case TEE_PARAM_TYPE_MEMREF_INOUT:
+			va = mobj_get_va(param->u[n].mem.mobj,
+					 param->u[n].mem.offs);
+			if (!va)
+				return TEE_ERROR_BAD_PARAMETERS;
+			tee_param[n].memref.buffer = va;
+			tee_param[n].memref.size = param->u[n].mem.size;
+			break;
+		default:
+			memset(tee_param + n, 0, sizeof(TEE_Param));
+			break;
+		}
+	}
+
+	return TEE_SUCCESS;
+}
+
+static void update_out_param(TEE_Param tee_param[TEE_NUM_PARAMS],
+			     struct tee_ta_param *param)
+{
+	size_t n;
+
+	for (n = 0; n < TEE_NUM_PARAMS; n++) {
+		switch (TEE_PARAM_TYPE_GET(param->types, n)) {
+		case TEE_PARAM_TYPE_VALUE_OUTPUT:
+		case TEE_PARAM_TYPE_VALUE_INOUT:
+			param->u[n].val.a = tee_param[n].value.a;
+			param->u[n].val.b = tee_param[n].value.b;
+			break;
+		case TEE_PARAM_TYPE_MEMREF_OUTPUT:
+		case TEE_PARAM_TYPE_MEMREF_INOUT:
+			param->u[n].mem.size = tee_param[n].memref.size;
+			break;
+		default:
+			break;
+		}
+	}
+}
+
+static TEE_Result pseudo_ta_enter_open_session(struct tee_ta_session *s,
+			struct tee_ta_param *param, TEE_ErrorOrigin *eo)
+{
+	TEE_Result res = TEE_SUCCESS;
+	struct pseudo_ta_ctx *stc = to_pseudo_ta_ctx(s->ctx);
+	TEE_Param tee_param[TEE_NUM_PARAMS];
+
+	tee_ta_push_current_session(s);
+	*eo = TEE_ORIGIN_TRUSTED_APP;
+
+	if ((s->ctx->ref_count == 1) && stc->pseudo_ta->create_entry_point) {
+		res = stc->pseudo_ta->create_entry_point();
+		if (res != TEE_SUCCESS)
+			goto out;
+	}
+
+	if (stc->pseudo_ta->open_session_entry_point) {
+		res = copy_in_param(param, tee_param);
+		if (res != TEE_SUCCESS) {
+			*eo = TEE_ORIGIN_TEE;
+			goto out;
+		}
+
+		res = stc->pseudo_ta->open_session_entry_point(param->types,
+								tee_param,
+								&s->user_ctx);
+		update_out_param(tee_param, param);
+	}
+
+out:
+	tee_ta_pop_current_session();
+	return res;
+}
+
+static TEE_Result pseudo_ta_enter_invoke_cmd(struct tee_ta_session *s,
+			uint32_t cmd, struct tee_ta_param *param,
+			TEE_ErrorOrigin *eo)
+{
+	TEE_Result res;
+	struct pseudo_ta_ctx *stc = to_pseudo_ta_ctx(s->ctx);
+	TEE_Param tee_param[TEE_NUM_PARAMS];
+
+	tee_ta_push_current_session(s);
+	res = copy_in_param(param, tee_param);
+	if (res != TEE_SUCCESS) {
+		*eo = TEE_ORIGIN_TEE;
+		goto out;
+	}
+
+	*eo = TEE_ORIGIN_TRUSTED_APP;
+	res = stc->pseudo_ta->invoke_command_entry_point(s->user_ctx, cmd,
+							 param->types,
+							 tee_param);
+	update_out_param(tee_param, param);
+out:
+	tee_ta_pop_current_session();
+	return res;
+}
+
+static void pseudo_ta_enter_close_session(struct tee_ta_session *s)
+{
+	struct pseudo_ta_ctx *stc = to_pseudo_ta_ctx(s->ctx);
+
+	tee_ta_push_current_session(s);
+
+	if (stc->pseudo_ta->close_session_entry_point)
+		stc->pseudo_ta->close_session_entry_point(s->user_ctx);
+
+	if ((s->ctx->ref_count == 1) && stc->pseudo_ta->destroy_entry_point)
+		stc->pseudo_ta->destroy_entry_point();
+
+	tee_ta_pop_current_session();
+}
+
+static void pseudo_ta_destroy(struct tee_ta_ctx *ctx)
+{
+	free(to_pseudo_ta_ctx(ctx));
+}
+
+static const struct tee_ta_ops pseudo_ta_ops = {
+	.enter_open_session = pseudo_ta_enter_open_session,
+	.enter_invoke_cmd = pseudo_ta_enter_invoke_cmd,
+	.enter_close_session = pseudo_ta_enter_close_session,
+	.destroy = pseudo_ta_destroy,
+};
+
+
+/* Defined in link script */
+extern const struct pseudo_ta_head __start_ta_head_section;
+extern const struct pseudo_ta_head __stop_ta_head_section;
+
+/* Insures declared pseudo TAs conforms with core expectations */
+static TEE_Result verify_pseudo_tas_conformance(void)
+{
+	const struct pseudo_ta_head *start = &__start_ta_head_section;
+	const struct pseudo_ta_head *end = &__stop_ta_head_section;
+	const struct pseudo_ta_head *pta;
+
+	for (pta = start; pta < end; pta++) {
+		const struct pseudo_ta_head *pta2;
+
+		/* PTAs must all have a specific UUID */
+		for (pta2 = pta + 1; pta2 < end; pta2++)
+			if (!memcmp(&pta->uuid, &pta2->uuid, sizeof(TEE_UUID)))
+				goto err;
+
+		if (!pta->name ||
+		    (pta->flags & PTA_MANDATORY_FLAGS) != PTA_MANDATORY_FLAGS ||
+		    pta->flags & ~PTA_ALLOWED_FLAGS ||
+		    !pta->invoke_command_entry_point)
+			goto err;
+	}
+	return TEE_SUCCESS;
+err:
+	DMSG("pseudo TA error at %p", (void *)pta);
+	panic("pta");
+}
+
+service_init(verify_pseudo_tas_conformance);
+
+/*-----------------------------------------------------------------------------
+ * Initialises a session based on the UUID or ptr to the ta
+ * Returns ptr to the session (ta_session) and a TEE_Result
+ *---------------------------------------------------------------------------*/
+TEE_Result tee_ta_init_pseudo_ta_session(const TEE_UUID *uuid,
+			struct tee_ta_session *s)
+{
+	struct pseudo_ta_ctx *stc = NULL;
+	struct tee_ta_ctx *ctx;
+	const struct pseudo_ta_head *ta;
+
+	DMSG("   Lookup for Static TA %pUl", (void *)uuid);
+
+	ta = &__start_ta_head_section;
+	while (true) {
+		if (ta >= &__stop_ta_head_section)
+			return TEE_ERROR_ITEM_NOT_FOUND;
+		if (memcmp(&ta->uuid, uuid, sizeof(TEE_UUID)) == 0)
+			break;
+		ta++;
+	}
+
+	/* Load a new TA and create a session */
+	DMSG("      Open %s", ta->name);
+	stc = calloc(1, sizeof(struct pseudo_ta_ctx));
+	if (!stc)
+		return TEE_ERROR_OUT_OF_MEMORY;
+	ctx = &stc->ctx;
+
+	ctx->ref_count = 1;
+	s->ctx = ctx;
+	ctx->flags = ta->flags;
+	stc->pseudo_ta = ta;
+	ctx->uuid = ta->uuid;
+	ctx->ops = &pseudo_ta_ops;
+	TAILQ_INSERT_TAIL(&tee_ctxes, ctx, link);
+
+	DMSG("      %s : %pUl", stc->pseudo_ta->name, (void *)&ctx->uuid);
+
+	return TEE_SUCCESS;
+}
diff --git a/core/arch/arm/kernel/spin_lock_a32.S b/core/arch/arm/kernel/spin_lock_a32.S
new file mode 100644
index 0000000..52d8e9f
--- /dev/null
+++ b/core/arch/arm/kernel/spin_lock_a32.S
@@ -0,0 +1,85 @@
+/*
+ * Copyright (c) 2016, Linaro Limited
+ * Copyright (c) 2016, ARM Limited and Contributors. All rights reserved.
+ * 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.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE 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 <asm.S>
+#include <kernel/spinlock.h>
+#include <kernel/unwind.h>
+
+/* void __cpu_spin_lock(unsigned int *lock) */
+FUNC __cpu_spin_lock , :
+UNWIND(	.fnstart)
+	mov r2, #SPINLOCK_LOCK
+1:
+	ldrex r1, [r0]
+	cmp r1, #SPINLOCK_UNLOCK
+	wfene
+	strexeq r1, r2, [r0]
+	cmpeq r1, #0
+	bne 1b
+	dmb
+	bx lr
+UNWIND(	.fnend)
+END_FUNC __cpu_spin_lock
+
+/* int __cpu_spin_trylock(unsigned int *lock) - return 0 on success */
+FUNC __cpu_spin_trylock , :
+UNWIND(	.fnstart)
+	mov r2, #SPINLOCK_LOCK
+	mov r1, r0
+1:
+	ldrex r0, [r1]
+	cmp r0, #0
+	bne 1f
+	strex r0, r2, [r1]
+	cmp r0, #0
+	bne 1b
+	dmb
+	bx lr
+1:
+	clrex
+	dmb
+	bx lr
+UNWIND(	.fnend)
+END_FUNC __cpu_spin_trylock
+
+/* void __cpu_spin_unlock(unsigned int *lock) */
+FUNC __cpu_spin_unlock , :
+UNWIND(	.fnstart)
+	dmb
+	mov r1, #SPINLOCK_UNLOCK
+	str r1, [r0]
+	dsb
+	sev
+	bx lr
+UNWIND(	.fnend)
+END_FUNC __cpu_spin_unlock
diff --git a/core/arch/arm/kernel/spin_lock_a64.S b/core/arch/arm/kernel/spin_lock_a64.S
new file mode 100644
index 0000000..97fce42
--- /dev/null
+++ b/core/arch/arm/kernel/spin_lock_a64.S
@@ -0,0 +1,89 @@
+/*
+ * 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.
+ */
+
+/*
+ * Copyright (c) 2013-2014, ARM Limited and Contributors. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * 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.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE 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 <asm.S>
+#include <kernel/spinlock.h>
+
+/* void __cpu_spin_lock(unsigned int *lock); */
+FUNC __cpu_spin_lock , :
+	mov	w2, #SPINLOCK_LOCK
+	sevl
+l1:	wfe
+l2:	ldaxr	w1, [x0]
+	cbnz	w1, l1
+	stxr	w1, w2, [x0]
+	cbnz	w1, l2
+	ret
+END_FUNC __cpu_spin_lock
+
+/* unsigned int __cpu_spin_trylock(unsigned int *lock); */
+FUNC __cpu_spin_trylock , :
+	mov     x1, x0
+	mov     w2, #SPINLOCK_LOCK
+.loop:	ldaxr   w0, [x1]
+	cbnz    w0, .cpu_spin_trylock_out
+	stxr    w0, w2, [x1]
+	cbnz    w0, .loop
+.cpu_spin_trylock_out:
+	ret
+END_FUNC __cpu_spin_trylock
+
+/* void __cpu_spin_unlock(unsigned int *lock); */
+FUNC __cpu_spin_unlock , :
+	stlr	wzr, [x0]
+	ret
+END_FUNC __cpu_spin_unlock
diff --git a/core/arch/arm/kernel/spin_lock_debug.c b/core/arch/arm/kernel/spin_lock_debug.c
new file mode 100644
index 0000000..2a450a5
--- /dev/null
+++ b/core/arch/arm/kernel/spin_lock_debug.c
@@ -0,0 +1,63 @@
+/*
+ * Copyright (c) 2016, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <assert.h>
+#include <kernel/spinlock.h>
+#include "thread_private.h"
+
+void spinlock_count_incr(void)
+{
+	struct thread_core_local *l = thread_get_core_local();
+
+	l->locked_count++;
+	assert(l->locked_count);
+}
+
+void spinlock_count_decr(void)
+{
+	struct thread_core_local *l = thread_get_core_local();
+
+	assert(l->locked_count);
+	l->locked_count--;
+}
+
+bool have_spinlock(void)
+{
+	struct thread_core_local *l;
+
+	if (!thread_irq_disabled()) {
+		/*
+		 * Normally we can't be holding a spinlock since doing so would
+		 * imply IRQ are disabled (or the spinlock logic is flawed).
+		 */
+		return false;
+	}
+
+	l = thread_get_core_local();
+
+	return !!l->locked_count;
+}
diff --git a/core/arch/arm/kernel/ssvce_a32.S b/core/arch/arm/kernel/ssvce_a32.S
new file mode 100644
index 0000000..e2850f1
--- /dev/null
+++ b/core/arch/arm/kernel/ssvce_a32.S
@@ -0,0 +1,334 @@
+/*
+ * 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.
+ */
+
+/*
+ * ARMv7 Secure Services library
+ */
+
+/*
+ * Variable(s)
+ */
+#include <asm.S>
+#include <arm.h>
+#include <arm32_macros.S>
+
+#include <kernel/tz_proc_def.h>
+#include <kernel/tz_ssvce_def.h>
+#include <kernel/unwind.h>
+
+	.section .text.ssvce
+
+
+
+/*
+ * - MMU maintenaince support ---------------------------------------------
+ */
+
+
+/*
+ * void secure_mmu_unifiedtlbinvall(void);
+ */
+FUNC secure_mmu_unifiedtlbinvall , :
+UNWIND(	.fnstart)
+
+	write_tlbiallis
+
+	DSB
+	ISB
+
+	MOV     PC, LR
+UNWIND(	.fnend)
+END_FUNC secure_mmu_unifiedtlbinvall
+
+/*
+ * void secure_mmu_unifiedtlbinvbymva(mva);
+ *
+ * Combine VA and current ASID, and invalidate matching TLB
+ */
+FUNC secure_mmu_unifiedtlbinvbymva , :
+UNWIND(	.fnstart)
+
+	b .	@ Wrong code to force fix/check the routine before using it
+
+	MRC     p15, 0, R1, c13, c0, 1		/* Read CP15 Context ID Register (CONTEXTIDR) */
+	ANDS    R1, R1, #0xFF			/* Get current ASID */
+	ORR     R1, R1, R0			/* Combine MVA and ASID */
+
+	MCR     p15, 0, R1, c8, c7, 1		/* Invalidate Unified TLB entry by MVA */
+
+	DSB
+	ISB
+
+	MOV     PC, LR
+UNWIND(	.fnend)
+END_FUNC secure_mmu_unifiedtlbinvbymva
+
+/*
+ * void secure_mmu_unifiedtlbinv_curasid(void)
+ *
+ * Invalidate TLB matching current ASID
+ */
+FUNC secure_mmu_unifiedtlbinv_curasid , :
+UNWIND(	.fnstart)
+	read_contextidr r0
+	and     r0, r0, #0xff               /* Get current ASID */
+	/* Invalidate unified TLB by ASID Inner Sharable */
+	write_tlbiasidis r0
+	dsb
+	isb
+	mov	pc, lr
+UNWIND(	.fnend)
+END_FUNC secure_mmu_unifiedtlbinv_curasid
+
+/*
+ * void secure_mmu_unifiedtlbinv_byasid(unsigned int asid)
+ *
+ * Invalidate TLB matching current ASID
+ */
+FUNC secure_mmu_unifiedtlbinv_byasid , :
+UNWIND(	.fnstart)
+	and     r0, r0, #0xff               /* Get ASID */
+	/* Invalidate unified TLB by ASID Inner Sharable */
+	write_tlbiasidis r0
+	dsb
+	isb
+	mov	pc, lr
+UNWIND(	.fnend)
+END_FUNC secure_mmu_unifiedtlbinv_byasid
+
+/*
+ * void arm_cl1_d_cleanbysetway(void)
+ */
+FUNC arm_cl1_d_cleanbysetway , :
+UNWIND(	.fnstart)
+
+	MOV     R0, #0                  @ ; write the Cache Size selection register to be
+	MCR     p15, 2, R0, c0, c0, 0   @ ; sure we address the data cache
+	ISB                             @ ; ISB to sync the change to the CacheSizeID reg
+
+	MOV     R0, #0                  @ ; set way number to 0
+_cl_nextWay:
+	MOV     R1, #0                  @ ; set line number (=index) to 0
+_cl_nextLine:
+	ORR     R2, R0, R1                          @ ; construct way/index value
+	MCR     p15, 0, R2, c7, c10, 2              @ ; DCCSW Clean data or unified cache line by set/way
+	ADD     R1, R1, #1 << LINE_FIELD_OFFSET     @ ; increment the index
+	CMP     R1, #1 << LINE_FIELD_OVERFLOW       @ ; look for overflow out of set field
+	BNE     _cl_nextLine
+	ADD     R0, R0, #1 << WAY_FIELD_OFFSET      @ ; increment the way number
+	CMP     R0, #0                              @ ; look for overflow out of way field
+	BNE     _cl_nextWay
+
+	DSB                             @ ; synchronise
+	MOV PC, LR
+UNWIND(	.fnend)
+END_FUNC arm_cl1_d_cleanbysetway
+
+FUNC arm_cl1_d_invbysetway , :
+UNWIND(	.fnstart)
+
+	MOV     R0, #0                  @ ; write the Cache Size selection register to be
+	MCR     p15, 2, R0, c0, c0, 0   @ ; sure we address the data cache
+	ISB                             @ ; ISB to sync the change to the CacheSizeID reg
+
+_inv_dcache_off:    
+	MOV     R0, #0                  @ ; set way number to 0
+_inv_nextWay:
+	MOV     R1, #0                  @ ; set line number (=index) to 0
+_inv_nextLine:
+	ORR     R2, R0, R1                          @ ; construct way/index value
+	MCR     p15, 0, R2, c7, c6, 2               @ ; DCISW Invalidate data or unified cache line by set/way
+	ADD     R1, R1, #1 << LINE_FIELD_OFFSET     @ ; increment the index
+	CMP     R1, #1 << LINE_FIELD_OVERFLOW       @ ; look for overflow out of set field
+	BNE     _inv_nextLine
+	ADD     R0, R0, #1 << WAY_FIELD_OFFSET      @ ; increment the way number
+	CMP     R0, #0                              @ ; look for overflow out of way field
+	BNE     _inv_nextWay
+
+	DSB                             @ ; synchronise
+	MOV      PC, LR
+UNWIND(	.fnend)
+END_FUNC arm_cl1_d_invbysetway
+
+FUNC arm_cl1_d_cleaninvbysetway , :
+UNWIND(	.fnstart)
+
+	MOV     R0, #0                  @ ; write the Cache Size selection register to be
+	MCR     p15, 2, R0, c0, c0, 0   @ ; sure we address the data cache
+	ISB                             @ ; ISB to sync the change to the CacheSizeID reg
+
+	MOV     R0, #0                  @ ; set way number to 0
+_cli_nextWay:
+	MOV     R1, #0                  @ ; set line number (=index) to 0
+_cli_nextLine:
+	ORR     R2, R0, R1                          @ ; construct way/index value
+	MCR     p15, 0, R2, c7, c14, 2              @ ; DCCISW Clean and Invalidate data or unified cache line by set/way
+	ADD     R1, R1, #1 << LINE_FIELD_OFFSET     @ ; increment the index
+	CMP     R1, #1 << LINE_FIELD_OVERFLOW       @ ; look for overflow out of set field
+	BNE     _cli_nextLine
+	ADD     R0, R0, #1 << WAY_FIELD_OFFSET      @ ; increment the way number
+	CMP     R0, #0                              @ ; look for overflow out of way field
+	BNE     _cli_nextWay
+
+	DSB                             @ ; synchronise
+	MOV PC, LR
+UNWIND(	.fnend)
+END_FUNC arm_cl1_d_cleaninvbysetway
+
+/*
+ * void arm_cl1_d_cleanbyva(void *s, void *e);
+ */
+FUNC arm_cl1_d_cleanbyva , :
+UNWIND(	.fnstart)
+
+	CMP     R0, R1                  @ ; check that end >= start. Otherwise return.
+	BHI     _cl_area_exit
+
+	MOV     R2, #0                  @ ; write the Cache Size selection register to be
+	MCR     p15, 2, R2, c0, c0, 0   @ ; sure we address the data cache
+	ISB                             @ ; ISB to sync the change to the CacheSizeID reg
+
+	BIC     R0, R0, #0x1F           @ ; Mask 5 LSBits
+_cl_area_nextLine:
+	MCR     p15, 0, R0, c7, c10, 1              @ ; Clean data or unified cache line by MVA to PoC
+	ADD     R0, R0, #1 << LINE_FIELD_OFFSET     @ ; Next cache line
+	CMP     R1, R0
+	BPL     _cl_area_nextLine
+
+_cl_area_exit:
+	
+	DSB                             @ ; synchronise
+	MOV PC, LR
+UNWIND(	.fnend)
+END_FUNC arm_cl1_d_cleanbyva
+
+/*
+ * void arm_cl1_d_invbyva(void *s, void *e);
+ */
+FUNC arm_cl1_d_invbyva , :
+UNWIND(	.fnstart)
+
+	CMP     R0, R1                      @ ; check that end >= start. Otherwise return.
+	BHI     _inv_area_dcache_exit
+	
+	MOV     R2, #0                      @ ; write the Cache Size selection register to be
+	MCR     p15, 2, R2, c0, c0, 0       @ ; sure we address the data cache
+	ISB                                 @ ; ISB to sync the change to the CacheSizeID reg
+
+_inv_area_dcache_off:
+	BIC     R0, R0, #0x1F                       @ ; Mask 5 LSBits
+_inv_area_dcache_nl:
+	MCR     p15, 0, R0, c7, c6, 1               @ ; Invalidate data or unified cache line by MVA to PoC
+	ADD     R0, R0, #1 << LINE_FIELD_OFFSET     @ ; Next cache line
+	CMP     R1, R0
+	BPL     _inv_area_dcache_nl
+
+_inv_area_dcache_exit:
+	DSB
+	MOV PC, LR
+UNWIND(	.fnend)
+END_FUNC arm_cl1_d_invbyva
+
+/*
+ * void arm_cl1_d_cleaninvbyva(void *s, void *e);
+ */
+FUNC arm_cl1_d_cleaninvbyva , :
+UNWIND(	.fnstart)
+
+	CMP     R0, R1                  @ ; check that end >= start. Otherwise return.
+	BHI     _cli_area_exit
+
+	MOV     R2, #0                  @ ; write the Cache Size selection register to be
+	MCR     p15, 2, R2, c0, c0, 0   @ ; sure we address the data cache
+	ISB                             @ ; ISB to sync the change to the CacheSizeID reg
+
+	BIC     R0, R0, #0x1F           @ ; Mask 5 LSBits
+_cli_area_nextLine:
+	MCR     p15, 0, R0, c7, c14, 1              @ ; Clean and Invalidate data or unified cache line by MVA to PoC
+	ADD     R0, R0, #1 << LINE_FIELD_OFFSET     @ ; Next cache line
+	CMP     R1, R0
+	BPL     _cli_area_nextLine
+
+_cli_area_exit:
+	DSB                             @ ; synchronise
+	MOV PC, LR
+UNWIND(	.fnend)
+END_FUNC arm_cl1_d_cleaninvbyva
+
+/*
+ * void arm_cl1_i_inv_all( void );
+ *
+ * Invalidates the whole instruction cache.
+ * It also invalidates the BTAC.
+ */
+FUNC arm_cl1_i_inv_all , :
+UNWIND(	.fnstart)
+
+    /* Invalidate Entire Instruction Cache */
+    write_icialluis
+    DSB
+
+    /* Flush entire branch target cache */
+    write_bpiallis
+
+    DSB                             /* ensure that maintenance operations are seen */
+    ISB                             /* by the instructions rigth after the ISB */
+
+    BX      LR
+UNWIND(	.fnend)
+END_FUNC arm_cl1_i_inv_all
+
+/*
+ * void arm_cl1_i_inv(void *start, void *end);
+ *
+ * Invalidates instruction cache area whose limits are given in parameters.
+ * It also invalidates the BTAC.
+ */
+FUNC arm_cl1_i_inv , :
+UNWIND(	.fnstart)
+
+    CMP     R0, R1                              /* Check that end >= start. Otherwise return. */
+    BHI     _inv_icache_exit
+
+    BIC     R0, R0, #0x1F                       /* Mask 5 LSBits */
+_inv_icache_nextLine:
+    MCR     p15, 0, R0, c7, c5, 1               /* Invalidate ICache single entry (MVA) */
+    ADD     R0, R0, #1 << LINE_FIELD_OFFSET     /* Next cache line */
+    CMP     R1, R0
+    BPL     _inv_icache_nextLine
+    DSB
+
+    /* Flush entire branch target cache */
+    MOV     R1, #0
+    MCR     p15, 0, R1, c7, c5, 6   /* write to Cache operations register */
+    DSB                             /* ensure that maintenance operations are seen */
+    ISB                             /* by the instructions rigth after the ISB */
+
+_inv_icache_exit:
+    BX      LR
+UNWIND(	.fnend)
+END_FUNC arm_cl1_i_inv
diff --git a/core/arch/arm/kernel/ssvce_a64.S b/core/arch/arm/kernel/ssvce_a64.S
new file mode 100644
index 0000000..6c9bbac
--- /dev/null
+++ b/core/arch/arm/kernel/ssvce_a64.S
@@ -0,0 +1,115 @@
+/*
+ * 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/tz_ssvce.h>
+#include <arm64.h>
+#include <asm.S>
+
+/* void secure_mmu_unifiedtlbinvall(void); */
+FUNC secure_mmu_unifiedtlbinvall , :
+	tlbi	vmalle1
+	isb
+	ret
+END_FUNC secure_mmu_unifiedtlbinvall
+
+/* void secure_mmu_unifiedtlbinv_curasid(void) */
+FUNC secure_mmu_unifiedtlbinv_curasid , :
+	mrs	x0, ttbr0_el1
+	lsr	x0, x0, #TTBR_ASID_SHIFT
+	b	secure_mmu_unifiedtlbinv_byasid
+END_FUNC secure_mmu_unifiedtlbinv_curasid
+
+/* void secure_mmu_unifiedtlbinv_byasid(unsigned int asid); */
+FUNC secure_mmu_unifiedtlbinv_byasid , :
+	and	x0, x0, #TTBR_ASID_MASK
+	tlbi	aside1, x0
+	isb
+	ret
+END_FUNC secure_mmu_unifiedtlbinv_byasid
+
+/*
+ * Compatibility wrappers to be used while the rest of the code stops caring
+ * about which cache level it operates on. CL1 -> Inner cache.
+ */
+
+/* void arm_cl1_d_cleanbysetway(void); */
+FUNC arm_cl1_d_cleanbysetway , :
+	mov	x0, #DCCSW
+	b	dcsw_op_all
+END_FUNC arm_cl1_d_cleanbysetway
+
+/* void arm_cl1_d_invbysetway(void); */
+FUNC arm_cl1_d_invbysetway , :
+	mov	x0, #DCISW
+	b	dcsw_op_all
+END_FUNC arm_cl1_d_invbysetway
+
+/* void arm_cl1_d_cleaninvbysetway(void); */
+FUNC arm_cl1_d_cleaninvbysetway , :
+	mov	x0, #DCCISW
+	b	dcsw_op_all
+END_FUNC arm_cl1_d_cleaninvbysetway
+
+/* void arm_cl1_d_cleanbyva(void *s, void *e); */
+FUNC arm_cl1_d_cleanbyva , :
+	sub	x1, x1, x0
+	add	x1, x1, #1
+	/*
+	 * flush_dcache_range() does Clean+Invalidate, but that shouldn't
+	 * matter to the caller.
+	 */
+	b	flush_dcache_range
+END_FUNC arm_cl1_d_cleanbyva
+
+/* void arm_cl1_d_invbyva(void *s, void *e); */
+FUNC arm_cl1_d_invbyva , :
+	sub	x1, x1, x0
+	add	x1, x1, #1
+	b	inv_dcache_range
+END_FUNC arm_cl1_d_invbyva
+
+/* void arm_cl1_d_cleaninvbyva(void *s, void *e); */
+FUNC arm_cl1_d_cleaninvbyva , :
+	sub	x1, x1, x0
+	add	x1, x1, #1
+	b	flush_dcache_range
+END_FUNC arm_cl1_d_cleaninvbyva
+
+/* void arm_cl1_i_inv_all( void ); */
+FUNC arm_cl1_i_inv_all , :
+	ic	ialluis
+	isb
+	ret
+END_FUNC arm_cl1_i_inv_all
+
+/* void arm_cl1_i_inv(void *start, void *end); */
+FUNC arm_cl1_i_inv , :
+	/*
+	 * Invalidate the entire icache instead, it shouldn't matter to the
+	 * caller.
+	 */
+	b	arm_cl1_i_inv_all
+END_FUNC arm_cl1_i_inv
diff --git a/core/arch/arm/kernel/sub.mk b/core/arch/arm/kernel/sub.mk
new file mode 100644
index 0000000..cee3aee
--- /dev/null
+++ b/core/arch/arm/kernel/sub.mk
@@ -0,0 +1,45 @@
+srcs-$(CFG_WITH_USER_TA) += user_ta.c
+srcs-y += pseudo_ta.c
+srcs-y += elf_load.c
+srcs-y += tee_time.c
+
+srcs-$(CFG_SECURE_TIME_SOURCE_CNTPCT) += tee_time_arm_cntpct.c
+srcs-$(CFG_SECURE_TIME_SOURCE_REE) += tee_time_ree.c
+
+srcs-$(CFG_ARM32_core) += proc_a32.S
+srcs-$(CFG_ARM32_core) += spin_lock_a32.S
+srcs-$(CFG_ARM64_core) += proc_a64.S
+srcs-$(CFG_ARM64_core) += spin_lock_a64.S
+srcs-$(CFG_TEE_CORE_DEBUG) += spin_lock_debug.c
+srcs-$(CFG_ARM32_core) += ssvce_a32.S
+srcs-$(CFG_ARM64_core) += ssvce_a64.S
+srcs-$(CFG_ARM64_core) += cache_helpers_a64.S
+srcs-$(CFG_PL310) += tz_ssvce_pl310_a32.S
+srcs-$(CFG_PL310) += tee_l2cc_mutex.c
+
+srcs-$(CFG_ARM32_core) += thread_a32.S
+srcs-$(CFG_ARM64_core) += thread_a64.S
+srcs-y += thread.c
+srcs-y += abort.c
+srcs-$(CFG_WITH_VFP) += vfp.c
+ifeq ($(CFG_WITH_VFP),y)
+srcs-$(CFG_ARM32_core) += vfp_a32.S
+srcs-$(CFG_ARM64_core) += vfp_a64.S
+endif
+srcs-y += trace_ext.c
+srcs-$(CFG_ARM32_core) += misc_a32.S
+srcs-$(CFG_ARM64_core) += misc_a64.S
+srcs-y += mutex.c
+srcs-y += wait_queue.c
+srcs-$(CFG_PM_STUBS) += pm_stubs.c
+
+srcs-$(CFG_GENERIC_BOOT) += generic_boot.c
+ifeq ($(CFG_GENERIC_BOOT),y)
+srcs-$(CFG_ARM32_core) += generic_entry_a32.S
+srcs-$(CFG_ARM64_core) += generic_entry_a64.S
+endif
+
+ifeq ($(CFG_CORE_UNWIND),y)
+srcs-$(CFG_ARM32_core) += unwind_arm32.c
+srcs-$(CFG_ARM64_core) += unwind_arm64.c
+endif
diff --git a/core/arch/arm/kernel/tee_l2cc_mutex.c b/core/arch/arm/kernel/tee_l2cc_mutex.c
new file mode 100644
index 0000000..2afda4d
--- /dev/null
+++ b/core/arch/arm/kernel/tee_l2cc_mutex.c
@@ -0,0 +1,160 @@
+/*
+ * Copyright (c) 2014, STMicroelectronics International N.V.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+#include <kernel/tee_common.h>
+#include <kernel/tee_l2cc_mutex.h>
+#include <kernel/spinlock.h>
+#include <mm/tee_mm.h>
+#include <mm/core_memprot.h>
+#include <mm/core_mmu.h>
+#include <tee_api_defines.h>
+#include <trace.h>
+
+/*
+ * l2cc_mutex_va holds teecore virtual address of TZ L2CC mutex or NULL.
+ *
+ * l2cc_mutex_pa holds TZ L2CC mutex physical address. It is relevant only
+ * if 'l2cc_mutex_va' hold a non-NULL address.
+ */
+#define MUTEX_SZ sizeof(uint32_t)
+
+static uint32_t *l2cc_mutex_va;
+static uint32_t l2cc_mutex_pa;
+static uint32_t l2cc_mutex_boot_pa;
+static unsigned int *l2cc_mutex;
+
+void tee_l2cc_store_mutex_boot_pa(uint32_t pa)
+{
+	l2cc_mutex_boot_pa = pa;
+}
+
+/*
+ * Allocate public RAM to get a L2CC mutex to shared with NSec.
+ * Return 0 on success.
+ */
+static int l2cc_mutex_alloc(void)
+{
+	void *va;
+
+	if (l2cc_mutex_va != NULL)
+		return -1;
+
+	l2cc_mutex_pa = l2cc_mutex_boot_pa;
+
+	va = phys_to_virt(l2cc_mutex_pa, MEM_AREA_NSEC_SHM);
+	if (!va)
+		return -1;
+
+	*(uint32_t *)va = 0;
+	l2cc_mutex_va = va;
+	return 0;
+}
+
+static void l2cc_mutex_set(void *mutex)
+{
+	l2cc_mutex = (unsigned int *)mutex;
+}
+
+/*
+ * tee_xxx_l2cc_mutex():  Handle L2 mutex configuration requests from NSec
+ *
+ * Policy:
+ * - if NSec did not register a L2 mutex, default allocate it in public RAM.
+ * - if NSec disables L2 mutex, disable the current mutex and unregister it.
+ *
+ * Enable L2CC: NSec allows teecore to run safe outer maintance
+ *		with shared mutex.
+ * Disable L2CC: NSec will run outer maintenance with locking
+ *               shared mutex. teecore cannot run outer maintenance.
+ * Set L2CC: NSec proposes a Shared Memory locaiotn for the outer
+ *           maintenance shared mutex.
+ * Get L2CC: NSec requests the outer maintenance shared mutex
+ *           location. If NSec has successufully registered one,
+ *           return its location, otherwise, allocated one in NSec
+ *           and provided NSec the physical location.
+ */
+TEE_Result tee_enable_l2cc_mutex(void)
+{
+	int ret;
+
+	if (!l2cc_mutex_va) {
+		ret = l2cc_mutex_alloc();
+		if (ret)
+			return TEE_ERROR_GENERIC;
+	}
+	l2cc_mutex_set(l2cc_mutex_va);
+	return TEE_SUCCESS;
+}
+
+TEE_Result tee_disable_l2cc_mutex(void)
+{
+	l2cc_mutex_va = NULL;
+	l2cc_mutex_set(NULL);
+	return TEE_SUCCESS;
+}
+
+TEE_Result tee_get_l2cc_mutex(paddr_t *mutex)
+{
+	int ret;
+
+	if (!l2cc_mutex_va) {
+		ret = l2cc_mutex_alloc();
+		if (ret)
+			return TEE_ERROR_GENERIC;
+	}
+	*mutex = l2cc_mutex_pa;
+	return TEE_SUCCESS;
+}
+
+TEE_Result tee_set_l2cc_mutex(paddr_t *mutex)
+{
+	uint32_t addr;
+	void *va;
+
+	if (l2cc_mutex_va != NULL)
+		return TEE_ERROR_BAD_PARAMETERS;
+	addr = *mutex;
+	if (core_pbuf_is(CORE_MEM_NSEC_SHM, addr, MUTEX_SZ) == false)
+		return TEE_ERROR_BAD_PARAMETERS;
+	va = phys_to_virt(addr, MEM_AREA_NSEC_SHM);
+	if (!va)
+		return TEE_ERROR_BAD_PARAMETERS;
+	l2cc_mutex_pa = addr;
+	l2cc_mutex_va = va;
+	return TEE_SUCCESS;
+}
+
+void tee_l2cc_mutex_lock(void)
+{
+	if (l2cc_mutex)
+		cpu_spin_lock(l2cc_mutex);
+}
+
+void tee_l2cc_mutex_unlock(void)
+{
+	if (l2cc_mutex)
+		cpu_spin_unlock(l2cc_mutex);
+}
diff --git a/core/arch/arm/kernel/tee_time.c b/core/arch/arm/kernel/tee_time.c
new file mode 100644
index 0000000..671a8e9
--- /dev/null
+++ b/core/arch/arm/kernel/tee_time.c
@@ -0,0 +1,83 @@
+/*
+ * Copyright (c) 2016, Linaro Limied
+ * 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 <compiler.h>
+#include <string.h>
+#include <stdlib.h>
+
+#include <kernel/tee_time.h>
+#include <kernel/time_source.h>
+#include <kernel/thread.h>
+#include <optee_msg.h>
+#include <mm/core_mmu.h>
+
+struct time_source _time_source;
+
+TEE_Result tee_time_get_sys_time(TEE_Time *time)
+{
+	return _time_source.get_sys_time(time);
+}
+
+uint32_t tee_time_get_sys_time_protection_level(void)
+{
+	return _time_source.protection_level;
+}
+
+void tee_time_wait(uint32_t milliseconds_delay)
+{
+	struct optee_msg_param params;
+
+	memset(&params, 0, sizeof(params));
+	params.attr = OPTEE_MSG_ATTR_TYPE_VALUE_INPUT;
+	params.u.value.a = milliseconds_delay;
+	thread_rpc_cmd(OPTEE_MSG_RPC_CMD_SUSPEND, 1, &params);
+}
+
+/*
+ * tee_time_get_ree_time(): this function implements the GP Internal API
+ * function TEE_GetREETime()
+ * Goal is to get the time of the Rich Execution Environment
+ * This is why this time is provided through the supplicant
+ */
+TEE_Result tee_time_get_ree_time(TEE_Time *time)
+{
+	TEE_Result res;
+	struct optee_msg_param params;
+
+	if (!time)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	memset(&params, 0, sizeof(params));
+	params.attr = OPTEE_MSG_ATTR_TYPE_VALUE_OUTPUT;
+	res = thread_rpc_cmd(OPTEE_MSG_RPC_CMD_GET_TIME, 1, &params);
+	if (res == TEE_SUCCESS) {
+		time->seconds = params.u.value.a;
+		time->millis = params.u.value.b / 1000000;
+	}
+
+	return res;
+}
diff --git a/core/arch/arm/kernel/tee_time_arm_cntpct.c b/core/arch/arm/kernel/tee_time_arm_cntpct.c
new file mode 100644
index 0000000..90e7f20
--- /dev/null
+++ b/core/arch/arm/kernel/tee_time_arm_cntpct.c
@@ -0,0 +1,100 @@
+/*
+ * Copyright (c) 2014, 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/misc.h>
+#include <kernel/tee_time.h>
+#include <trace.h>
+#include <kernel/time_source.h>
+#include <mm/core_mmu.h>
+#include <utee_defines.h>
+
+#include <tee/tee_cryp_utl.h>
+
+#include <stdint.h>
+#include <mpa.h>
+#include <arm.h>
+
+static TEE_Result arm_cntpct_get_sys_time(TEE_Time *time)
+{
+	uint64_t cntpct = read_cntpct();
+	uint32_t cntfrq = read_cntfrq();
+
+	time->seconds = cntpct / cntfrq;
+	time->millis = (cntpct % cntfrq) / (cntfrq / TEE_TIME_MILLIS_BASE);
+
+	return TEE_SUCCESS;
+}
+
+static const struct time_source arm_cntpct_time_source = {
+	.name = "arm cntpct",
+	.protection_level = 1000,
+	.get_sys_time = arm_cntpct_get_sys_time,
+};
+
+REGISTER_TIME_SOURCE(arm_cntpct_time_source)
+
+/*
+ * We collect jitter using cntpct in 32- or 64-bit mode that is typically
+ * clocked at around 1MHz.
+ *
+ * The first time we are called, we add low 16 bits of the counter as entropy.
+ *
+ * Subsequently, accumulate 2 low bits each time by:
+ *
+ *  - rotating the accumumlator by 2 bits
+ *  - XORing it in 2-bit chunks with the whole CNTPCT contents
+ *
+ * and adding one byte of entropy when we reach 8 rotated bits.
+ */
+
+void plat_prng_add_jitter_entropy(void)
+{
+	uint64_t tsc = read_cntpct();
+	int bytes = 0, n;
+	static uint8_t first, bits;
+	static uint16_t acc;
+
+	if (!first) {
+		acc = tsc;
+		bytes = 2;
+		first = 1;
+	} else {
+		acc = (acc << 2) | ((acc >> 6) & 3);
+		for (n = 0; n < 64; n += 2)
+			acc ^= (tsc >> n) & 3;
+		bits += 2;
+		if (bits >= 8) {
+			bits = 0;
+			bytes = 1;
+		}
+	}
+	if (bytes) {
+		DMSG("%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/tee_time_ree.c b/core/arch/arm/kernel/tee_time_ree.c
new file mode 100644
index 0000000..d2a9bb1
--- /dev/null
+++ b/core/arch/arm/kernel/tee_time_ree.c
@@ -0,0 +1,62 @@
+/*
+ * Copyright (c) 2014, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <kernel/tee_time.h>
+#include <kernel/time_source.h>
+#include <kernel/mutex.h>
+
+static TEE_Time prev;
+
+static struct mutex time_mu = MUTEX_INITIALIZER;
+
+static TEE_Result get_monotonic_ree_time(TEE_Time *time)
+{
+	TEE_Result res;
+
+	res = tee_time_get_ree_time(time);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	mutex_lock(&time_mu);
+	if (time->seconds < prev.seconds ||
+		(time->seconds == prev.seconds &&
+		 time->millis < prev.millis))
+		*time = prev; /* REE time was rolled back */
+	else
+		prev = *time;
+	mutex_unlock(&time_mu);
+
+	return res;
+}
+
+static const struct time_source ree_time_source = {
+	.name = "ree",
+	.protection_level = 100,
+	.get_sys_time = get_monotonic_ree_time,
+};
+
+REGISTER_TIME_SOURCE(ree_time_source)
diff --git a/core/arch/arm/kernel/thread.c b/core/arch/arm/kernel/thread.c
new file mode 100644
index 0000000..c988b65
--- /dev/null
+++ b/core/arch/arm/kernel/thread.c
@@ -0,0 +1,1365 @@
+/*
+ * Copyright (c) 2016, Linaro Limited
+ * 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 <arm.h>
+#include <assert.h>
+#include <keep.h>
+#include <kernel/misc.h>
+#include <kernel/panic.h>
+#include <kernel/spinlock.h>
+#include <kernel/tee_ta_manager.h>
+#include <kernel/thread_defs.h>
+#include <kernel/thread.h>
+#include <mm/core_memprot.h>
+#include <mm/tee_mm.h>
+#include <mm/tee_mmu.h>
+#include <mm/tee_pager.h>
+#include <optee_msg.h>
+#include <sm/optee_smc.h>
+#include <sm/sm.h>
+#include <tee/tee_fs_rpc.h>
+#include <tee/tee_cryp_utl.h>
+#include <trace.h>
+#include <util.h>
+
+#include "thread_private.h"
+
+#ifdef CFG_WITH_ARM_TRUSTED_FW
+#define STACK_TMP_OFFS		0
+#else
+#define STACK_TMP_OFFS		SM_STACK_TMP_RESERVE_SIZE
+#endif
+
+
+#ifdef ARM32
+#ifdef CFG_CORE_SANITIZE_KADDRESS
+#define STACK_TMP_SIZE		(3072 + STACK_TMP_OFFS)
+#else
+#define STACK_TMP_SIZE		(1024 + STACK_TMP_OFFS)
+#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*/
+
+#ifdef ARM64
+#define STACK_TMP_SIZE		(2048 + STACK_TMP_OFFS)
+#define STACK_THREAD_SIZE	8192
+
+#if TRACE_LEVEL > 0
+#define STACK_ABT_SIZE		3072
+#else
+#define STACK_ABT_SIZE		1024
+#endif
+#endif /*ARM64*/
+
+struct thread_ctx threads[CFG_NUM_THREADS];
+
+static struct thread_core_local thread_core_local[CFG_TEE_CORE_NB_CORE];
+
+#ifdef CFG_WITH_STACK_CANARIES
+#ifdef ARM32
+#define STACK_CANARY_SIZE	(4 * sizeof(uint32_t))
+#endif
+#ifdef ARM64
+#define STACK_CANARY_SIZE	(8 * sizeof(uint32_t))
+#endif
+#define START_CANARY_VALUE	0xdededede
+#define END_CANARY_VALUE	0xabababab
+#define GET_START_CANARY(name, stack_num) name[stack_num][0]
+#define GET_END_CANARY(name, stack_num) \
+	name[stack_num][sizeof(name[stack_num]) / sizeof(uint32_t) - 1]
+#else
+#define STACK_CANARY_SIZE	0
+#endif
+
+#define DECLARE_STACK(name, num_stacks, stack_size, linkage) \
+linkage uint32_t name[num_stacks] \
+		[ROUNDUP(stack_size + STACK_CANARY_SIZE, STACK_ALIGNMENT) / \
+		sizeof(uint32_t)] \
+		__attribute__((section(".nozi_stack"), \
+			       aligned(STACK_ALIGNMENT)))
+
+#define STACK_SIZE(stack) (sizeof(stack) - STACK_CANARY_SIZE / 2)
+
+#define GET_STACK(stack) \
+	((vaddr_t)(stack) + STACK_SIZE(stack))
+
+DECLARE_STACK(stack_tmp, CFG_TEE_CORE_NB_CORE, STACK_TMP_SIZE, /* global */);
+DECLARE_STACK(stack_abt, CFG_TEE_CORE_NB_CORE, STACK_ABT_SIZE, static);
+#ifndef CFG_WITH_PAGER
+DECLARE_STACK(stack_thread, CFG_NUM_THREADS, STACK_THREAD_SIZE, static);
+#endif
+
+const uint32_t stack_tmp_stride = sizeof(stack_tmp[0]);
+const uint32_t stack_tmp_offset = STACK_TMP_OFFS + STACK_CANARY_SIZE / 2;
+
+/*
+ * These stack setup info are required by secondary boot cores before they
+ * each locally enable the pager (the mmu). Hence kept in pager sections.
+ */
+KEEP_PAGER(stack_tmp);
+KEEP_PAGER(stack_tmp_stride);
+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_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;
+thread_pm_handler_t thread_cpu_resume_handler_ptr;
+thread_pm_handler_t thread_system_off_handler_ptr;
+thread_pm_handler_t thread_system_reset_handler_ptr;
+
+
+static unsigned int thread_global_lock = SPINLOCK_UNLOCK;
+static bool thread_prealloc_rpc_cache;
+
+static void init_canaries(void)
+{
+#ifdef CFG_WITH_STACK_CANARIES
+	size_t n;
+#define INIT_CANARY(name)						\
+	for (n = 0; n < ARRAY_SIZE(name); n++) {			\
+		uint32_t *start_canary = &GET_START_CANARY(name, n);	\
+		uint32_t *end_canary = &GET_END_CANARY(name, n);	\
+									\
+		*start_canary = START_CANARY_VALUE;			\
+		*end_canary = END_CANARY_VALUE;				\
+		DMSG("#Stack canaries for %s[%zu] with top at %p\n",	\
+			#name, n, (void *)(end_canary - 1));		\
+		DMSG("watch *%p\n", (void *)end_canary);		\
+	}
+
+	INIT_CANARY(stack_tmp);
+	INIT_CANARY(stack_abt);
+#ifndef CFG_WITH_PAGER
+	INIT_CANARY(stack_thread);
+#endif
+#endif/*CFG_WITH_STACK_CANARIES*/
+}
+
+#define CANARY_DIED(stack, loc, n) \
+	do { \
+		EMSG_RAW("Dead canary at %s of '%s[%zu]'", #loc, #stack, n); \
+		panic(); \
+	} while (0)
+
+void thread_check_canaries(void)
+{
+#ifdef CFG_WITH_STACK_CANARIES
+	size_t n;
+
+	for (n = 0; n < ARRAY_SIZE(stack_tmp); n++) {
+		if (GET_START_CANARY(stack_tmp, n) != START_CANARY_VALUE)
+			CANARY_DIED(stack_tmp, start, n);
+		if (GET_END_CANARY(stack_tmp, n) != END_CANARY_VALUE)
+			CANARY_DIED(stack_tmp, end, n);
+	}
+
+	for (n = 0; n < ARRAY_SIZE(stack_abt); n++) {
+		if (GET_START_CANARY(stack_abt, n) != START_CANARY_VALUE)
+			CANARY_DIED(stack_abt, start, n);
+		if (GET_END_CANARY(stack_abt, n) != END_CANARY_VALUE)
+			CANARY_DIED(stack_abt, end, n);
+
+	}
+#ifndef CFG_WITH_PAGER
+	for (n = 0; n < ARRAY_SIZE(stack_thread); n++) {
+		if (GET_START_CANARY(stack_thread, n) != START_CANARY_VALUE)
+			CANARY_DIED(stack_thread, start, n);
+		if (GET_END_CANARY(stack_thread, n) != END_CANARY_VALUE)
+			CANARY_DIED(stack_thread, end, n);
+	}
+#endif
+#endif/*CFG_WITH_STACK_CANARIES*/
+}
+
+static void lock_global(void)
+{
+	cpu_spin_lock(&thread_global_lock);
+}
+
+static void unlock_global(void)
+{
+	cpu_spin_unlock(&thread_global_lock);
+}
+
+#ifdef ARM32
+uint32_t thread_get_exceptions(void)
+{
+	uint32_t cpsr = read_cpsr();
+
+	return (cpsr >> CPSR_F_SHIFT) & THREAD_EXCP_ALL;
+}
+
+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))
+		assert_have_no_spinlock();
+
+	cpsr &= ~(THREAD_EXCP_ALL << CPSR_F_SHIFT);
+	cpsr |= ((exceptions & THREAD_EXCP_ALL) << CPSR_F_SHIFT);
+	write_cpsr(cpsr);
+}
+#endif /*ARM32*/
+
+#ifdef ARM64
+uint32_t thread_get_exceptions(void)
+{
+	uint32_t daif = read_daif();
+
+	return (daif >> DAIF_F_SHIFT) & THREAD_EXCP_ALL;
+}
+
+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))
+		assert_have_no_spinlock();
+
+	daif &= ~(THREAD_EXCP_ALL << DAIF_F_SHIFT);
+	daif |= ((exceptions & THREAD_EXCP_ALL) << DAIF_F_SHIFT);
+	write_daif(daif);
+}
+#endif /*ARM64*/
+
+uint32_t thread_mask_exceptions(uint32_t exceptions)
+{
+	uint32_t state = thread_get_exceptions();
+
+	thread_set_exceptions(state | (exceptions & THREAD_EXCP_ALL));
+	return state;
+}
+
+void thread_unmask_exceptions(uint32_t state)
+{
+	thread_set_exceptions(state & THREAD_EXCP_ALL);
+}
+
+
+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
+	 * middle of this function.
+	 */
+	assert(thread_get_exceptions() & THREAD_EXCP_IRQ);
+
+	assert(cpu_id < CFG_TEE_CORE_NB_CORE);
+	return &thread_core_local[cpu_id];
+}
+
+static void thread_lazy_save_ns_vfp(void)
+{
+#ifdef CFG_WITH_VFP
+	struct thread_ctx *thr = threads + thread_get_id();
+
+	thr->vfp_state.ns_saved = false;
+#if defined(ARM64) && defined(CFG_WITH_ARM_TRUSTED_FW)
+	/*
+	 * ARM TF saves and restores CPACR_EL1, so we must assume NS world
+	 * uses VFP and always preserve the register file when secure world
+	 * is about to use it
+	 */
+	thr->vfp_state.ns.force_save = true;
+#endif
+	vfp_lazy_save_state_init(&thr->vfp_state.ns);
+#endif /*CFG_WITH_VFP*/
+}
+
+static void thread_lazy_restore_ns_vfp(void)
+{
+#ifdef CFG_WITH_VFP
+	struct thread_ctx *thr = threads + thread_get_id();
+	struct thread_user_vfp_state *tuv = thr->vfp_state.uvfp;
+
+	assert(!thr->vfp_state.sec_lazy_saved && !thr->vfp_state.sec_saved);
+
+	if (tuv && tuv->lazy_saved && !tuv->saved) {
+		vfp_lazy_save_state_final(&tuv->vfp);
+		tuv->saved = true;
+	}
+
+	vfp_lazy_restore_state(&thr->vfp_state.ns, thr->vfp_state.ns_saved);
+	thr->vfp_state.ns_saved = false;
+#endif /*CFG_WITH_VFP*/
+}
+
+#ifdef ARM32
+static void init_regs(struct thread_ctx *thread,
+		struct thread_smc_args *args)
+{
+	thread->regs.pc = (uint32_t)thread_std_smc_entry;
+
+	/*
+	 * Stdcalls starts in SVC mode with masked IRQ, masked Asynchronous
+	 * abort and unmasked FIQ.
+	  */
+	thread->regs.cpsr = read_cpsr() & ARM32_CPSR_E;
+	thread->regs.cpsr |= CPSR_MODE_SVC | CPSR_I | CPSR_A;
+	/* Enable thumb mode if it's a thumb instruction */
+	if (thread->regs.pc & 1)
+		thread->regs.cpsr |= CPSR_T;
+	/* Reinitialize stack pointer */
+	thread->regs.svc_sp = thread->stack_va_end;
+
+	/*
+	 * Copy arguments into context. This will make the
+	 * arguments appear in r0-r7 when thread is started.
+	 */
+	thread->regs.r0 = args->a0;
+	thread->regs.r1 = args->a1;
+	thread->regs.r2 = args->a2;
+	thread->regs.r3 = args->a3;
+	thread->regs.r4 = args->a4;
+	thread->regs.r5 = args->a5;
+	thread->regs.r6 = args->a6;
+	thread->regs.r7 = args->a7;
+}
+#endif /*ARM32*/
+
+#ifdef ARM64
+static void init_regs(struct thread_ctx *thread,
+		struct thread_smc_args *args)
+{
+	thread->regs.pc = (uint64_t)thread_std_smc_entry;
+
+	/*
+	 * Stdcalls starts in SVC mode with masked IRQ, masked Asynchronous
+	 * abort and unmasked FIQ.
+	  */
+	thread->regs.cpsr = SPSR_64(SPSR_64_MODE_EL1, SPSR_64_MODE_SP_EL0,
+				    DAIFBIT_IRQ | DAIFBIT_ABT);
+	/* Reinitialize stack pointer */
+	thread->regs.sp = thread->stack_va_end;
+
+	/*
+	 * Copy arguments into context. This will make the
+	 * arguments appear in x0-x7 when thread is started.
+	 */
+	thread->regs.x[0] = args->a0;
+	thread->regs.x[1] = args->a1;
+	thread->regs.x[2] = args->a2;
+	thread->regs.x[3] = args->a3;
+	thread->regs.x[4] = args->a4;
+	thread->regs.x[5] = args->a5;
+	thread->regs.x[6] = args->a6;
+	thread->regs.x[7] = args->a7;
+
+	/* Set up frame pointer as per the Aarch64 AAPCS */
+	thread->regs.x[29] = 0;
+}
+#endif /*ARM64*/
+
+void thread_init_boot_thread(void)
+{
+	struct thread_core_local *l = thread_get_core_local();
+	size_t n;
+
+	for (n = 0; n < CFG_NUM_THREADS; n++) {
+		TAILQ_INIT(&threads[n].mutexes);
+		TAILQ_INIT(&threads[n].tsd.sess_stack);
+#ifdef CFG_SMALL_PAGE_USER_TA
+		SLIST_INIT(&threads[n].tsd.pgt_cache);
+#endif
+	}
+
+	for (n = 0; n < CFG_TEE_CORE_NB_CORE; n++)
+		thread_core_local[n].curr_thread = -1;
+
+	l->curr_thread = 0;
+	threads[0].state = THREAD_STATE_ACTIVE;
+}
+
+void thread_clr_boot_thread(void)
+{
+	struct thread_core_local *l = thread_get_core_local();
+
+	assert(l->curr_thread >= 0 && l->curr_thread < CFG_NUM_THREADS);
+	assert(threads[l->curr_thread].state == THREAD_STATE_ACTIVE);
+	assert(TAILQ_EMPTY(&threads[l->curr_thread].mutexes));
+	threads[l->curr_thread].state = THREAD_STATE_FREE;
+	l->curr_thread = -1;
+}
+
+static void thread_alloc_and_run(struct thread_smc_args *args)
+{
+	size_t n;
+	struct thread_core_local *l = thread_get_core_local();
+	bool found_thread = false;
+
+	assert(l->curr_thread == -1);
+
+	lock_global();
+
+	for (n = 0; n < CFG_NUM_THREADS; n++) {
+		if (threads[n].state == THREAD_STATE_FREE) {
+			threads[n].state = THREAD_STATE_ACTIVE;
+			found_thread = true;
+			break;
+		}
+	}
+
+	unlock_global();
+
+	if (!found_thread) {
+		args->a0 = OPTEE_SMC_RETURN_ETHREAD_LIMIT;
+		return;
+	}
+
+	l->curr_thread = n;
+
+	threads[n].flags = 0;
+	init_regs(threads + n, args);
+
+	/* Save Hypervisor Client ID */
+	threads[n].hyp_clnt_id = args->a7;
+
+	thread_lazy_save_ns_vfp();
+	thread_resume(&threads[n].regs);
+}
+
+#ifdef ARM32
+static void copy_a0_to_a5(struct thread_ctx_regs *regs,
+		struct thread_smc_args *args)
+{
+	/*
+	 * Update returned values from RPC, values will appear in
+	 * r0-r3 when thread is resumed.
+	 */
+	regs->r0 = args->a0;
+	regs->r1 = args->a1;
+	regs->r2 = args->a2;
+	regs->r3 = args->a3;
+	regs->r4 = args->a4;
+	regs->r5 = args->a5;
+}
+#endif /*ARM32*/
+
+#ifdef ARM64
+static void copy_a0_to_a5(struct thread_ctx_regs *regs,
+		struct thread_smc_args *args)
+{
+	/*
+	 * Update returned values from RPC, values will appear in
+	 * x0-x3 when thread is resumed.
+	 */
+	regs->x[0] = args->a0;
+	regs->x[1] = args->a1;
+	regs->x[2] = args->a2;
+	regs->x[3] = args->a3;
+	regs->x[4] = args->a4;
+	regs->x[5] = args->a5;
+}
+#endif /*ARM64*/
+
+#ifdef ARM32
+static bool is_from_user(uint32_t cpsr)
+{
+	return (cpsr & ARM32_CPSR_MODE_MASK) == ARM32_CPSR_MODE_USR;
+}
+#endif
+
+#ifdef ARM64
+static bool is_from_user(uint32_t cpsr)
+{
+	if (cpsr & (SPSR_MODE_RW_32 << SPSR_MODE_RW_SHIFT))
+		return true;
+	if (((cpsr >> SPSR_64_MODE_EL_SHIFT) & SPSR_64_MODE_EL_MASK) ==
+	     SPSR_64_MODE_EL0)
+		return true;
+	return false;
+}
+#endif
+
+static bool is_user_mode(struct thread_ctx_regs *regs)
+{
+	return is_from_user((uint32_t)regs->cpsr);
+}
+
+static void thread_resume_from_rpc(struct thread_smc_args *args)
+{
+	size_t n = args->a3; /* thread id */
+	struct thread_core_local *l = thread_get_core_local();
+	uint32_t rv = 0;
+
+	assert(l->curr_thread == -1);
+
+	lock_global();
+
+	if (n < CFG_NUM_THREADS &&
+	    threads[n].state == THREAD_STATE_SUSPENDED &&
+	    args->a7 == threads[n].hyp_clnt_id)
+		threads[n].state = THREAD_STATE_ACTIVE;
+	else
+		rv = OPTEE_SMC_RETURN_ERESUME;
+
+	unlock_global();
+
+	if (rv) {
+		args->a0 = rv;
+		return;
+	}
+
+	l->curr_thread = n;
+
+	if (is_user_mode(&threads[n].regs))
+		tee_ta_update_session_utime_resume();
+
+	if (threads[n].have_user_map)
+		core_mmu_set_user_map(&threads[n].user_map);
+
+	/*
+	 * Return from RPC to request service of an IRQ must not
+	 * get parameters from non-secure world.
+	 */
+	if (threads[n].flags & THREAD_FLAGS_COPY_ARGS_ON_RETURN) {
+		copy_a0_to_a5(&threads[n].regs, args);
+		threads[n].flags &= ~THREAD_FLAGS_COPY_ARGS_ON_RETURN;
+	}
+
+	thread_lazy_save_ns_vfp();
+	thread_resume(&threads[n].regs);
+}
+
+void thread_handle_fast_smc(struct thread_smc_args *args)
+{
+	thread_check_canaries();
+	thread_fast_smc_handler_ptr(args);
+	/* Fast handlers must not unmask any exceptions */
+	assert(thread_get_exceptions() == THREAD_EXCP_ALL);
+}
+
+void thread_handle_std_smc(struct thread_smc_args *args)
+{
+	thread_check_canaries();
+
+	if (args->a0 == OPTEE_SMC_CALL_RETURN_FROM_RPC)
+		thread_resume_from_rpc(args);
+	else
+		thread_alloc_and_run(args);
+}
+
+/* Helper routine for the assembly function thread_std_smc_entry() */
+void __thread_std_smc_entry(struct thread_smc_args *args)
+{
+	struct thread_ctx *thr = threads + thread_get_id();
+
+	if (!thr->rpc_arg) {
+		paddr_t parg;
+		uint64_t carg;
+		void *arg;
+
+		thread_rpc_alloc_arg(
+			OPTEE_MSG_GET_ARG_SIZE(THREAD_RPC_MAX_NUM_PARAMS),
+			&parg, &carg);
+		if (!parg || !ALIGNMENT_IS_OK(parg, struct optee_msg_arg) ||
+		    !(arg = phys_to_virt(parg, MEM_AREA_NSEC_SHM))) {
+			thread_rpc_free_arg(carg);
+			args->a0 = OPTEE_SMC_RETURN_ENOMEM;
+			return;
+		}
+
+		thr->rpc_arg = arg;
+		thr->rpc_carg = carg;
+	}
+
+	thread_std_smc_handler_ptr(args);
+
+	tee_fs_rpc_cache_clear(&thr->tsd);
+	if (!thread_prealloc_rpc_cache) {
+		thread_rpc_free_arg(thr->rpc_carg);
+		thr->rpc_carg = 0;
+		thr->rpc_arg = 0;
+	}
+}
+
+void *thread_get_tmp_sp(void)
+{
+	struct thread_core_local *l = thread_get_core_local();
+
+	return (void *)l->tmp_stack_va_end;
+}
+
+#ifdef ARM64
+vaddr_t thread_get_saved_thread_sp(void)
+{
+	struct thread_core_local *l = thread_get_core_local();
+	int ct = l->curr_thread;
+
+	assert(ct != -1);
+	return threads[ct].kern_sp;
+}
+#endif /*ARM64*/
+
+bool thread_addr_is_in_stack(vaddr_t va)
+{
+	struct thread_ctx *thr;
+	int ct = thread_get_id_may_fail();
+
+	if (ct == -1)
+		return false;
+
+	thr = threads + ct;
+	return va < thr->stack_va_end &&
+	       va >= (thr->stack_va_end - STACK_THREAD_SIZE);
+}
+
+void thread_state_free(void)
+{
+	struct thread_core_local *l = thread_get_core_local();
+	int ct = l->curr_thread;
+
+	assert(ct != -1);
+	assert(TAILQ_EMPTY(&threads[ct].mutexes));
+
+	thread_lazy_restore_ns_vfp();
+	tee_pager_release_phys(
+		(void *)(threads[ct].stack_va_end - STACK_THREAD_SIZE),
+		STACK_THREAD_SIZE);
+
+	lock_global();
+
+	assert(threads[ct].state == THREAD_STATE_ACTIVE);
+	threads[ct].state = THREAD_STATE_FREE;
+	threads[ct].flags = 0;
+	l->curr_thread = -1;
+
+	unlock_global();
+}
+
+#ifdef CFG_WITH_PAGER
+static void release_unused_kernel_stack(struct thread_ctx *thr)
+{
+	vaddr_t sp = thr->regs.svc_sp;
+	vaddr_t base = thr->stack_va_end - STACK_THREAD_SIZE;
+	size_t len = sp - base;
+
+	tee_pager_release_phys((void *)base, len);
+}
+#else
+static void release_unused_kernel_stack(struct thread_ctx *thr __unused)
+{
+}
+#endif
+
+int thread_state_suspend(uint32_t flags, uint32_t cpsr, vaddr_t pc)
+{
+	struct thread_core_local *l = thread_get_core_local();
+	int ct = l->curr_thread;
+
+	assert(ct != -1);
+
+	thread_check_canaries();
+
+	release_unused_kernel_stack(threads + ct);
+
+	if (is_from_user(cpsr)) {
+		thread_user_save_vfp();
+		tee_ta_update_session_utime_suspend();
+		tee_ta_gprof_sample_pc(pc);
+	}
+	thread_lazy_restore_ns_vfp();
+
+	lock_global();
+
+	assert(threads[ct].state == THREAD_STATE_ACTIVE);
+	threads[ct].flags |= flags;
+	threads[ct].regs.cpsr = cpsr;
+	threads[ct].regs.pc = pc;
+	threads[ct].state = THREAD_STATE_SUSPENDED;
+
+	threads[ct].have_user_map = core_mmu_user_mapping_is_active();
+	if (threads[ct].have_user_map) {
+		core_mmu_get_user_map(&threads[ct].user_map);
+		core_mmu_set_user_map(NULL);
+	}
+
+	l->curr_thread = -1;
+
+	unlock_global();
+
+	return ct;
+}
+
+#ifdef ARM32
+static void set_tmp_stack(struct thread_core_local *l, vaddr_t sp)
+{
+	l->tmp_stack_va_end = sp;
+	thread_set_irq_sp(sp);
+	thread_set_fiq_sp(sp);
+}
+
+static void set_abt_stack(struct thread_core_local *l __unused, vaddr_t sp)
+{
+	thread_set_abt_sp(sp);
+}
+#endif /*ARM32*/
+
+#ifdef ARM64
+static void set_tmp_stack(struct thread_core_local *l, vaddr_t sp)
+{
+	/*
+	 * We're already using the tmp stack when this function is called
+	 * so there's no need to assign it to any stack pointer. However,
+	 * we'll need to restore it at different times so store it here.
+	 */
+	l->tmp_stack_va_end = sp;
+}
+
+static void set_abt_stack(struct thread_core_local *l, vaddr_t sp)
+{
+	l->abt_stack_va_end = sp;
+}
+#endif /*ARM64*/
+
+bool thread_init_stack(uint32_t thread_id, vaddr_t sp)
+{
+	if (thread_id >= CFG_NUM_THREADS)
+		return false;
+	threads[thread_id].stack_va_end = sp;
+	return true;
+}
+
+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);
+	struct thread_core_local *l = thread_get_core_local();
+	int ct = l->curr_thread;
+
+	thread_unmask_exceptions(exceptions);
+	return ct;
+}
+
+int thread_get_id(void)
+{
+	int ct = thread_get_id_may_fail();
+
+	assert(ct >= 0 && ct < CFG_NUM_THREADS);
+	return ct;
+}
+
+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_cpu_on_handler_ptr = handlers->cpu_on;
+	thread_cpu_off_handler_ptr = handlers->cpu_off;
+	thread_cpu_suspend_handler_ptr = handlers->cpu_suspend;
+	thread_cpu_resume_handler_ptr = handlers->cpu_resume;
+	thread_system_off_handler_ptr = handlers->system_off;
+	thread_system_reset_handler_ptr = handlers->system_reset;
+}
+
+#ifdef CFG_WITH_PAGER
+static void init_thread_stacks(void)
+{
+	size_t n;
+
+	/*
+	 * Allocate virtual memory for thread stacks.
+	 */
+	for (n = 0; n < CFG_NUM_THREADS; n++) {
+		tee_mm_entry_t *mm;
+		vaddr_t sp;
+
+		/* Find vmem for thread stack and its protection gap */
+		mm = tee_mm_alloc(&tee_mm_vcore,
+				  SMALL_PAGE_SIZE + STACK_THREAD_SIZE);
+		assert(mm);
+
+		/* Claim eventual physical page */
+		tee_pager_add_pages(tee_mm_get_smem(mm), tee_mm_get_size(mm),
+				    true);
+
+		/* Add the area to the pager */
+		tee_pager_add_core_area(tee_mm_get_smem(mm) + SMALL_PAGE_SIZE,
+					tee_mm_get_bytes(mm) - SMALL_PAGE_SIZE,
+					TEE_MATTR_PRW | TEE_MATTR_LOCKED,
+					NULL, NULL);
+
+		/* init effective stack */
+		sp = tee_mm_get_smem(mm) + tee_mm_get_bytes(mm);
+		if (!thread_init_stack(n, sp))
+			panic("init stack failed");
+	}
+}
+#else
+static void init_thread_stacks(void)
+{
+	size_t n;
+
+	/* Assign the thread stacks */
+	for (n = 0; n < CFG_NUM_THREADS; n++) {
+		if (!thread_init_stack(n, GET_STACK(stack_thread[n])))
+			panic("thread_init_stack failed");
+	}
+}
+#endif /*CFG_WITH_PAGER*/
+
+void thread_init_primary(const struct thread_handlers *handlers)
+{
+	init_handlers(handlers);
+
+	/* Initialize canaries around the stacks */
+	init_canaries();
+
+	init_thread_stacks();
+	pgt_init();
+}
+
+static void init_sec_mon(size_t pos __maybe_unused)
+{
+#if !defined(CFG_WITH_ARM_TRUSTED_FW)
+	/* Initialize secure monitor */
+	sm_init(GET_STACK(stack_tmp[pos]));
+#endif
+}
+
+void thread_init_per_cpu(void)
+{
+	size_t pos = get_core_pos();
+	struct thread_core_local *l = thread_get_core_local();
+
+	init_sec_mon(pos);
+
+	set_tmp_stack(l, GET_STACK(stack_tmp[pos]) - STACK_TMP_OFFS);
+	set_abt_stack(l, GET_STACK(stack_abt[pos]));
+
+	thread_init_vbar();
+}
+
+struct thread_specific_data *thread_get_tsd(void)
+{
+	return &threads[thread_get_id()].tsd;
+}
+
+struct thread_ctx_regs *thread_get_ctx_regs(void)
+{
+	struct thread_core_local *l = thread_get_core_local();
+
+	assert(l->curr_thread != -1);
+	return &threads[l->curr_thread].regs;
+}
+
+void thread_set_irq(bool enable)
+{
+	/* thread_get_core_local() requires IRQs to be disabled */
+	uint32_t exceptions = thread_mask_exceptions(THREAD_EXCP_IRQ);
+	struct thread_core_local *l;
+
+	l = thread_get_core_local();
+
+	assert(l->curr_thread != -1);
+
+	if (enable) {
+		threads[l->curr_thread].flags |= THREAD_FLAGS_IRQ_ENABLE;
+		thread_set_exceptions(exceptions & ~THREAD_EXCP_IRQ);
+	} else {
+		/*
+		 * No need to disable IRQ here since it's already disabled
+		 * above.
+		 */
+		threads[l->curr_thread].flags &= ~THREAD_FLAGS_IRQ_ENABLE;
+	}
+}
+
+void thread_restore_irq(void)
+{
+	/* thread_get_core_local() requires IRQs to be disabled */
+	uint32_t exceptions = thread_mask_exceptions(THREAD_EXCP_IRQ);
+	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);
+}
+
+#ifdef CFG_WITH_VFP
+uint32_t thread_kernel_enable_vfp(void)
+{
+	uint32_t exceptions = thread_mask_exceptions(THREAD_EXCP_IRQ);
+	struct thread_ctx *thr = threads + thread_get_id();
+	struct thread_user_vfp_state *tuv = thr->vfp_state.uvfp;
+
+	assert(!vfp_is_enabled());
+
+	if (!thr->vfp_state.ns_saved) {
+		vfp_lazy_save_state_final(&thr->vfp_state.ns);
+		thr->vfp_state.ns_saved = true;
+	} else if (thr->vfp_state.sec_lazy_saved &&
+		   !thr->vfp_state.sec_saved) {
+		/*
+		 * This happens when we're handling an abort while the
+		 * thread was using the VFP state.
+		 */
+		vfp_lazy_save_state_final(&thr->vfp_state.sec);
+		thr->vfp_state.sec_saved = true;
+	} else if (tuv && tuv->lazy_saved && !tuv->saved) {
+		/*
+		 * This can happen either during syscall or abort
+		 * processing (while processing a syscall).
+		 */
+		vfp_lazy_save_state_final(&tuv->vfp);
+		tuv->saved = true;
+	}
+
+	vfp_enable();
+	return exceptions;
+}
+
+void thread_kernel_disable_vfp(uint32_t state)
+{
+	uint32_t exceptions;
+
+	assert(vfp_is_enabled());
+
+	vfp_disable();
+	exceptions = thread_get_exceptions();
+	assert(exceptions & THREAD_EXCP_IRQ);
+	exceptions &= ~THREAD_EXCP_IRQ;
+	exceptions |= state & THREAD_EXCP_IRQ;
+	thread_set_exceptions(exceptions);
+}
+
+void thread_kernel_save_vfp(void)
+{
+	struct thread_ctx *thr = threads + thread_get_id();
+
+	assert(thread_get_exceptions() & THREAD_EXCP_IRQ);
+	if (vfp_is_enabled()) {
+		vfp_lazy_save_state_init(&thr->vfp_state.sec);
+		thr->vfp_state.sec_lazy_saved = true;
+	}
+}
+
+void thread_kernel_restore_vfp(void)
+{
+	struct thread_ctx *thr = threads + thread_get_id();
+
+	assert(thread_get_exceptions() & THREAD_EXCP_IRQ);
+	assert(!vfp_is_enabled());
+	if (thr->vfp_state.sec_lazy_saved) {
+		vfp_lazy_restore_state(&thr->vfp_state.sec,
+				       thr->vfp_state.sec_saved);
+		thr->vfp_state.sec_saved = false;
+		thr->vfp_state.sec_lazy_saved = false;
+	}
+}
+
+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(!vfp_is_enabled());
+
+	if (!thr->vfp_state.ns_saved) {
+		vfp_lazy_save_state_final(&thr->vfp_state.ns);
+		thr->vfp_state.ns_saved = true;
+	} else if (tuv && uvfp != tuv) {
+		if (tuv->lazy_saved && !tuv->saved) {
+			vfp_lazy_save_state_final(&tuv->vfp);
+			tuv->saved = true;
+		}
+	}
+
+	if (uvfp->lazy_saved)
+		vfp_lazy_restore_state(&uvfp->vfp, uvfp->saved);
+	uvfp->lazy_saved = false;
+	uvfp->saved = false;
+
+	thr->vfp_state.uvfp = uvfp;
+	vfp_enable();
+}
+
+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);
+	if (!vfp_is_enabled())
+		return;
+
+	assert(tuv && !tuv->lazy_saved && !tuv->saved);
+	vfp_lazy_save_state_init(&tuv->vfp);
+	tuv->lazy_saved = true;
+}
+
+void thread_user_clear_vfp(struct thread_user_vfp_state *uvfp)
+{
+	struct thread_ctx *thr = threads + thread_get_id();
+
+	if (uvfp == thr->vfp_state.uvfp)
+		thr->vfp_state.uvfp = NULL;
+	uvfp->lazy_saved = false;
+	uvfp->saved = false;
+}
+#endif /*CFG_WITH_VFP*/
+
+#ifdef ARM32
+static bool get_spsr(bool is_32bit, unsigned long entry_func, uint32_t *spsr)
+{
+	uint32_t s;
+
+	if (!is_32bit)
+		return false;
+
+	s = read_spsr();
+	s &= ~(CPSR_MODE_MASK | CPSR_T | CPSR_IT_MASK1 | CPSR_IT_MASK2);
+	s |= CPSR_MODE_USR;
+	if (entry_func & 1)
+		s |= CPSR_T;
+	*spsr = s;
+	return true;
+}
+#endif
+
+#ifdef ARM64
+static bool get_spsr(bool is_32bit, unsigned long entry_func, uint32_t *spsr)
+{
+	uint32_t s;
+
+	if (is_32bit) {
+		s = read_daif() & (SPSR_32_AIF_MASK << SPSR_32_AIF_SHIFT);
+		s |= SPSR_MODE_RW_32 << SPSR_MODE_RW_SHIFT;
+		s |= (entry_func & SPSR_32_T_MASK) << SPSR_32_T_SHIFT;
+	} else {
+		s = read_daif() & (SPSR_64_DAIF_MASK << SPSR_64_DAIF_SHIFT);
+	}
+
+	*spsr = s;
+	return true;
+}
+#endif
+
+uint32_t thread_enter_user_mode(unsigned long a0, unsigned long a1,
+		unsigned long a2, unsigned long a3, unsigned long user_sp,
+		unsigned long entry_func, bool is_32bit,
+		uint32_t *exit_status0, uint32_t *exit_status1)
+{
+	uint32_t spsr;
+
+	tee_ta_update_session_utime_resume();
+
+	if (!get_spsr(is_32bit, entry_func, &spsr)) {
+		*exit_status0 = 1; /* panic */
+		*exit_status1 = 0xbadbadba;
+		return 0;
+	}
+	return __thread_enter_user_mode(a0, a1, a2, a3, user_sp, entry_func,
+					spsr, exit_status0, exit_status1);
+}
+
+void thread_add_mutex(struct mutex *m)
+{
+	struct thread_core_local *l = thread_get_core_local();
+	int ct = l->curr_thread;
+
+	assert(ct != -1 && threads[ct].state == THREAD_STATE_ACTIVE);
+	assert(m->owner_id == -1);
+	m->owner_id = ct;
+	TAILQ_INSERT_TAIL(&threads[ct].mutexes, m, link);
+}
+
+void thread_rem_mutex(struct mutex *m)
+{
+	struct thread_core_local *l = thread_get_core_local();
+	int ct = l->curr_thread;
+
+	assert(ct != -1 && threads[ct].state == THREAD_STATE_ACTIVE);
+	assert(m->owner_id == ct);
+	m->owner_id = -1;
+	TAILQ_REMOVE(&threads[ct].mutexes, m, link);
+}
+
+bool thread_disable_prealloc_rpc_cache(uint64_t *cookie)
+{
+	bool rv;
+	size_t n;
+	uint32_t exceptions = thread_mask_exceptions(THREAD_EXCP_IRQ);
+
+	lock_global();
+
+	for (n = 0; n < CFG_NUM_THREADS; n++) {
+		if (threads[n].state != THREAD_STATE_FREE) {
+			rv = false;
+			goto out;
+		}
+	}
+
+	rv = true;
+	for (n = 0; n < CFG_NUM_THREADS; n++) {
+		if (threads[n].rpc_arg) {
+			*cookie = threads[n].rpc_carg;
+			threads[n].rpc_carg = 0;
+			threads[n].rpc_arg = NULL;
+			goto out;
+		}
+	}
+
+	*cookie = 0;
+	thread_prealloc_rpc_cache = false;
+out:
+	unlock_global();
+	thread_unmask_exceptions(exceptions);
+	return rv;
+}
+
+bool thread_enable_prealloc_rpc_cache(void)
+{
+	bool rv;
+	size_t n;
+	uint32_t exceptions = thread_mask_exceptions(THREAD_EXCP_IRQ);
+
+	lock_global();
+
+	for (n = 0; n < CFG_NUM_THREADS; n++) {
+		if (threads[n].state != THREAD_STATE_FREE) {
+			rv = false;
+			goto out;
+		}
+	}
+
+	rv = true;
+	thread_prealloc_rpc_cache = true;
+out:
+	unlock_global();
+	thread_unmask_exceptions(exceptions);
+	return rv;
+}
+
+static uint32_t rpc_cmd_nolock(uint32_t cmd, size_t num_params,
+		struct optee_msg_param *params)
+{
+	uint32_t rpc_args[THREAD_RPC_NUM_ARGS] = { OPTEE_SMC_RETURN_RPC_CMD };
+	struct thread_ctx *thr = threads + thread_get_id();
+	struct optee_msg_arg *arg = thr->rpc_arg;
+	uint64_t carg = thr->rpc_carg;
+	const size_t params_size = sizeof(struct optee_msg_param) * num_params;
+	size_t n;
+
+	assert(arg && carg && num_params <= THREAD_RPC_MAX_NUM_PARAMS);
+
+	plat_prng_add_jitter_entropy();
+
+	memset(arg, 0, OPTEE_MSG_GET_ARG_SIZE(THREAD_RPC_MAX_NUM_PARAMS));
+	arg->cmd = cmd;
+	arg->ret = TEE_ERROR_GENERIC; /* in case value isn't updated */
+	arg->num_params = num_params;
+	memcpy(OPTEE_MSG_GET_PARAMS(arg), params, params_size);
+
+	reg_pair_from_64(carg, rpc_args + 1, rpc_args + 2);
+	thread_rpc(rpc_args);
+	for (n = 0; n < num_params; n++) {
+		switch (params[n].attr & OPTEE_MSG_ATTR_TYPE_MASK) {
+		case OPTEE_MSG_ATTR_TYPE_VALUE_OUTPUT:
+		case OPTEE_MSG_ATTR_TYPE_VALUE_INOUT:
+		case OPTEE_MSG_ATTR_TYPE_RMEM_OUTPUT:
+		case OPTEE_MSG_ATTR_TYPE_RMEM_INOUT:
+		case OPTEE_MSG_ATTR_TYPE_TMEM_OUTPUT:
+		case OPTEE_MSG_ATTR_TYPE_TMEM_INOUT:
+			memcpy(params + n, OPTEE_MSG_GET_PARAMS(arg) + n,
+			       sizeof(struct optee_msg_param));
+			break;
+		default:
+			break;
+		}
+	}
+	return arg->ret;
+}
+
+uint32_t thread_rpc_cmd(uint32_t cmd, size_t num_params,
+		struct optee_msg_param *params)
+{
+	uint32_t ret;
+
+	ret = rpc_cmd_nolock(cmd, num_params, params);
+
+	return ret;
+}
+
+static bool check_alloced_shm(paddr_t pa, size_t len, size_t align)
+{
+	if (pa & (align - 1))
+		return false;
+	return core_pbuf_is(CORE_MEM_NSEC_SHM, pa, len);
+}
+
+void thread_rpc_free_arg(uint64_t cookie)
+{
+	if (cookie) {
+		uint32_t rpc_args[THREAD_RPC_NUM_ARGS] = {
+			OPTEE_SMC_RETURN_RPC_FREE
+		};
+
+		reg_pair_from_64(cookie, rpc_args + 1, rpc_args + 2);
+		thread_rpc(rpc_args);
+	}
+}
+
+void thread_rpc_alloc_arg(size_t size, paddr_t *arg, uint64_t *cookie)
+{
+	paddr_t pa;
+	uint64_t co;
+	uint32_t rpc_args[THREAD_RPC_NUM_ARGS] = {
+		OPTEE_SMC_RETURN_RPC_ALLOC, size
+	};
+
+	thread_rpc(rpc_args);
+
+	pa = reg_pair_to_64(rpc_args[1], rpc_args[2]);
+	co = reg_pair_to_64(rpc_args[4], rpc_args[5]);
+	if (!check_alloced_shm(pa, size, sizeof(uint64_t))) {
+		thread_rpc_free_arg(co);
+		pa = 0;
+		co = 0;
+	}
+
+	*arg = pa;
+	*cookie = co;
+}
+
+/**
+ * Free physical memory previously allocated with thread_rpc_alloc()
+ *
+ * @cookie:	cookie received when allocating the buffer
+ * @bt:		 must be the same as supplied when allocating
+ */
+static void thread_rpc_free(unsigned int bt, uint64_t cookie)
+{
+	uint32_t rpc_args[THREAD_RPC_NUM_ARGS] = { OPTEE_SMC_RETURN_RPC_CMD };
+	struct thread_ctx *thr = threads + thread_get_id();
+	struct optee_msg_arg *arg = thr->rpc_arg;
+	uint64_t carg = thr->rpc_carg;
+	struct optee_msg_param *params = OPTEE_MSG_GET_PARAMS(arg);
+
+	memset(arg, 0, OPTEE_MSG_GET_ARG_SIZE(1));
+	arg->cmd = OPTEE_MSG_RPC_CMD_SHM_FREE;
+	arg->ret = TEE_ERROR_GENERIC; /* in case value isn't updated */
+	arg->num_params = 1;
+
+	params[0].attr = OPTEE_MSG_ATTR_TYPE_VALUE_INPUT;
+	params[0].u.value.a = bt;
+	params[0].u.value.b = cookie;
+	params[0].u.value.c = 0;
+
+	reg_pair_from_64(carg, rpc_args + 1, rpc_args + 2);
+	thread_rpc(rpc_args);
+}
+
+/**
+ * Allocates shared memory buffer via RPC
+ *
+ * @size:	size in bytes of shared memory buffer
+ * @align:	required alignment of buffer
+ * @bt:		buffer type OPTEE_MSG_RPC_SHM_TYPE_*
+ * @payload:	returned physical pointer to buffer, 0 if allocation
+ *		failed.
+ * @cookie:	returned cookie used when freeing the buffer
+ */
+static void thread_rpc_alloc(size_t size, size_t align, unsigned int bt,
+			paddr_t *payload, uint64_t *cookie)
+{
+	uint32_t rpc_args[THREAD_RPC_NUM_ARGS] = { OPTEE_SMC_RETURN_RPC_CMD };
+	struct thread_ctx *thr = threads + thread_get_id();
+	struct optee_msg_arg *arg = thr->rpc_arg;
+	uint64_t carg = thr->rpc_carg;
+	struct optee_msg_param *params = OPTEE_MSG_GET_PARAMS(arg);
+
+	memset(arg, 0, OPTEE_MSG_GET_ARG_SIZE(1));
+	arg->cmd = OPTEE_MSG_RPC_CMD_SHM_ALLOC;
+	arg->ret = TEE_ERROR_GENERIC; /* in case value isn't updated */
+	arg->num_params = 1;
+
+	params[0].attr = OPTEE_MSG_ATTR_TYPE_VALUE_INPUT;
+	params[0].u.value.a = bt;
+	params[0].u.value.b = size;
+	params[0].u.value.c = align;
+
+	reg_pair_from_64(carg, rpc_args + 1, rpc_args + 2);
+	thread_rpc(rpc_args);
+	if (arg->ret != TEE_SUCCESS)
+		goto fail;
+
+	if (arg->num_params != 1)
+		goto fail;
+
+	if (params[0].attr != OPTEE_MSG_ATTR_TYPE_TMEM_OUTPUT)
+		goto fail;
+
+	if (!check_alloced_shm(params[0].u.tmem.buf_ptr, size, align)) {
+		thread_rpc_free(bt, params[0].u.tmem.shm_ref);
+		goto fail;
+	}
+
+	*payload = params[0].u.tmem.buf_ptr;
+	*cookie = params[0].u.tmem.shm_ref;
+	return;
+fail:
+	*payload = 0;
+	*cookie = 0;
+}
+
+void thread_rpc_alloc_payload(size_t size, paddr_t *payload, uint64_t *cookie)
+{
+	thread_rpc_alloc(size, 8, OPTEE_MSG_RPC_SHM_TYPE_APPL, payload, cookie);
+}
+
+void thread_rpc_free_payload(uint64_t cookie)
+{
+	thread_rpc_free(OPTEE_MSG_RPC_SHM_TYPE_APPL, cookie);
+}
diff --git a/core/arch/arm/kernel/thread_a32.S b/core/arch/arm/kernel/thread_a32.S
new file mode 100644
index 0000000..6d3ac35
--- /dev/null
+++ b/core/arch/arm/kernel/thread_a32.S
@@ -0,0 +1,645 @@
+/*
+ * Copyright (c) 2016, Linaro Limited
+ * 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 <asm.S>
+#include <arm.h>
+#include <arm32_macros.S>
+#include <sm/optee_smc.h>
+#include <sm/teesmc_opteed_macros.h>
+#include <sm/teesmc_opteed.h>
+#include <kernel/abort.h>
+#include <kernel/thread_defs.h>
+#include <kernel/unwind.h>
+
+	.section .text.thread_asm
+
+LOCAL_FUNC vector_std_smc_entry , :
+UNWIND(	.fnstart)
+UNWIND(	.cantunwind)
+	push	{r0-r7}
+	mov	r0, sp
+	bl	thread_handle_std_smc
+	/*
+	 * Normally thread_handle_std_smc() should return via
+	 * thread_exit(), thread_rpc(), but if thread_handle_std_smc()
+	 * hasn't switched stack (error detected) it will do a normal "C"
+	 * return.
+	 */
+	pop	{r1-r8}
+	ldr	r0, =TEESMC_OPTEED_RETURN_CALL_DONE
+	smc	#0
+	b	.	/* SMC should not return */
+UNWIND(	.fnend)
+END_FUNC vector_std_smc_entry
+
+LOCAL_FUNC vector_fast_smc_entry , :
+UNWIND(	.fnstart)
+UNWIND(	.cantunwind)
+	push	{r0-r7}
+	mov	r0, sp
+	bl	thread_handle_fast_smc
+	pop	{r1-r8}
+	ldr	r0, =TEESMC_OPTEED_RETURN_CALL_DONE
+	smc	#0
+	b	.	/* SMC should not return */
+UNWIND(	.fnend)
+END_FUNC vector_fast_smc_entry
+
+LOCAL_FUNC vector_fiq_entry , :
+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, [lr]
+ 	blx	lr
+	mov	r1, r0
+	ldr	r0, =TEESMC_OPTEED_RETURN_FIQ_DONE
+	smc	#0
+	b	.	/* SMC should not return */
+UNWIND(	.fnend)
+END_FUNC vector_fiq_entry
+
+LOCAL_FUNC vector_cpu_on_entry , :
+UNWIND(	.fnstart)
+UNWIND(	.cantunwind)
+	ldr	lr, =thread_cpu_on_handler_ptr
+	ldr	lr, [lr]
+	blx	lr
+	mov	r1, r0
+	ldr	r0, =TEESMC_OPTEED_RETURN_ON_DONE
+	smc	#0
+	b	.	/* SMC should not return */
+UNWIND(	.fnend)
+END_FUNC vector_cpu_on_entry
+
+LOCAL_FUNC vector_cpu_off_entry , :
+UNWIND(	.fnstart)
+UNWIND(	.cantunwind)
+	ldr	lr, =thread_cpu_off_handler_ptr
+	ldr	lr, [lr]
+	blx	lr
+	mov	r1, r0
+	ldr	r0, =TEESMC_OPTEED_RETURN_OFF_DONE
+	smc	#0
+	b	.	/* SMC should not return */
+UNWIND(	.fnend)
+END_FUNC vector_cpu_off_entry
+
+LOCAL_FUNC vector_cpu_suspend_entry , :
+UNWIND(	.fnstart)
+UNWIND(	.cantunwind)
+	ldr	lr, =thread_cpu_suspend_handler_ptr
+	ldr	lr, [lr]
+	blx	lr
+	mov	r1, r0
+	ldr	r0, =TEESMC_OPTEED_RETURN_SUSPEND_DONE
+	smc	#0
+	b	.	/* SMC should not return */
+UNWIND(	.fnend)
+END_FUNC vector_cpu_suspend_entry
+
+LOCAL_FUNC vector_cpu_resume_entry , :
+UNWIND(	.fnstart)
+UNWIND(	.cantunwind)
+	ldr	lr, =thread_cpu_resume_handler_ptr
+	ldr	lr, [lr]
+	blx	lr
+	mov	r1, r0
+	ldr	r0, =TEESMC_OPTEED_RETURN_RESUME_DONE
+	smc	#0
+	b	.	/* SMC should not return */
+UNWIND(	.fnend)
+END_FUNC vector_cpu_resume_entry
+
+LOCAL_FUNC vector_system_off_entry , :
+UNWIND(	.fnstart)
+UNWIND(	.cantunwind)
+	ldr	lr, =thread_system_off_handler_ptr
+	ldr	lr, [lr]
+	blx	lr
+	mov	r1, r0
+	ldr	r0, =TEESMC_OPTEED_RETURN_SYSTEM_OFF_DONE
+	smc	#0
+	b	.	/* SMC should not return */
+UNWIND(	.fnend)
+END_FUNC vector_system_off_entry
+
+LOCAL_FUNC vector_system_reset_entry , :
+UNWIND(	.fnstart)
+UNWIND(	.cantunwind)
+	ldr	lr, =thread_system_reset_handler_ptr
+	ldr	lr, [lr]
+	blx	lr
+	mov	r1, r0
+	ldr	r0, =TEESMC_OPTEED_RETURN_SYSTEM_RESET_DONE
+	smc	#0
+	b	.	/* SMC should not return */
+UNWIND(	.fnend)
+END_FUNC vector_system_reset_entry
+
+/*
+ * Vector table supplied to ARM Trusted Firmware (ARM-TF) at
+ * initialization.  Also used when compiled with the internal monitor, but
+ * the cpu_*_entry and system_*_entry are not used then.
+ *
+ * Note that ARM-TF depends on the layout of this vector table, any change
+ * in layout has to be synced with ARM-TF.
+ */
+FUNC thread_vector_table , :
+UNWIND(	.fnstart)
+UNWIND(	.cantunwind)
+	b	vector_std_smc_entry
+	b	vector_fast_smc_entry
+	b	vector_cpu_on_entry
+	b	vector_cpu_off_entry
+	b	vector_cpu_resume_entry
+	b	vector_cpu_suspend_entry
+	b	vector_fiq_entry
+	b	vector_system_off_entry
+	b	vector_system_reset_entry
+UNWIND(	.fnend)
+END_FUNC thread_vector_table
+
+FUNC thread_set_abt_sp , :
+UNWIND(	.fnstart)
+UNWIND(	.cantunwind)
+	mrs	r1, cpsr
+	cps	#CPSR_MODE_ABT
+	mov	sp, r0
+	msr	cpsr, r1
+	bx	lr
+UNWIND(	.fnend)
+END_FUNC thread_set_abt_sp
+
+FUNC thread_set_irq_sp , :
+UNWIND(	.fnstart)
+UNWIND(	.cantunwind)
+	mrs	r1, cpsr
+	cps	#CPSR_MODE_IRQ
+	mov	sp, r0
+	msr	cpsr, r1
+	bx	lr
+UNWIND(	.fnend)
+END_FUNC thread_set_irq_sp
+
+FUNC thread_set_fiq_sp , :
+UNWIND(	.fnstart)
+UNWIND(	.cantunwind)
+	mrs	r1, cpsr
+	cps	#CPSR_MODE_FIQ
+	mov	sp, r0
+	msr	cpsr, r1
+	bx	lr
+UNWIND(	.fnend)
+END_FUNC thread_set_fiq_sp
+
+/* void thread_resume(struct thread_ctx_regs *regs) */
+FUNC thread_resume , :
+UNWIND(	.fnstart)
+UNWIND(	.cantunwind)
+	add	r12, r0, #(13 * 4)	/* Restore registers r0-r12 later */
+
+	cps	#CPSR_MODE_SYS
+	ldm	r12!, {sp, lr}
+
+	cps	#CPSR_MODE_SVC
+	ldm	r12!, {r1, sp, lr}
+	msr	spsr_fsxc, r1
+
+	cps	#CPSR_MODE_SVC
+	ldm	r12, {r1, r2}
+	push	{r1, r2}
+
+	ldm	r0, {r0-r12}
+
+	/* Restore CPSR and jump to the instruction to resume at */
+	rfefd	sp!
+UNWIND(	.fnend)
+END_FUNC thread_resume
+
+/*
+ * Disables IRQ and FIQ and saves state of thread, returns original
+ * CPSR.
+ */
+LOCAL_FUNC thread_save_state , :
+UNWIND(	.fnstart)
+UNWIND(	.cantunwind)
+	push	{r12, lr}
+	/*
+	 * Uses stack for temporary storage, while storing needed
+	 * context in the thread context struct.
+	 */
+
+	mrs	r12, cpsr
+
+	cpsid	aif			/* Disable Async abort, IRQ and FIQ */
+
+	push	{r4-r7}
+	push	{r0-r3}
+
+	mov	r5, r12			/* Save CPSR in a preserved register */
+	mrs	r6, cpsr		/* Save current CPSR */
+
+	bl	thread_get_ctx_regs
+
+	pop	{r1-r4}			/* r0-r3 pushed above */
+	stm	r0!, {r1-r4}
+	pop	{r1-r4}			/* r4-r7 pushed above */
+	stm	r0!, {r1-r4}
+	stm	r0!, {r8-r11}
+
+	pop	{r12, lr}
+	stm	r0!, {r12}
+
+        cps     #CPSR_MODE_SYS
+        stm     r0!, {sp, lr}
+
+        cps     #CPSR_MODE_SVC
+        mrs     r1, spsr
+        stm     r0!, {r1, sp, lr}
+
+	orr	r6, r6, #ARM32_CPSR_FIA	/* Disable Async abort, IRQ and FIQ */
+	msr	cpsr, r6		/* Restore mode */
+
+	mov	r0, r5			/* Return original CPSR */
+	bx	lr
+UNWIND(	.fnend)
+END_FUNC thread_save_state
+
+FUNC thread_std_smc_entry , :
+UNWIND(	.fnstart)
+UNWIND(	.cantunwind)
+	/* Pass r0-r7 in a struct thread_smc_args */
+	push	{r0-r7}
+	mov	r0, sp
+	bl	__thread_std_smc_entry
+	/*
+	 * Load the returned r0-r3 into preserved registers and skip the
+	 * "returned" r4-r7 since they will not be returned to normal
+	 * world.
+	 */
+	pop	{r4-r7}
+	add	sp, #(4 * 4)
+
+	/* Disable interrupts before switching to temporary stack */
+	cpsid	aif
+	bl	thread_get_tmp_sp
+	mov	sp, r0
+
+	bl	thread_state_free
+
+	ldr	r0, =TEESMC_OPTEED_RETURN_CALL_DONE
+	mov	r1, r4
+	mov	r2, r5
+	mov	r3, r6
+	mov	r4, r7
+	smc	#0
+	b	.	/* SMC should not return */
+UNWIND(	.fnend)
+END_FUNC thread_std_smc_entry
+
+
+/* void thread_rpc(uint32_t rv[THREAD_RPC_NUM_ARGS]) */
+FUNC thread_rpc , :
+/*
+ * r0-r2 are used to pass parameters to normal world
+ * r0-r5 are used to pass return vaule back from normal world
+ *
+ * note that r3 is used to pass "resume information", that is, which
+ * thread it is that should resume.
+ *
+ * Since the this function is following AAPCS we need to preserve r4-r5
+ * which are otherwise modified when returning back from normal world.
+ */
+UNWIND(	.fnstart)
+	push	{r4-r5, lr}
+UNWIND(	.save	{r4-r5, lr})
+	push	{r0}
+UNWIND(	.save	{r0})
+
+	bl	thread_save_state
+	mov	r4, r0			/* Save original CPSR */
+
+	/*
+ 	 * Switch to temporary stack and SVC mode. Save CPSR to resume into.
+	 */
+	bl	thread_get_tmp_sp
+	ldr	r5, [sp]		/* Get pointer to rv[] */
+	cps	#CPSR_MODE_SVC		/* Change to SVC mode */
+	mov	sp, r0			/* Switch to tmp stack */
+
+	mov	r0, #THREAD_FLAGS_COPY_ARGS_ON_RETURN
+	mov	r1, r4			/* CPSR to restore */
+	ldr	r2, =.thread_rpc_return
+	bl	thread_state_suspend
+	mov	r4, r0			/* Supply thread index */
+	ldr	r0, =TEESMC_OPTEED_RETURN_CALL_DONE
+	ldm	r5, {r1-r3}		/* Load rv[] into r0-r2 */
+	smc	#0
+	b	.	/* SMC should not return */
+
+.thread_rpc_return:
+	/*
+	 * At this point has the stack pointer been restored to the value
+	 * it had when thread_save_state() was called above.
+	 *
+	 * Jumps here from thread_resume above when RPC has returned. The
+	 * IRQ and FIQ bits are restored to what they where when this
+	 * function was originally entered.
+	 */
+	pop	{r12}			/* Get pointer to rv[] */
+	stm	r12, {r0-r5}		/* Store r0-r5 into rv[] */
+	pop	{r4-r5, pc}
+UNWIND(	.fnend)
+END_FUNC thread_rpc
+
+LOCAL_FUNC thread_fiq_handler , :
+UNWIND(	.fnstart)
+UNWIND(	.cantunwind)
+	/* FIQ has a +4 offset for lr compared to preferred return address */
+	sub     lr, lr, #4
+	/*
+	 * We're saving {r0-r3} and the banked fiq registers {r8-r12}. The
+	 * banked fiq registers need to be saved because the secure monitor
+	 * doesn't save those. The treatment of the banked fiq registers is
+	 * somewhat analogous to the lazy save of VFP registers.
+	 */
+	push	{r0-r3, r8-r12, lr}
+	bl	thread_check_canaries
+	ldr	lr, =thread_fiq_handler_ptr
+	ldr	lr, [lr]
+	blx	lr
+	pop	{r0-r3, r8-r12, lr}
+	movs	pc, lr
+UNWIND(	.fnend)
+END_FUNC thread_fiq_handler
+
+LOCAL_FUNC thread_irq_handler , :
+UNWIND(	.fnstart)
+UNWIND(	.cantunwind)
+	/*
+	 * IRQ mode is set up to use tmp stack so FIQ has to be
+	 * disabled before touching the stack. We can also assign
+	 * SVC sp from IRQ sp to get SVC mode into the state we
+	 * need when doing the SMC below.
+	 */
+	cpsid	f			/* Disable FIQ also */
+	sub	lr, lr, #4
+	push	{lr}
+	push	{r12}
+
+	bl	thread_save_state
+
+	mov	r0, #THREAD_FLAGS_EXIT_ON_IRQ
+	mrs	r1, spsr
+	pop	{r12}
+	pop	{r2}
+	blx	thread_state_suspend
+	mov	r4, r0		/* Supply thread index */
+
+	/*
+	 * Switch to SVC mode and copy current stack pointer as it already
+	 * is the tmp stack.
+	 */
+	mov	r0, sp
+	cps	#CPSR_MODE_SVC
+	mov	sp, r0
+
+	ldr	r0, =TEESMC_OPTEED_RETURN_CALL_DONE
+	ldr	r1, =OPTEE_SMC_RETURN_RPC_IRQ
+	mov	r2, #0
+	mov	r3, #0
+	/* r4 is already filled in above */
+	smc	#0
+	b	.	/* SMC should not return */
+UNWIND(	.fnend)
+END_FUNC thread_irq_handler
+
+FUNC thread_init_vbar , :
+UNWIND(	.fnstart)
+	/* Set vector (VBAR) */
+	ldr	r0, =thread_vect_table
+	write_vbar r0
+	bx	lr
+UNWIND(	.fnend)
+END_FUNC thread_init_vbar
+
+/*
+ * Below are low level routines handling entry and return from user mode.
+ *
+ * thread_enter_user_mode() saves all that registers user mode can change
+ * so kernel mode can restore needed registers when resuming execution
+ * after the call to thread_enter_user_mode() has returned.
+ * thread_enter_user_mode() doesn't return directly since it enters user
+ * mode instead, it's thread_unwind_user_mode() that does the
+ * returning by restoring the registers saved by thread_enter_user_mode().
+ *
+ * There's three ways for thread_enter_user_mode() to return to caller,
+ * user TA calls utee_return, user TA calls utee_panic or through an abort.
+ *
+ * Calls to utee_return or utee_panic are handled as:
+ * thread_svc_handler() -> tee_svc_handler() ->	tee_svc_do_call() which
+ * calls syscall_return() or syscall_panic().
+ *
+ * These function calls returns normally except thread_svc_handler() which
+ * which is an exception handling routine so it reads return address and
+ * SPSR to restore from the stack. syscall_return() and syscall_panic()
+ * changes return address and SPSR used by thread_svc_handler() to instead of
+ * returning into user mode as with other syscalls it returns into
+ * thread_unwind_user_mode() in kernel mode instead.  When
+ * thread_svc_handler() returns the stack pointer at the point where
+ * thread_enter_user_mode() left it so this is where
+ * thread_unwind_user_mode() can operate.
+ *
+ * Aborts are handled in a similar way but by thread_abort_handler()
+ * instead, when the pager sees that it's an abort from user mode that
+ * can't be handled it updates SPSR and return address used by
+ * thread_abort_handler() to return into thread_unwind_user_mode()
+ * instead.
+ */
+
+/*
+ * uint32_t __thread_enter_user_mode(unsigned long a0, unsigned long a1,
+ *               unsigned long a2, unsigned long a3, unsigned long user_sp,
+ *               unsigned long user_func, unsigned long spsr,
+ *               uint32_t *exit_status0, uint32_t *exit_status1)
+ *
+ */
+FUNC __thread_enter_user_mode , :
+UNWIND(	.fnstart)
+UNWIND(	.cantunwind)
+	/*
+	 * Save all registers to allow syscall_return() to resume execution
+	 * as if this function would have returned. This is also used in
+	 * syscall_panic().
+	 *
+	 * If stack usage of this function is changed
+	 * thread_unwind_user_mode() has to be updated.
+	 */
+	push    {r4-r12,lr}
+
+	ldr     r4, [sp, #(10 * 0x4)]   /* user stack pointer */
+	ldr     r5, [sp, #(11 * 0x4)]   /* user function */
+	ldr     r6, [sp, #(12 * 0x4)]   /* spsr */
+
+	/*
+	 * Set the saved Processors Status Register to user mode to allow
+	 * entry of user mode through movs below.
+	 */
+	msr     spsr_cxsf, r6
+	
+	/*
+	 * Save old user sp and set new user sp.
+	 */
+	cps	#CPSR_MODE_SYS
+	mov	r6, sp
+	mov     sp, r4
+	cps	#CPSR_MODE_SVC
+	push	{r6,r7}
+
+	/*
+	* Don't allow return from this function, return is done through
+	* thread_unwind_user_mode() below.
+	*/
+	mov     lr, #0
+	/* Call the user function with its arguments */
+	movs    pc, r5
+UNWIND(	.fnend)
+END_FUNC __thread_enter_user_mode
+
+/*
+ * void thread_unwind_user_mode(uint32_t ret, uint32_t exit_status0,
+ *              uint32_t exit_status1);
+ * See description in thread.h
+ */
+FUNC thread_unwind_user_mode , :
+UNWIND(	.fnstart)
+UNWIND(	.cantunwind)
+	ldr     ip, [sp, #(15 * 0x4)]   /* &ctx->panicked */
+	str	r1, [ip]
+	ldr     ip, [sp, #(16 * 0x4)]   /* &ctx->panic_code */
+	str	r2, [ip]
+
+	/* Restore old user sp */
+	pop	{r4,r7}
+	cps	#CPSR_MODE_SYS
+	mov	sp, r4
+	cps	#CPSR_MODE_SVC
+
+	pop     {r4-r12,pc}	/* Match the push in thread_enter_user_mode()*/
+UNWIND(	.fnend)
+END_FUNC thread_unwind_user_mode
+
+LOCAL_FUNC thread_abort_handler , :
+thread_abort_handler:
+thread_und_handler:
+UNWIND(	.fnstart)
+UNWIND(	.cantunwind)
+	/*
+	 * Switch to abort mode to use that stack instead.
+	 */
+	cps	#CPSR_MODE_ABT
+	push	{r0-r11, ip}
+	cps	#CPSR_MODE_UND
+	mrs	r0, spsr
+	tst	r0, #CPSR_T
+	subne	r1, lr, #2
+	subeq	r1, lr, #4
+	cps	#CPSR_MODE_ABT
+	push	{r0, r1}
+	msr	spsr_fsxc, r0	/* In case some code reads spsr directly */
+	mov	r0, #ABORT_TYPE_UNDEF
+	b	.thread_abort_generic
+
+thread_dabort_handler:
+	push	{r0-r11, ip}
+	sub	r1, lr, #8
+	mrs	r0, spsr
+	push	{r0, r1}
+	mov	r0, #ABORT_TYPE_DATA
+	b	.thread_abort_generic
+
+thread_pabort_handler:
+	push	{r0-r11, ip}
+	sub	r1, lr, #4
+	mrs	r0, spsr
+	push	{r0, r1}
+	mov	r0, #ABORT_TYPE_PREFETCH
+	b	.thread_abort_generic
+
+.thread_abort_generic:
+	cps	#CPSR_MODE_SYS
+	mov	r1, sp
+	mov	r2, lr
+	cps	#CPSR_MODE_ABT
+	push	{r1-r3}
+	mov	r1, sp
+	bl	abort_handler
+	pop	{r1-r3}
+	cps	#CPSR_MODE_SYS
+	mov	sp, r1
+	mov	lr, r2
+	cps	#CPSR_MODE_ABT
+	pop	{r0, r1}
+	mov	lr, r1
+	msr	spsr_fsxc, r0
+	pop	{r0-r11, ip}
+	movs	pc, lr
+UNWIND(	.fnend)
+END_FUNC thread_abort_handler
+
+LOCAL_FUNC thread_svc_handler , :
+UNWIND(	.fnstart)
+UNWIND(	.cantunwind)
+	push	{r0-r7, lr}
+	mrs	r0, spsr
+	push	{r0}
+	mov	r0, sp
+	bl	tee_svc_handler
+	pop	{r0}
+	msr	spsr_fsxc, r0
+	pop	{r0-r7, lr}
+	movs	pc, lr
+UNWIND(	.fnend)
+END_FUNC thread_svc_handler
+
+        .align	5
+LOCAL_FUNC thread_vect_table , :
+UNWIND(	.fnstart)
+UNWIND(	.cantunwind)
+	b	.			/* Reset			*/
+	b	thread_und_handler	/* Undefined instruction	*/
+	b	thread_svc_handler	/* System call			*/
+	b	thread_pabort_handler	/* Prefetch abort		*/
+	b	thread_dabort_handler	/* Data abort			*/
+	b	.			/* Reserved			*/
+	b	thread_irq_handler	/* IRQ				*/
+	b	thread_fiq_handler	/* FIQ				*/
+UNWIND(	.fnend)
+END_FUNC thread_vect_table
diff --git a/core/arch/arm/kernel/thread_a64.S b/core/arch/arm/kernel/thread_a64.S
new file mode 100644
index 0000000..abd482b
--- /dev/null
+++ b/core/arch/arm/kernel/thread_a64.S
@@ -0,0 +1,816 @@
+/*
+ * 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 <asm.S>
+#include <arm64_macros.S>
+#include <arm64.h>
+#include <sm/optee_smc.h>
+#include <sm/teesmc_opteed_macros.h>
+#include <sm/teesmc_opteed.h>
+#include <asm-defines.h>
+#include <kernel/thread_defs.h>
+#include "thread_private.h"
+
+	.macro get_thread_ctx core_local, res, tmp0, tmp1
+		ldr	w\tmp0, [\core_local, \
+				#THREAD_CORE_LOCAL_CURR_THREAD]
+		adr	x\res, threads
+		mov	x\tmp1, #THREAD_CTX_SIZE
+		madd	x\res, x\tmp0, x\tmp1, x\res
+	.endm
+
+	.section .text.thread_asm
+LOCAL_FUNC vector_std_smc_entry , :
+	sub	sp, sp, #THREAD_SMC_ARGS_SIZE
+	store_xregs sp, THREAD_SMC_ARGS_X0, 0, 7
+	mov	x0, sp
+	bl	thread_handle_std_smc
+	/*
+	 * Normally thread_handle_std_smc() should return via
+	 * thread_exit(), thread_rpc(), but if thread_handle_std_smc()
+	 * hasn't switched stack (error detected) it will do a normal "C"
+	 * return.
+	 */
+	load_xregs sp, THREAD_SMC_ARGS_X0, 1, 8
+	add	sp, sp, #THREAD_SMC_ARGS_SIZE
+	ldr	x0, =TEESMC_OPTEED_RETURN_CALL_DONE
+	smc	#0
+	b	.	/* SMC should not return */
+END_FUNC vector_std_smc_entry
+
+LOCAL_FUNC vector_fast_smc_entry , :
+	sub	sp, sp, #THREAD_SMC_ARGS_SIZE
+	store_xregs sp, THREAD_SMC_ARGS_X0, 0, 7
+	mov	x0, sp
+	bl	thread_handle_fast_smc
+	load_xregs sp, THREAD_SMC_ARGS_X0, 1, 8
+	add	sp, sp, #THREAD_SMC_ARGS_SIZE
+	ldr	x0, =TEESMC_OPTEED_RETURN_CALL_DONE
+	smc	#0
+	b	.	/* SMC should not return */
+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
+	ldr	x16, [x16]
+	blr	x16
+	ldr	x0, =TEESMC_OPTEED_RETURN_FIQ_DONE
+	smc	#0
+	b	.	/* SMC should not return */
+END_FUNC vector_fiq_entry
+
+LOCAL_FUNC vector_cpu_on_entry , :
+	adr	x16, thread_cpu_on_handler_ptr
+	ldr	x16, [x16]
+	blr	x16
+	mov	x1, x0
+	ldr	x0, =TEESMC_OPTEED_RETURN_ON_DONE
+	smc	#0
+	b	.	/* SMC should not return */
+END_FUNC vector_cpu_on_entry
+
+LOCAL_FUNC vector_cpu_off_entry , :
+	adr	x16, thread_cpu_off_handler_ptr
+	ldr	x16, [x16]
+	blr	x16
+	mov	x1, x0
+	ldr	x0, =TEESMC_OPTEED_RETURN_OFF_DONE
+	smc	#0
+	b	.	/* SMC should not return */
+END_FUNC vector_cpu_off_entry
+
+LOCAL_FUNC vector_cpu_suspend_entry , :
+	adr	x16, thread_cpu_suspend_handler_ptr
+	ldr	x16, [x16]
+	blr	x16
+	mov	x1, x0
+	ldr	x0, =TEESMC_OPTEED_RETURN_SUSPEND_DONE
+	smc	#0
+	b	.	/* SMC should not return */
+END_FUNC vector_cpu_suspend_entry
+
+LOCAL_FUNC vector_cpu_resume_entry , :
+	adr	x16, thread_cpu_resume_handler_ptr
+	ldr	x16, [x16]
+	blr	x16
+	mov	x1, x0
+	ldr	x0, =TEESMC_OPTEED_RETURN_RESUME_DONE
+	smc	#0
+	b	.	/* SMC should not return */
+END_FUNC vector_cpu_resume_entry
+
+LOCAL_FUNC vector_system_off_entry , :
+	adr	x16, thread_system_off_handler_ptr
+	ldr	x16, [x16]
+	blr	x16
+	mov	x1, x0
+	ldr	x0, =TEESMC_OPTEED_RETURN_SYSTEM_OFF_DONE
+	smc	#0
+	b	.	/* SMC should not return */
+END_FUNC vector_system_off_entry
+
+LOCAL_FUNC vector_system_reset_entry , :
+	adr	x16, thread_system_reset_handler_ptr
+	ldr	x16, [x16]
+	blr	x16
+	mov	x1, x0
+	ldr	x0, =TEESMC_OPTEED_RETURN_SYSTEM_RESET_DONE
+	smc	#0
+	b	.	/* SMC should not return */
+END_FUNC vector_system_reset_entry
+
+/*
+ * Vector table supplied to ARM Trusted Firmware (ARM-TF) at
+ * initialization.
+ *
+ * Note that ARM-TF depends on the layout of this vector table, any change
+ * in layout has to be synced with ARM-TF.
+ */
+FUNC thread_vector_table , :
+	b	vector_std_smc_entry
+	b	vector_fast_smc_entry
+	b	vector_cpu_on_entry
+	b	vector_cpu_off_entry
+	b	vector_cpu_resume_entry
+	b	vector_cpu_suspend_entry
+	b	vector_fiq_entry
+	b	vector_system_off_entry
+	b	vector_system_reset_entry
+END_FUNC thread_vector_table
+
+
+/* void thread_resume(struct thread_ctx_regs *regs) */
+FUNC thread_resume , :
+	load_xregs x0, THREAD_CTX_REGS_SP, 1, 3
+	mov	sp, x1
+	msr	elr_el1, x2
+	msr	spsr_el1, x3
+	load_xregs x0, THREAD_CTX_REGS_X1, 1, 30
+	ldr	x0, [x0, THREAD_CTX_REGS_X0]
+	eret
+END_FUNC thread_resume
+
+FUNC thread_std_smc_entry , :
+	/* pass x0-x7 in a struct thread_smc_args */
+	sub	sp, sp, #THREAD_SMC_ARGS_SIZE
+	store_xregs sp, THREAD_SMC_ARGS_X0, 0, 7
+	mov	x0, sp
+
+	/* Call the registered handler */
+	bl	__thread_std_smc_entry
+
+	/*
+	 * Load the returned x0-x3 into preserved registers and skip the
+	 * "returned" x4-x7 since they will not be returned to normal
+	 * world.
+	 */
+	load_xregs sp, THREAD_SMC_ARGS_X0, 20, 23
+	add	sp, sp, #THREAD_SMC_ARGS_SIZE
+
+	/* Mask all maskable exceptions before switching to temporary stack */
+	msr	daifset, #DAIFBIT_ALL
+	bl	thread_get_tmp_sp
+	mov	sp, x0
+
+	bl	thread_state_free
+
+	ldr	x0, =TEESMC_OPTEED_RETURN_CALL_DONE
+	mov	x1, x20
+	mov	x2, x21
+	mov	x3, x22
+	mov	x4, x23
+	smc	#0
+	b	.	/* SMC should not return */
+END_FUNC thread_std_smc_entry
+
+/* void thread_rpc(uint32_t rv[THREAD_RPC_NUM_ARGS]) */
+FUNC thread_rpc , :
+	/* Read daif and create an SPSR */
+	mrs	x1, daif
+	orr	x1, x1, #(SPSR_64_MODE_EL1 << SPSR_64_MODE_EL_SHIFT)
+
+	/* Mask all maskable exceptions before switching to temporary stack */
+	msr	daifset, #DAIFBIT_ALL
+	push	x0, xzr
+	push	x1, x30
+	bl	thread_get_ctx_regs
+	ldr	x30, [sp, #8]
+	store_xregs x0, THREAD_CTX_REGS_X19, 19, 30
+	mov	x19, x0
+
+	bl	thread_get_tmp_sp
+	pop	x1, xzr		/* Match "push x1, x30" above */
+	mov	x2, sp
+	str	x2, [x19, #THREAD_CTX_REGS_SP]
+	ldr	x20, [sp]	/* Get pointer to rv[] */
+	mov	sp, x0		/* Switch to tmp stack */
+
+	adr	x2, .thread_rpc_return
+	mov	w0, #THREAD_FLAGS_COPY_ARGS_ON_RETURN
+	bl	thread_state_suspend
+	mov	x4, x0		/* Supply thread index */
+	ldr	w0, =TEESMC_OPTEED_RETURN_CALL_DONE
+	load_wregs x20, 0, 1, 3	/* Load rv[] into w0-w2 */
+	smc	#0
+	b	.		/* SMC should not return */
+
+.thread_rpc_return:
+	/*
+	 * At this point has the stack pointer been restored to the value
+	 * stored in THREAD_CTX above.
+	 *
+	 * Jumps here from thread_resume above when RPC has returned. The
+	 * IRQ and FIQ bits are restored to what they where when this
+	 * function was originally entered.
+	 */
+	pop	x16, xzr	/* Get pointer to rv[] */
+	store_wregs x16, 0, 0, 5	/* Store w0-w5 into rv[] */
+	ret
+END_FUNC thread_rpc
+
+FUNC thread_init_vbar , :
+	adr	x0, thread_vect_table
+	msr	vbar_el1, x0
+	ret
+END_FUNC thread_init_vbar
+
+/*
+ * uint32_t __thread_enter_user_mode(unsigned long a0, unsigned long a1,
+ *               unsigned long a2, unsigned long a3, unsigned long user_sp,
+ *               unsigned long user_func, unsigned long spsr,
+ *               uint32_t *exit_status0, uint32_t *exit_status1)
+ *
+ */
+FUNC __thread_enter_user_mode , :
+	ldr	x8, [sp]
+	/*
+	 * Create the and fill in the struct thread_user_mode_rec
+	 */
+	sub	sp, sp, #THREAD_USER_MODE_REC_SIZE
+	store_xregs sp, THREAD_USER_MODE_REC_EXIT_STATUS0_PTR, 7, 8
+	store_xregs sp, THREAD_USER_MODE_REC_X19, 19, 30
+
+	/*
+	 * Switch to SP_EL1
+	 * Disable exceptions
+	 * Save kern sp in x19
+	 */
+	msr	daifset, #DAIFBIT_ALL
+	mov	x19, sp
+	msr	spsel, #1
+
+	/*
+	 * Save the kernel stack pointer in the thread context
+	 */
+	/* get pointer to current thread context */
+	get_thread_ctx sp, 21, 20, 22
+	/*
+	 * Save kernel stack pointer to ensure that el0_svc() uses
+	 * correct stack pointer
+	 */
+	str	x19, [x21, #THREAD_CTX_KERN_SP]
+
+	/*
+	 * Initialize SPSR, ELR_EL1, and SP_EL0 to enter user mode
+	 */
+	msr	spsr_el1, x6
+	/* Set user sp */
+	mov	x13, x4		/* Used when running TA in Aarch32 */
+	msr	sp_el0, x4	/* Used when running TA in Aarch64 */
+	/* Set user function */
+	msr	elr_el1, x5
+
+	/* Jump into user mode */
+	eret
+END_FUNC __thread_enter_user_mode
+
+/*
+ * void thread_unwind_user_mode(uint32_t ret, uint32_t exit_status0,
+ * 		uint32_t exit_status1);
+ * See description in thread.h
+ */
+FUNC thread_unwind_user_mode , :
+	/* Store the exit status */
+	ldp	x3, x4, [sp, #THREAD_USER_MODE_REC_EXIT_STATUS0_PTR]
+	str	w1, [x3]
+	str	w2, [x4]
+	/* Restore x19..x30 */
+	load_xregs sp, THREAD_USER_MODE_REC_X19, 19, 30
+	add	sp, sp, #THREAD_USER_MODE_REC_SIZE
+	/* Return from the call of thread_enter_user_mode() */
+	ret
+END_FUNC thread_unwind_user_mode
+
+	/*
+	 * This macro verifies that the a given vector doesn't exceed the
+	 * architectural limit of 32 instructions. This is meant to be placed
+	 * immedately after the last instruction in the vector. It takes the
+	 * vector entry as the parameter
+	 */
+	.macro check_vector_size since
+	  .if (. - \since) > (32 * 4)
+	    .error "Vector exceeds 32 instructions"
+	  .endif
+	.endm
+
+
+	.align	11
+LOCAL_FUNC thread_vect_table , :
+	/* -----------------------------------------------------
+	 * EL1 with SP0 : 0x0 - 0x180
+	 * -----------------------------------------------------
+	 */
+	.align	7
+sync_el1_sp0:
+	store_xregs sp, THREAD_CORE_LOCAL_X0, 0, 3
+	b	el1_sync_abort
+	check_vector_size sync_el1_sp0
+
+	.align	7
+irq_el1_sp0:
+	store_xregs sp, THREAD_CORE_LOCAL_X0, 0, 3
+	b	elx_irq
+	check_vector_size irq_el1_sp0
+
+	.align	7
+fiq_el1_sp0:
+	store_xregs sp, THREAD_CORE_LOCAL_X0, 0, 3
+	b	elx_fiq
+	check_vector_size fiq_el1_sp0
+
+	.align	7
+SErrorSP0:
+	b	SErrorSP0
+	check_vector_size SErrorSP0
+
+	/* -----------------------------------------------------
+	 * Current EL with SPx: 0x200 - 0x380
+	 * -----------------------------------------------------
+	 */
+	.align	7
+SynchronousExceptionSPx:
+	b	SynchronousExceptionSPx
+	check_vector_size SynchronousExceptionSPx
+
+	.align	7
+IrqSPx:
+	b	IrqSPx
+	check_vector_size IrqSPx
+
+	.align	7
+FiqSPx:
+	b	FiqSPx
+	check_vector_size FiqSPx
+
+	.align	7
+SErrorSPx:
+	b	SErrorSPx
+	check_vector_size SErrorSPx
+
+	/* -----------------------------------------------------
+	 * Lower EL using AArch64 : 0x400 - 0x580
+	 * -----------------------------------------------------
+	 */
+	.align	7
+el0_sync_a64:
+	store_xregs sp, THREAD_CORE_LOCAL_X0, 0, 3
+	mrs	x2, esr_el1
+	mrs	x3, sp_el0
+	lsr	x2, x2, #ESR_EC_SHIFT
+	cmp	x2, #ESR_EC_AARCH64_SVC
+	b.eq	el0_svc
+	b	el0_sync_abort
+	check_vector_size el0_sync_a64
+
+	.align	7
+el0_irq_a64:
+	store_xregs sp, THREAD_CORE_LOCAL_X0, 0, 3
+	b	elx_irq
+	check_vector_size el0_irq_a64
+
+	.align	7
+el0_fiq_a64:
+	store_xregs sp, THREAD_CORE_LOCAL_X0, 0, 3
+	b	elx_fiq
+	check_vector_size el0_fiq_a64
+
+	.align	7
+SErrorA64:
+	b   	SErrorA64
+	check_vector_size SErrorA64
+
+	/* -----------------------------------------------------
+	 * Lower EL using AArch32 : 0x0 - 0x180
+	 * -----------------------------------------------------
+	 */
+	.align	7
+el0_sync_a32:
+	store_xregs sp, THREAD_CORE_LOCAL_X0, 0, 3
+	mrs	x2, esr_el1
+	mrs	x3, sp_el0
+	lsr	x2, x2, #ESR_EC_SHIFT
+	cmp	x2, #ESR_EC_AARCH32_SVC
+	b.eq	el0_svc
+	b	el0_sync_abort
+	check_vector_size el0_sync_a32
+
+	.align	7
+el0_irq_a32:
+	store_xregs sp, THREAD_CORE_LOCAL_X0, 0, 3
+	b	elx_irq
+	check_vector_size el0_irq_a32
+
+	.align	7
+el0_fiq_a32:
+	store_xregs sp, THREAD_CORE_LOCAL_X0, 0, 3
+	b	elx_fiq
+	check_vector_size el0_fiq_a32
+
+	.align	7
+SErrorA32:
+	b	SErrorA32
+	check_vector_size SErrorA32
+
+END_FUNC thread_vect_table
+
+LOCAL_FUNC el0_svc , :
+	/* get pointer to current thread context in x0 */
+	get_thread_ctx sp, 0, 1, 2
+	/* load saved kernel sp */
+	ldr	x0, [x0, #THREAD_CTX_KERN_SP]
+	/* Keep pointer to initial recod in x1 */
+	mov	x1, sp
+	/* Switch to SP_EL0 and restore kernel sp */
+	msr	spsel, #0
+	mov	x2, sp	/* Save SP_EL0 */
+	mov	sp, x0
+
+	/* Make room for struct thread_svc_regs */
+	sub	sp, sp, #THREAD_SVC_REG_SIZE
+	stp	x30,x2, [sp, #THREAD_SVC_REG_X30]
+
+	/* Restore x0-x3 */
+	ldp	x2, x3, [x1, #THREAD_CORE_LOCAL_X2]
+	ldp	x0, x1, [x1, #THREAD_CORE_LOCAL_X0]
+
+	/* Prepare the argument for the handler */
+	store_xregs sp, THREAD_SVC_REG_X0, 0, 14
+	mrs	x0, elr_el1
+	mrs	x1, spsr_el1
+	store_xregs sp, THREAD_SVC_REG_ELR, 0, 1
+	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.
+	 */
+	msr	daifclr, #(DAIFBIT_FIQ | DAIFBIT_ABT | DAIFBIT_DBG)
+
+	/* Call the handler */
+	bl	tee_svc_handler
+
+	/* Mask all maskable exceptions since we're switching back to sp_el1 */
+	msr	daifset, #DAIFBIT_ALL
+
+	/*
+	 * Save kernel sp we'll had at the beginning of this function.
+	 * This is when this TA has called another TA because
+	 * __thread_enter_user_mode() also saves the stack pointer in this
+	 * field.
+	 */
+	msr	spsel, #1
+	get_thread_ctx sp, 0, 1, 2
+	msr	spsel, #0
+	add	x1, sp, #THREAD_SVC_REG_SIZE
+	str	x1, [x0, #THREAD_CTX_KERN_SP]
+
+	/* Restore registers to the required state and return*/
+	load_xregs sp, THREAD_SVC_REG_ELR, 0, 1
+	msr	elr_el1, x0
+	msr	spsr_el1, x1
+	load_xregs sp, THREAD_SVC_REG_X0, 0, 14
+	mov	x30, sp
+	ldr	x0, [x30, #THREAD_SVC_REG_SP_EL0]
+	mov	sp, x0
+	ldr	x0, [x30, THREAD_SVC_REG_X0]
+	ldr	x30, [x30, #THREAD_SVC_REG_X30]
+
+	eret
+END_FUNC el0_svc
+
+LOCAL_FUNC el1_sync_abort , :
+	mov	x0, sp
+	msr	spsel, #0
+	mov	x3, sp		/* Save original sp */
+
+	/*
+	 * Update core local flags.
+	 * flags = (flags << THREAD_CLF_SAVED_SHIFT) | THREAD_CLF_ABORT;
+	 */
+	ldr	w1, [x0, #THREAD_CORE_LOCAL_FLAGS]
+	lsl	w1, w1, #THREAD_CLF_SAVED_SHIFT
+	orr	w1, w1, #THREAD_CLF_ABORT
+	tbnz	w1, #(THREAD_CLF_SAVED_SHIFT + THREAD_CLF_ABORT_SHIFT), \
+			.Lsel_tmp_sp
+
+	/* Select abort stack */
+	ldr	x2, [x0, #THREAD_CORE_LOCAL_ABT_STACK_VA_END]
+	b	.Lset_sp
+
+.Lsel_tmp_sp:
+	/* Select tmp stack */
+	ldr	x2, [x0, #THREAD_CORE_LOCAL_TMP_STACK_VA_END]
+	orr	w1, w1, #THREAD_CLF_TMP	/* flags |= THREAD_CLF_TMP; */
+
+.Lset_sp:
+	mov	sp, x2
+	str	w1, [x0, #THREAD_CORE_LOCAL_FLAGS]
+
+	/*
+	 * Save state on stack
+	 */
+	sub	sp, sp, #THREAD_ABT_REGS_SIZE
+	mrs	x2, spsr_el1
+	/* Store spsr, sp_el0 */
+	stp	x2, x3, [sp, #THREAD_ABT_REG_SPSR]
+	/* Store original x0, x1 */
+	ldp	x2, x3, [x0, #THREAD_CORE_LOCAL_X0]
+	stp	x2, x3, [sp, #THREAD_ABT_REG_X0]
+	/* Store original x2, x3 and x4 to x29 */
+	ldp	x2, x3, [x0, #THREAD_CORE_LOCAL_X2]
+	store_xregs sp, THREAD_ABT_REG_X2, 2, 29
+	/* Store x30, elr_el1 */
+	mrs	x0, elr_el1
+	stp	x30, x0, [sp, #THREAD_ABT_REG_X30]
+
+	/*
+	 * Call handler
+	 */
+	mov	x0, #0
+	mov	x1, sp
+	bl	abort_handler
+
+	/*
+	 * Restore state from stack
+	 */
+	/* Load x30, elr_el1 */
+	ldp	x30, x0, [sp, #THREAD_ABT_REG_X30]
+	msr	elr_el1, x0
+	/* Load x0 to x29 */
+	load_xregs sp, THREAD_ABT_REG_X0, 0, 29
+	/* Switch to SP_EL1 */
+	msr	spsel, #1
+	/* Save x0 to x3 in CORE_LOCAL */
+	store_xregs sp, THREAD_CORE_LOCAL_X0, 0, 3
+	/* Restore spsr_el1 and sp_el0 */
+	mrs	x3, sp_el0
+	ldp	x0, x1, [x3, #THREAD_ABT_REG_SPSR]
+	msr	spsr_el1, x0
+	msr	sp_el0, x1
+
+	/* Update core local flags */
+	ldr	w0, [sp, #THREAD_CORE_LOCAL_FLAGS]
+	lsr	w0, w0, #THREAD_CLF_SAVED_SHIFT
+	str	w0, [sp, #THREAD_CORE_LOCAL_FLAGS]
+
+	/* Restore x0 to x3 */
+	load_xregs sp, THREAD_CORE_LOCAL_X0, 0, 3
+
+	/* Return from exception */
+	eret
+END_FUNC el1_sync_abort
+
+	/* sp_el0 in x3 */
+LOCAL_FUNC el0_sync_abort , :
+	/*
+	 * Update core local flags
+	 */
+	ldr	w1, [sp, #THREAD_CORE_LOCAL_FLAGS]
+	lsl	w1, w1, #THREAD_CLF_SAVED_SHIFT
+	orr	w1, w1, #THREAD_CLF_ABORT
+	str	w1, [sp, #THREAD_CORE_LOCAL_FLAGS]
+
+	/*
+	 * Save state on stack
+	 */
+
+	/* load abt_stack_va_end */
+	ldr	x1, [sp, #THREAD_CORE_LOCAL_ABT_STACK_VA_END]
+	/* Keep pointer to initial record in x0 */
+	mov	x0, sp
+	/* Switch to SP_EL0 */
+	msr	spsel, #0
+	mov	sp, x1
+	sub	sp, sp, #THREAD_ABT_REGS_SIZE
+	mrs	x2, spsr_el1
+	/* Store spsr, sp_el0 */
+	stp	x2, x3, [sp, #THREAD_ABT_REG_SPSR]
+	/* Store original x0, x1 */
+	ldp	x2, x3, [x0, #THREAD_CORE_LOCAL_X0]
+	stp	x2, x3, [sp, #THREAD_ABT_REG_X0]
+	/* Store original x2, x3 and x4 to x29 */
+	ldp	x2, x3, [x0, #THREAD_CORE_LOCAL_X2]
+	store_xregs sp, THREAD_ABT_REG_X2, 2, 29
+	/* Store x30, elr_el1 */
+	mrs	x0, elr_el1
+	stp	x30, x0, [sp, #THREAD_ABT_REG_X30]
+
+	/*
+	 * Call handler
+	 */
+	mov	x0, #0
+	mov	x1, sp
+	bl	abort_handler
+
+	/*
+	 * Restore state from stack
+	 */
+
+	/* Load x30, elr_el1 */
+	ldp	x30, x0, [sp, #THREAD_ABT_REG_X30]
+	msr	elr_el1, x0
+	/* Load x0 to x29 */
+	load_xregs sp, THREAD_ABT_REG_X0, 0, 29
+	/* Switch to SP_EL1 */
+	msr	spsel, #1
+	/* Save x0 to x3 in EL1_REC */
+	store_xregs sp, THREAD_CORE_LOCAL_X0, 0, 3
+	/* Restore spsr_el1 and sp_el0 */
+	mrs	x3, sp_el0
+	ldp	x0, x1, [x3, #THREAD_ABT_REG_SPSR]
+	msr	spsr_el1, x0
+	msr	sp_el0, x1
+
+	/* Update core local flags */
+	ldr	w0, [sp, #THREAD_CORE_LOCAL_FLAGS]
+	lsr	w0, w0, #THREAD_CLF_SAVED_SHIFT
+	str	w0, [sp, #THREAD_CORE_LOCAL_FLAGS]
+
+	/* Restore x0 to x3 */
+	load_xregs sp, THREAD_CORE_LOCAL_X0, 0, 3
+
+	/* Return from exception */
+	eret
+END_FUNC el0_sync_abort
+
+LOCAL_FUNC elx_irq , :
+	/*
+	 * Update core local flags
+	 */
+	ldr	w1, [sp, #THREAD_CORE_LOCAL_FLAGS]
+	lsl	w1, w1, #THREAD_CLF_SAVED_SHIFT
+	orr	w1, w1, #THREAD_CLF_TMP
+	orr	w1, w1, #THREAD_CLF_IRQ
+	str	w1, [sp, #THREAD_CORE_LOCAL_FLAGS]
+
+	/* get pointer to current thread context in x0 */
+	get_thread_ctx sp, 0, 1, 2
+	/* Keep original SP_EL0 */
+	mrs	x2, sp_el0
+
+	/* Store original sp_el0 */
+	str	x2, [x0, #THREAD_CTX_REGS_SP]
+	/* store x4..x30 */
+	store_xregs x0, THREAD_CTX_REGS_X4, 4, 30
+	/* Load original x0..x3 into x10..x13 */
+	load_xregs sp, THREAD_CORE_LOCAL_X0, 10, 13
+	/* Save original x0..x3 */
+	store_xregs x0, THREAD_CTX_REGS_X0, 10, 13
+
+	/* load tmp_stack_va_end */
+	ldr	x1, [sp, #THREAD_CORE_LOCAL_TMP_STACK_VA_END]
+	/* Switch to SP_EL0 */
+	msr	spsel, #0
+	mov	sp, x1
+
+	/*
+	 * Mark current thread as suspended
+	 */
+	mov	w0, #THREAD_FLAGS_EXIT_ON_IRQ
+	mrs	x1, spsr_el1
+	mrs	x2, elr_el1
+	bl	thread_state_suspend
+	mov	w4, w0		/* Supply thread index */
+
+	/* Update core local flags */
+	/* Switch to SP_EL1 */
+	msr	spsel, #1
+	ldr	w0, [sp, #THREAD_CORE_LOCAL_FLAGS]
+	lsr	w0, w0, #THREAD_CLF_SAVED_SHIFT
+	str	w0, [sp, #THREAD_CORE_LOCAL_FLAGS]
+	msr	spsel, #0
+
+	/*
+	 * Note that we're exiting with SP_EL0 selected since the entry
+	 * functions expects to have SP_EL0 selected with the tmp stack
+	 * set.
+	 */
+
+	ldr	w0, =TEESMC_OPTEED_RETURN_CALL_DONE
+	ldr	w1, =OPTEE_SMC_RETURN_RPC_IRQ
+	mov	w2, #0
+	mov	w3, #0
+	/* w4 is already filled in above */
+	smc	#0
+	b	.	/* SMC should not return */
+END_FUNC elx_irq
+
+/*
+ * This struct is never used from C it's only here to visualize the
+ * layout.
+ *
+ * struct elx_fiq_rec {
+ * 	uint64_t x[19 - 4]; x4..x18
+ * 	uint64_t lr;
+ * 	uint64_t sp_el0;
+ * };
+ */
+#define ELX_FIQ_REC_X(x)		(8 * ((x) - 4))
+#define ELX_FIQ_REC_LR			(8 + ELX_FIQ_REC_X(19))
+#define ELX_FIQ_REC_SP_EL0		(8 + ELX_FIQ_REC_LR)
+#define ELX_FIQ_REC_SIZE		(8 + ELX_FIQ_REC_SP_EL0)
+
+LOCAL_FUNC elx_fiq , :
+	/*
+	 * Update core local flags
+	 */
+	ldr	w1, [sp, #THREAD_CORE_LOCAL_FLAGS]
+	lsl	w1, w1, #THREAD_CLF_SAVED_SHIFT
+	orr	w1, w1, #THREAD_CLF_FIQ
+	orr	w1, w1, #THREAD_CLF_TMP
+	str	w1, [sp, #THREAD_CORE_LOCAL_FLAGS]
+
+	/* load tmp_stack_va_end */
+	ldr	x1, [sp, #THREAD_CORE_LOCAL_TMP_STACK_VA_END]
+	/* Keep original SP_EL0 */
+	mrs	x2, sp_el0
+	/* Switch to SP_EL0 */
+	msr	spsel, #0
+	mov	sp, x1
+
+	/*
+	 * Save registers on stack that can be corrupted by a call to
+	 * a C function
+	 */
+	/* Make room for struct elx_fiq_rec */
+	sub	sp, sp, #ELX_FIQ_REC_SIZE
+	/* Store x4..x18 */
+	store_xregs sp, ELX_FIQ_REC_X(4), 4, 18
+	/* Store lr and original sp_el0 */
+	stp	x30, x2, [sp, #ELX_FIQ_REC_LR]
+
+	bl	thread_check_canaries
+	adr	x16, thread_fiq_handler_ptr
+	ldr	x16, [x16]
+	blr	x16
+
+	/*
+	 * Restore registers
+	 */
+	/* Restore x4..x18 */
+	load_xregs sp, ELX_FIQ_REC_X(4), 4, 18
+	/* Load  lr and original sp_el0 */
+	ldp	x30, x2, [sp, #ELX_FIQ_REC_LR]
+	/* Restore SP_El0 */
+	mov	sp, x2
+	/* Switch back to SP_EL1 */
+	msr	spsel, #1
+
+	/* Update core local flags */
+	ldr	w0, [sp, #THREAD_CORE_LOCAL_FLAGS]
+	lsr	w0, w0, #THREAD_CLF_SAVED_SHIFT
+	str	w0, [sp, #THREAD_CORE_LOCAL_FLAGS]
+
+	/* Restore x0..x3 */
+	load_xregs sp, THREAD_CORE_LOCAL_X0, 0, 3
+
+	/* Return from exception */
+	eret
+END_FUNC elx_fiq
diff --git a/core/arch/arm/kernel/thread_private.h b/core/arch/arm/kernel/thread_private.h
new file mode 100644
index 0000000..3d87c88
--- /dev/null
+++ b/core/arch/arm/kernel/thread_private.h
@@ -0,0 +1,251 @@
+/*
+ * Copyright (c) 2016, Linaro Limited
+ * 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.
+ */
+
+#ifndef THREAD_PRIVATE_H
+#define THREAD_PRIVATE_H
+
+#ifndef ASM
+
+#include <mm/core_mmu.h>
+#include <mm/pgt_cache.h>
+#include <kernel/vfp.h>
+#include <kernel/mutex.h>
+#include <kernel/thread.h>
+
+enum thread_state {
+	THREAD_STATE_FREE,
+	THREAD_STATE_SUSPENDED,
+	THREAD_STATE_ACTIVE,
+};
+
+#ifdef ARM32
+struct thread_ctx_regs {
+	uint32_t r0;
+	uint32_t r1;
+	uint32_t r2;
+	uint32_t r3;
+	uint32_t r4;
+	uint32_t r5;
+	uint32_t r6;
+	uint32_t r7;
+	uint32_t r8;
+	uint32_t r9;
+	uint32_t r10;
+	uint32_t r11;
+	uint32_t r12;
+	uint32_t usr_sp;
+	uint32_t usr_lr;
+	uint32_t svc_spsr;
+	uint32_t svc_sp;
+	uint32_t svc_lr;
+	uint32_t pc;
+	uint32_t cpsr;
+};
+#endif /*ARM32*/
+
+#ifdef ARM64
+struct thread_ctx_regs {
+	uint64_t sp;
+	uint64_t pc;
+	uint64_t cpsr;
+	uint64_t x[31];
+};
+#endif /*ARM64*/
+
+#ifdef ARM64
+struct thread_user_mode_rec {
+	uint64_t exit_status0_ptr;
+	uint64_t exit_status1_ptr;
+	uint64_t x[31 - 19]; /* x19..x30 */
+};
+#endif /*ARM64*/
+
+#ifdef CFG_WITH_VFP
+struct thread_vfp_state {
+	bool ns_saved;
+	bool sec_saved;
+	bool sec_lazy_saved;
+	struct vfp_state ns;
+	struct vfp_state sec;
+	struct thread_user_vfp_state *uvfp;
+};
+
+#endif /*CFG_WITH_VFP*/
+
+struct thread_ctx {
+	struct thread_ctx_regs regs;
+	enum thread_state state;
+	vaddr_t stack_va_end;
+	uint32_t hyp_clnt_id;
+	uint32_t flags;
+	struct core_mmu_user_map user_map;
+	bool have_user_map;
+#ifdef ARM64
+	vaddr_t kern_sp;	/* Saved kernel SP during user TA execution */
+#endif
+#ifdef CFG_WITH_VFP
+	struct thread_vfp_state vfp_state;
+#endif
+	void *rpc_arg;
+	uint64_t rpc_carg;
+	struct mutex_head mutexes;
+	struct thread_specific_data tsd;
+};
+
+#ifdef ARM64
+/*
+ * struct thread_core_local need to have alignment suitable for a stack
+ * pointer since SP_EL1 points to this
+ */
+#define THREAD_CORE_LOCAL_ALIGNED __aligned(16)
+#else
+#define THREAD_CORE_LOCAL_ALIGNED
+#endif
+
+struct thread_core_local {
+	vaddr_t tmp_stack_va_end;
+	int curr_thread;
+#ifdef ARM64
+	uint32_t flags;
+	vaddr_t abt_stack_va_end;
+	uint64_t x[4];
+#endif
+#ifdef CFG_TEE_CORE_DEBUG
+	unsigned int locked_count; /* Number of spinlocks held */
+#endif
+} THREAD_CORE_LOCAL_ALIGNED;
+
+#endif /*ASM*/
+
+#ifdef ARM64
+#ifdef CFG_WITH_VFP
+#define THREAD_VFP_STATE_SIZE				\
+	(16 + (16 * 32 + 16) * 2 + 16)
+#else
+#define THREAD_VFP_STATE_SIZE				0
+#endif
+
+/* Describes the flags field of struct thread_core_local */
+#define THREAD_CLF_SAVED_SHIFT			4
+#define THREAD_CLF_CURR_SHIFT			0
+#define THREAD_CLF_MASK				0xf
+#define THREAD_CLF_TMP_SHIFT			0
+#define THREAD_CLF_ABORT_SHIFT			1
+#define THREAD_CLF_IRQ_SHIFT			2
+#define THREAD_CLF_FIQ_SHIFT			3
+
+#define THREAD_CLF_TMP				(1 << THREAD_CLF_TMP_SHIFT)
+#define THREAD_CLF_ABORT			(1 << THREAD_CLF_ABORT_SHIFT)
+#define THREAD_CLF_IRQ				(1 << THREAD_CLF_IRQ_SHIFT)
+#define THREAD_CLF_FIQ				(1 << THREAD_CLF_FIQ_SHIFT)
+
+#endif /*ARM64*/
+
+#ifndef ASM
+/*
+ * Initializes VBAR for current CPU (called by thread_init_per_cpu()
+ */
+void thread_init_vbar(void);
+
+/* Handles a stdcall, r0-r7 holds the parameters */
+void thread_std_smc_entry(void);
+
+struct thread_core_local *thread_get_core_local(void);
+
+/*
+ * Resumes execution of currently active thread by restoring context and
+ * jumping to the instruction where to continue execution.
+ *
+ * Arguments supplied by non-secure world will be copied into the saved
+ * context of the current thread if THREAD_FLAGS_COPY_ARGS_ON_RETURN is set
+ * in the flags field in the thread context.
+ */
+void thread_resume(struct thread_ctx_regs *regs);
+
+uint32_t __thread_enter_user_mode(unsigned long a0, unsigned long a1,
+		unsigned long a2, unsigned long a3, unsigned long user_sp,
+		unsigned long user_func, unsigned long spsr,
+		uint32_t *exit_status0, uint32_t *exit_status1);
+
+/*
+ * Private functions made available for thread_asm.S
+ */
+
+/* Returns the temp stack for current CPU */
+void *thread_get_tmp_sp(void);
+
+/*
+ * Marks the current thread as suspended. And updated the flags
+ * for the thread context (see thread resume for use of flags).
+ * Returns thread index of the thread that was suspended.
+ */
+int thread_state_suspend(uint32_t flags, uint32_t cpsr, vaddr_t pc);
+
+/*
+ * Marks the current thread as free.
+ */
+void thread_state_free(void);
+
+/* Returns a pointer to the saved registers in current thread context. */
+struct thread_ctx_regs *thread_get_ctx_regs(void);
+
+#ifdef ARM32
+/* Sets sp for abort mode */
+void thread_set_abt_sp(vaddr_t sp);
+
+/* Sets sp for irq mode */
+void thread_set_irq_sp(vaddr_t sp);
+
+/* Sets sp for fiq mode */
+void thread_set_fiq_sp(vaddr_t sp);
+#endif /*ARM32*/
+
+/* Handles a fast SMC by dispatching it to the registered fast SMC handler */
+void thread_handle_fast_smc(struct thread_smc_args *args);
+
+/* Handles a std SMC by dispatching it to the registered std SMC handler */
+void thread_handle_std_smc(struct thread_smc_args *args);
+
+/*
+ * Suspends current thread and temorarily exits to non-secure world.
+ * This function returns later when non-secure world returns.
+ *
+ * The purpose of this function is to request services from non-secure
+ * world.
+ */
+#define THREAD_RPC_NUM_ARGS     6
+void thread_rpc(uint32_t rv[THREAD_RPC_NUM_ARGS]);
+
+/* Checks stack canaries */
+void thread_check_canaries(void);
+
+void __thread_std_smc_entry(struct thread_smc_args *args);
+
+#endif /*ASM*/
+
+#endif /*THREAD_PRIVATE_H*/
diff --git a/core/arch/arm/kernel/trace_ext.c b/core/arch/arm/kernel/trace_ext.c
new file mode 100644
index 0000000..8b8454c
--- /dev/null
+++ b/core/arch/arm/kernel/trace_ext.c
@@ -0,0 +1,50 @@
+/*
+ * Copyright (c) 2014, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+#include <stdbool.h>
+#include <trace.h>
+#include <console.h>
+#include <kernel/thread.h>
+
+const char trace_ext_prefix[] = "TEE-CORE";
+int trace_level = TRACE_LEVEL;
+
+void trace_ext_puts(const char *str)
+{
+	const char *p;
+
+	console_flush();
+
+	for (p = str; *p; p++)
+		console_putc(*p);
+
+	console_flush();
+}
+
+int trace_ext_get_thread_id(void)
+{
+	return thread_get_id_may_fail();
+}
diff --git a/core/arch/arm/kernel/tz_ssvce_pl310_a32.S b/core/arch/arm/kernel/tz_ssvce_pl310_a32.S
new file mode 100644
index 0000000..184e936
--- /dev/null
+++ b/core/arch/arm/kernel/tz_ssvce_pl310_a32.S
@@ -0,0 +1,258 @@
+/*
+ * 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 <asm.S>
+#include <kernel/tz_proc_def.h>
+#include <kernel/tz_ssvce_def.h>
+#include <kernel/unwind.h>
+#include <platform_config.h>
+
+#define PL310_LOCKDOWN_NBREGS	8
+#define PL310_LOCKDOWN_SZREG	4
+
+#define PL310_8WAYS_MASK	0x00FF
+#define PL310_16WAYS_UPPERMASK	0xFF00
+
+/*
+ * void arm_cl2_lockallways(vaddr_t base)
+ *
+ * lock all L2 caches ways for data and instruction
+ */
+FUNC arm_cl2_lockallways , :
+UNWIND(	.fnstart)
+	add	r1, r0, #PL310_DCACHE_LOCKDOWN_BASE
+	ldr	r2, [r0, #PL310_AUX_CTRL]
+	tst	r2, #PL310_AUX_16WAY_BIT
+	mov	r2, #PL310_8WAYS_MASK
+	orrne	r2, #PL310_16WAYS_UPPERMASK
+	mov	r0, #PL310_LOCKDOWN_NBREGS
+1:	/* lock Dcache and Icache */
+	str	r2, [r1], #PL310_LOCKDOWN_SZREG
+	str	r2, [r1], #PL310_LOCKDOWN_SZREG
+	subs	r0, r0, #1
+	bne	1b
+
+	mov	pc, lr
+UNWIND(	.fnend)
+END_FUNC arm_cl2_lockallways
+
+/*
+ * Set sync operation mask according to ways associativity.
+ * Preserve r0 = pl310 iomem base address
+ */
+.macro syncbyway_set_mask reg
+	ldr	\reg, [r0, #PL310_AUX_CTRL]
+	tst	\reg, #PL310_AUX_16WAY_BIT
+	mov	\reg, #PL310_8WAYS_MASK
+	orrne	\reg, \reg, #PL310_16WAYS_UPPERMASK
+.endm
+
+/*
+ * void arm_cl2_cleaninvbyway(vaddr_t base)
+ * clean & invalidate the whole L2 cache.
+ */
+FUNC arm_cl2_cleaninvbyway , :
+UNWIND(	.fnstart)
+
+	syncbyway_set_mask r1
+	str	r1, [r0, #PL310_FLUSH_BY_WAY]
+
+	/* Wait for all cache ways to be cleaned and invalidated */
+loop_cli_way_done:
+	ldr	r2, [r0, #PL310_FLUSH_BY_WAY]
+	and	r2, r2, r1
+	cmp	r2, #0
+	bne	loop_cli_way_done
+
+	/* Cache Sync */
+
+	/* Wait for writing cache sync */
+loop_cli_sync:
+	ldr	r1, [r0, #PL310_SYNC]
+	cmp	r1, #0
+	bne	loop_cli_sync
+
+	mov	r1, #1
+	str	r1, [r0, #PL310_SYNC]
+
+loop_cli_sync_done:
+	ldr	r1, [r0, #PL310_SYNC]
+	cmp	r1, #0
+	bne	loop_cli_sync_done
+
+	mov	pc, lr
+UNWIND(	.fnend)
+END_FUNC arm_cl2_cleaninvbyway
+
+/* void arm_cl2_invbyway(vaddr_t base) */
+FUNC arm_cl2_invbyway , :
+UNWIND(	.fnstart)
+
+	syncbyway_set_mask r1
+	str	r1, [r0, #PL310_INV_BY_WAY]
+
+loop_inv_way_done:
+	ldr	r2, [r0, #PL310_INV_BY_WAY]
+	and	r2, r2, r1
+	cmp	r2, #0
+	bne	loop_inv_way_done
+
+loop_inv_way_sync:
+	ldr	r1, [r0, #PL310_SYNC]
+	cmp	r1, #0
+	bne	loop_inv_way_sync
+
+	mov	r1, #1
+	str	r1, [r0, #PL310_SYNC]
+
+loop_inv_way_sync_done:
+	ldr	r1, [r0, #PL310_SYNC]
+	cmp	r1, #0
+	bne	loop_inv_way_sync_done
+
+	mov	pc, lr
+UNWIND(	.fnend)
+END_FUNC arm_cl2_invbyway
+
+/* void arm_cl2_cleanbyway(vaddr_t base) */
+FUNC arm_cl2_cleanbyway , :
+UNWIND(	.fnstart)
+
+	syncbyway_set_mask r1
+	str	r1, [r0, #PL310_CLEAN_BY_WAY]
+
+loop_cl_way_done:
+	ldr	r2, [r0, #PL310_CLEAN_BY_WAY]
+	and	r2, r2, r1
+	cmp	r2, #0
+	bne	loop_cl_way_done
+
+loop_cl_way_sync:
+	ldr	r1, [r0, #PL310_SYNC]
+	cmp	r1, #0
+	bne	loop_cl_way_sync
+
+	mov	r1, #1
+	str	r1, [r0, #PL310_SYNC]
+
+loop_cl_way_sync_done:
+	ldr	r1, [r0, #PL310_SYNC]
+	cmp	r1, #0
+	bne	loop_cl_way_sync_done
+
+	mov	pc, lr
+UNWIND(	.fnend)
+END_FUNC arm_cl2_cleanbyway
+
+/*
+ * void _arm_cl2_xxxbypa(vaddr_t pl310_base, paddr_t start, paddr_t end,
+ *			 int pl310value);
+ * pl310value is one of PL310_CLEAN_BY_PA, PL310_INV_BY_PA or PL310_FLUSH_BY_PA
+ */
+LOCAL_FUNC _arm_cl2_xxxbypa , :
+UNWIND(	.fnstart)
+	/* Align start address on PL310 line size */
+	and	r1, #(~(PL310_LINE_SIZE - 1))
+
+	/*
+	 * ARM ERRATA #764369
+	 * Undocummented SCU Diagnostic Control Register
+	 */
+	/*
+	 * NOTE:
+	 * We're assuming that if mmu is enabled PL310_BASE and SCU_BASE
+	 * still have the same relative offsets from each other.
+	 */
+	sub	r0, r0, #(PL310_BASE - SCU_BASE)
+	mov	r12, #1
+	str	r12, [r0, #SCU_ERRATA744369]
+	dsb
+	add	r0, r0, #(PL310_BASE - SCU_BASE)
+
+loop_cl2_xxxbypa:
+	str	r1, [r0, r3]
+
+loop_xxx_pa_done:
+	ldr	r12, [r0, r3]
+	and	r12, r12, r1
+	cmp	r12, #0
+	bne	loop_xxx_pa_done
+
+	add	r1, r1, #PL310_LINE_SIZE
+	cmp	r2, r1
+	bpl	loop_cl2_xxxbypa
+
+loop_xxx_pa_sync:
+	ldr	r12, [r0, #PL310_SYNC]
+	cmp	r12, #0
+	bne	loop_xxx_pa_sync
+
+	mov	r12, #1
+	str	r12, [r0, #PL310_SYNC]
+
+loop_xxx_pa_sync_done:
+	ldr	r12, [r0, #PL310_SYNC]
+	cmp	r12, #0
+	bne	loop_xxx_pa_sync_done
+
+	mov	pc, lr
+UNWIND(	.fnend)
+END_FUNC _arm_cl2_xxxbypa
+
+/*
+ * void _arm_cl2_cleanbypa(vaddr_t pl310_base, paddr_t start, paddr_t end);
+ * clean L2 cache by physical address range.
+ */
+FUNC arm_cl2_cleanbypa , :
+UNWIND(	.fnstart)
+	mov	r3, #PL310_CLEAN_BY_PA
+	b	_arm_cl2_xxxbypa
+UNWIND(	.fnend)
+END_FUNC arm_cl2_cleanbypa
+
+/*
+ * void arm_cl2_invbypa(vaddr_t pl310_base, paddr_t start, paddr_t end);
+ * invalidate L2 cache by physical address range.
+ */
+FUNC arm_cl2_invbypa , :
+UNWIND(	.fnstart)
+	mov	r3, #PL310_INV_BY_PA
+	b	_arm_cl2_xxxbypa
+UNWIND(	.fnend)
+END_FUNC arm_cl2_invbypa
+
+/*
+ * void arm_cl2_cleaninvbypa(vaddr_t pl310_base, paddr_t start, paddr_t end);
+ * clean and invalidate L2 cache by physical address range.
+ */
+FUNC arm_cl2_cleaninvbypa , :
+UNWIND(	.fnstart)
+	mov	r3, #PL310_FLUSH_BY_PA
+	b	_arm_cl2_xxxbypa
+UNWIND(	.fnend)
+END_FUNC arm_cl2_cleaninvbypa
+
diff --git a/core/arch/arm/kernel/unwind_arm32.c b/core/arch/arm/kernel/unwind_arm32.c
new file mode 100644
index 0000000..7efe94b
--- /dev/null
+++ b/core/arch/arm/kernel/unwind_arm32.c
@@ -0,0 +1,417 @@
+/*
+ * Copyright 2015 Linaro Limited
+ * Copyright 2013-2014 Andrew Turner.
+ * Copyright 2013-2014 Ian Lepore.
+ * Copyright 2013-2014 Rui Paulo.
+ * Copyright 2013 Eitan Adler.
+ * 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 AUTHOR 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 AUTHOR 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 <arm.h>
+#include <kernel/misc.h>
+#include <kernel/unwind.h>
+#include <string.h>
+#include <trace.h>
+
+/* The register names */
+#define	FP	11
+#define	SP	13
+#define	LR	14
+#define	PC	15
+
+/*
+ * Definitions for the instruction interpreter.
+ *
+ * The ARM EABI specifies how to perform the frame unwinding in the
+ * Exception Handling ABI for the ARM Architecture document. To perform
+ * the unwind we need to know the initial frame pointer, stack pointer,
+ * link register and program counter. We then find the entry within the
+ * index table that points to the function the program counter is within.
+ * This gives us either a list of three instructions to process, a 31-bit
+ * relative offset to a table of instructions, or a value telling us
+ * we can't unwind any further.
+ *
+ * When we have the instructions to process we need to decode them
+ * following table 4 in section 9.3. This describes a collection of bit
+ * patterns to encode that steps to take to update the stack pointer and
+ * link register to the correct values at the start of the function.
+ */
+
+/* A special case when we are unable to unwind past this function */
+#define	EXIDX_CANTUNWIND	1
+
+/*
+ * Entry types.
+ * These are the only entry types that have been seen in the kernel.
+ */
+#define	ENTRY_MASK	0xff000000
+#define	ENTRY_ARM_SU16	0x80000000
+#define	ENTRY_ARM_LU16	0x81000000
+
+/* Instruction masks. */
+#define	INSN_VSP_MASK		0xc0
+#define	INSN_VSP_SIZE_MASK	0x3f
+#define	INSN_STD_MASK		0xf0
+#define	INSN_STD_DATA_MASK	0x0f
+#define	INSN_POP_TYPE_MASK	0x08
+#define	INSN_POP_COUNT_MASK	0x07
+#define	INSN_VSP_LARGE_INC_MASK	0xff
+
+/* Instruction definitions */
+#define	INSN_VSP_INC		0x00
+#define	INSN_VSP_DEC		0x40
+#define	INSN_POP_MASKED		0x80
+#define	INSN_VSP_REG		0x90
+#define	INSN_POP_COUNT		0xa0
+#define	INSN_FINISH		0xb0
+#define	INSN_POP_REGS		0xb1
+#define	INSN_VSP_LARGE_INC	0xb2
+
+/* An item in the exception index table */
+struct unwind_idx {
+	uint32_t offset;
+	uint32_t insn;
+};
+
+/*
+ * These are set in the linker script. Their addresses will be
+ * either the start or end of the exception table or index.
+ */
+extern struct unwind_idx __exidx_start;
+extern struct unwind_idx __exidx_end;
+
+/* Expand a 31-bit signed value to a 32-bit signed value */
+static int32_t expand_prel31(uint32_t prel31)
+{
+
+	return ((int32_t)(prel31 & 0x7fffffffu) << 1) / 2;
+}
+
+/*
+ * Perform a binary search of the index table to find the function
+ * with the largest address that doesn't exceed addr.
+ */
+static struct unwind_idx *find_index(uint32_t addr)
+{
+	vaddr_t idx_start, idx_end;
+	unsigned int min, mid, max;
+	struct unwind_idx *start;
+	struct unwind_idx *item;
+	int32_t prel31_addr;
+	uint32_t func_addr;
+
+	start = &__exidx_start;
+	idx_start = (vaddr_t)&__exidx_start;
+	idx_end = (vaddr_t)&__exidx_end;
+
+	min = 0;
+	max = (idx_end - idx_start) / sizeof(struct unwind_idx);
+
+	while (min != max) {
+		mid = min + (max - min + 1) / 2;
+
+		item = &start[mid];
+
+		prel31_addr = expand_prel31(item->offset);
+		func_addr = (uint32_t)&item->offset + prel31_addr;
+
+		if (func_addr <= addr) {
+			min = mid;
+		} else {
+			max = mid - 1;
+		}
+	}
+
+	return &start[min];
+}
+
+/* Reads the next byte from the instruction list */
+static uint8_t unwind_exec_read_byte(struct unwind_state *state)
+{
+	uint8_t insn;
+
+	/* Read the unwind instruction */
+	insn = (*state->insn) >> (state->byte * 8);
+
+	/* Update the location of the next instruction */
+	if (state->byte == 0) {
+		state->byte = 3;
+		state->insn++;
+		state->entries--;
+	} else
+		state->byte--;
+
+	return insn;
+}
+
+/* Executes the next instruction on the list */
+static bool unwind_exec_insn(struct unwind_state *state)
+{
+	unsigned int insn;
+	uint32_t *vsp = (uint32_t *)state->registers[SP];
+	int update_vsp = 0;
+
+	/* This should never happen */
+	if (state->entries == 0)
+		return false;
+
+	/* Read the next instruction */
+	insn = unwind_exec_read_byte(state);
+
+	if ((insn & INSN_VSP_MASK) == INSN_VSP_INC) {
+		state->registers[SP] += ((insn & INSN_VSP_SIZE_MASK) << 2) + 4;
+
+	} else if ((insn & INSN_VSP_MASK) == INSN_VSP_DEC) {
+		state->registers[SP] -= ((insn & INSN_VSP_SIZE_MASK) << 2) + 4;
+
+	} else if ((insn & INSN_STD_MASK) == INSN_POP_MASKED) {
+		unsigned int mask, reg;
+
+		/* Load the mask */
+		mask = unwind_exec_read_byte(state);
+		mask |= (insn & INSN_STD_DATA_MASK) << 8;
+
+		/* We have a refuse to unwind instruction */
+		if (mask == 0)
+			return false;
+
+		/* Update SP */
+		update_vsp = 1;
+
+		/* Load the registers */
+		for (reg = 4; mask && reg < 16; mask >>= 1, reg++) {
+			if (mask & 1) {
+				state->registers[reg] = *vsp++;
+				state->update_mask |= 1 << reg;
+
+				/* If we have updated SP kep its value */
+				if (reg == SP)
+					update_vsp = 0;
+			}
+		}
+
+	} else if ((insn & INSN_STD_MASK) == INSN_VSP_REG &&
+	    ((insn & INSN_STD_DATA_MASK) != 13) &&
+	    ((insn & INSN_STD_DATA_MASK) != 15)) {
+		/* sp = register */
+		state->registers[SP] =
+		    state->registers[insn & INSN_STD_DATA_MASK];
+
+	} else if ((insn & INSN_STD_MASK) == INSN_POP_COUNT) {
+		unsigned int count, reg;
+
+		/* Read how many registers to load */
+		count = insn & INSN_POP_COUNT_MASK;
+
+		/* Update sp */
+		update_vsp = 1;
+
+		/* Pop the registers */
+		for (reg = 4; reg <= 4 + count; reg++) {
+			state->registers[reg] = *vsp++;
+			state->update_mask |= 1 << reg;
+		}
+
+		/* Check if we are in the pop r14 version */
+		if ((insn & INSN_POP_TYPE_MASK) != 0) {
+			state->registers[14] = *vsp++;
+		}
+
+	} else if (insn == INSN_FINISH) {
+		/* Stop processing */
+		state->entries = 0;
+
+	} else if (insn == INSN_POP_REGS) {
+		unsigned int mask, reg;
+
+		mask = unwind_exec_read_byte(state);
+		if (mask == 0 || (mask & 0xf0) != 0)
+			return false;
+
+		/* Update SP */
+		update_vsp = 1;
+
+		/* Load the registers */
+		for (reg = 0; mask && reg < 4; mask >>= 1, reg++) {
+			if (mask & 1) {
+				state->registers[reg] = *vsp++;
+				state->update_mask |= 1 << reg;
+			}
+		}
+
+	} else if ((insn & INSN_VSP_LARGE_INC_MASK) == INSN_VSP_LARGE_INC) {
+		unsigned int uleb128;
+
+		/* Read the increment value */
+		uleb128 = unwind_exec_read_byte(state);
+
+		state->registers[SP] += 0x204 + (uleb128 << 2);
+
+	} else {
+		/* We hit a new instruction that needs to be implemented */
+		DMSG("Unhandled instruction %.2x\n", insn);
+		return false;
+	}
+
+	if (update_vsp) {
+		state->registers[SP] = (uint32_t)vsp;
+	}
+
+	return true;
+}
+
+/* Performs the unwind of a function */
+static bool unwind_tab(struct unwind_state *state)
+{
+	uint32_t entry;
+
+	/* Set PC to a known value */
+	state->registers[PC] = 0;
+
+	/* Read the personality */
+	entry = *state->insn & ENTRY_MASK;
+
+	if (entry == ENTRY_ARM_SU16) {
+		state->byte = 2;
+		state->entries = 1;
+	} else if (entry == ENTRY_ARM_LU16) {
+		state->byte = 1;
+		state->entries = ((*state->insn >> 16) & 0xFF) + 1;
+	} else {
+		DMSG("Unknown entry: %x\n", entry);
+		return true;
+	}
+
+	while (state->entries > 0) {
+		if (!unwind_exec_insn(state))
+			return true;
+	}
+
+	/*
+	 * The program counter was not updated, load it from the link register.
+	 */
+	if (state->registers[PC] == 0) {
+		state->registers[PC] = state->registers[LR];
+
+		/*
+		 * If the program counter changed, flag it in the update mask.
+		 */
+		if (state->start_pc != state->registers[PC])
+			state->update_mask |= 1 << PC;
+	}
+
+	return false;
+}
+
+bool unwind_stack(struct unwind_state *state)
+{
+	struct unwind_idx *index;
+	bool finished;
+
+	/* Reset the mask of updated registers */
+	state->update_mask = 0;
+
+	/* The pc value is correct and will be overwritten, save it */
+	state->start_pc = state->registers[PC];
+
+	/* Find the item to run */
+	index = find_index(state->start_pc);
+
+	finished = false;
+	if (index->insn != EXIDX_CANTUNWIND) {
+		if (index->insn & (1U << 31)) {
+			/* The data is within the instruction */
+			state->insn = &index->insn;
+		} else {
+			/* A prel31 offset to the unwind table */
+			state->insn = (uint32_t *)
+			    ((uintptr_t)&index->insn +
+			     expand_prel31(index->insn));
+		}
+		/* Run the unwind function */
+		finished = unwind_tab(state);
+	}
+
+	/* This is the top of the stack, finish */
+	if (index->insn == EXIDX_CANTUNWIND)
+		finished = true;
+
+	return !finished;
+}
+
+#if defined(CFG_CORE_UNWIND) && (TRACE_LEVEL > 0)
+
+void print_stack(int level)
+{
+	struct unwind_state state;
+
+	memset(state.registers, 0, sizeof(state.registers));
+	/* r7: Thumb-style frame pointer */
+	state.registers[7] = read_r7();
+	/* r11: ARM-style frame pointer */
+	state.registers[FP] = read_fp();
+	state.registers[SP] = read_sp();
+	state.registers[LR] = read_lr();
+	state.registers[PC] = (uint32_t)print_stack;
+
+	do {
+		switch (level) {
+		case TRACE_FLOW:
+			FMSG_RAW("pc  0x%08" PRIx32, state.registers[PC]);
+			break;
+		case TRACE_DEBUG:
+			DMSG_RAW("pc  0x%08" PRIx32, state.registers[PC]);
+			break;
+		case TRACE_INFO:
+			IMSG_RAW("pc  0x%08" PRIx32, state.registers[PC]);
+			break;
+		case TRACE_ERROR:
+			EMSG_RAW("pc  0x%08" PRIx32, state.registers[PC]);
+			break;
+		default:
+			break;
+		}
+	} while (unwind_stack(&state));
+}
+
+#endif /* defined(CFG_CORE_UNWIND) && (TRACE_LEVEL > 0) */
+
+/*
+ * These functions are referenced but never used
+ */
+void __aeabi_unwind_cpp_pr0(void);
+void __aeabi_unwind_cpp_pr0(void)
+{
+}
+
+void __aeabi_unwind_cpp_pr1(void);
+void __aeabi_unwind_cpp_pr1(void)
+{
+}
+
+void __aeabi_unwind_cpp_pr2(void);
+void __aeabi_unwind_cpp_pr2(void)
+{
+}
diff --git a/core/arch/arm/kernel/unwind_arm64.c b/core/arch/arm/kernel/unwind_arm64.c
new file mode 100644
index 0000000..10b70ef
--- /dev/null
+++ b/core/arch/arm/kernel/unwind_arm64.c
@@ -0,0 +1,84 @@
+/*-
+ * Copyright (c) 2015 Linaro Limited
+ * Copyright (c) 2015 The FreeBSD Foundation
+ * All rights reserved.
+ *
+ * This software was developed by Semihalf under
+ * the sponsorship of the FreeBSD Foundation.
+ *
+ * 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 AUTHOR 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 AUTHOR 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 <arm.h>
+#include <kernel/unwind.h>
+#include <kernel/thread.h>
+#include <string.h>
+#include <trace.h>
+
+bool unwind_stack(struct unwind_state *frame)
+{
+	uint64_t fp;
+
+	fp = frame->fp;
+	if (!thread_addr_is_in_stack(fp))
+		return false;
+
+	frame->sp = fp + 0x10;
+	/* FP to previous frame (X29) */
+	frame->fp = *(uint64_t *)(fp);
+	/* LR (X30) */
+	frame->pc = *(uint64_t *)(fp + 8) - 4;
+
+	return true;
+}
+
+#if defined(CFG_CORE_UNWIND) && (TRACE_LEVEL > 0)
+
+void print_stack(int level)
+{
+	struct unwind_state state;
+
+	memset(&state, 0, sizeof(state));
+	state.pc = read_pc();
+	state.fp = read_fp();
+
+	do {
+		switch (level) {
+		case TRACE_FLOW:
+			FMSG_RAW("pc  0x%016" PRIx64, state.pc);
+			break;
+		case TRACE_DEBUG:
+			DMSG_RAW("pc  0x%016" PRIx64, state.pc);
+			break;
+		case TRACE_INFO:
+			IMSG_RAW("pc  0x%016" PRIx64, state.pc);
+			break;
+		case TRACE_ERROR:
+			EMSG_RAW("pc  0x%016" PRIx64, state.pc);
+			break;
+		default:
+			break;
+		}
+	} while (unwind_stack(&state));
+}
+
+#endif /* defined(CFG_CORE_UNWIND) && (TRACE_LEVEL > 0) */
diff --git a/core/arch/arm/kernel/user_ta.c b/core/arch/arm/kernel/user_ta.c
new file mode 100644
index 0000000..a63fb22
--- /dev/null
+++ b/core/arch/arm/kernel/user_ta.c
@@ -0,0 +1,826 @@
+/*
+ * Copyright (c) 2014, STMicroelectronics International N.V.
+ * Copyright (c) 2015-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 <compiler.h>
+#include <keep.h>
+#include <kernel/panic.h>
+#include <kernel/tee_ta_manager.h>
+#include <kernel/thread.h>
+#include <kernel/user_ta.h>
+#include <mm/core_memprot.h>
+#include <mm/core_mmu.h>
+#include <mm/mobj.h>
+#include <mm/pgt_cache.h>
+#include <mm/tee_mm.h>
+#include <mm/tee_mmu.h>
+#include <mm/tee_pager.h>
+#include <optee_msg_supplicant.h>
+#include <signed_hdr.h>
+#include <stdlib.h>
+#include <ta_pub_key.h>
+#include <tee/tee_cryp_provider.h>
+#include <tee/tee_cryp_utl.h>
+#include <tee/tee_obj.h>
+#include <tee/tee_svc_cryp.h>
+#include <tee/tee_svc.h>
+#include <tee/tee_svc_storage.h>
+#include <tee/uuid.h>
+#include <trace.h>
+#include <types_ext.h>
+#include <utee_defines.h>
+#include <util.h>
+
+#include "elf_load.h"
+#include "elf_common.h"
+
+#define STACK_ALIGNMENT   (sizeof(long) * 2)
+
+static TEE_Result load_header(const struct shdr *signed_ta,
+		struct shdr **sec_shdr)
+{
+	size_t s;
+
+	if (!tee_vbuf_is_non_sec(signed_ta, sizeof(*signed_ta)))
+		return TEE_ERROR_SECURITY;
+
+	s = SHDR_GET_SIZE(signed_ta);
+	if (!tee_vbuf_is_non_sec(signed_ta, s))
+		return TEE_ERROR_SECURITY;
+
+	/* Copy signed header into secure memory */
+	*sec_shdr = malloc(s);
+	if (!*sec_shdr)
+		return TEE_ERROR_OUT_OF_MEMORY;
+	memcpy(*sec_shdr, signed_ta, s);
+
+	return TEE_SUCCESS;
+}
+
+static TEE_Result check_shdr(struct shdr *shdr)
+{
+	struct rsa_public_key key;
+	TEE_Result res;
+	uint32_t e = TEE_U32_TO_BIG_ENDIAN(ta_pub_key_exponent);
+	size_t hash_size;
+
+	if (shdr->magic != SHDR_MAGIC || shdr->img_type != SHDR_TA)
+		return TEE_ERROR_SECURITY;
+
+	if (TEE_ALG_GET_MAIN_ALG(shdr->algo) != TEE_MAIN_ALGO_RSA)
+		return TEE_ERROR_SECURITY;
+
+	res = tee_hash_get_digest_size(TEE_DIGEST_HASH_TO_ALGO(shdr->algo),
+				       &hash_size);
+	if (res != TEE_SUCCESS)
+		return res;
+	if (hash_size != shdr->hash_size)
+		return TEE_ERROR_SECURITY;
+
+	if (!crypto_ops.acipher.alloc_rsa_public_key ||
+	    !crypto_ops.acipher.free_rsa_public_key ||
+	    !crypto_ops.acipher.rsassa_verify ||
+	    !crypto_ops.bignum.bin2bn)
+		return TEE_ERROR_NOT_SUPPORTED;
+
+	res = crypto_ops.acipher.alloc_rsa_public_key(&key, shdr->sig_size);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = crypto_ops.bignum.bin2bn((uint8_t *)&e, sizeof(e), key.e);
+	if (res != TEE_SUCCESS)
+		goto out;
+	res = crypto_ops.bignum.bin2bn(ta_pub_key_modulus,
+				       ta_pub_key_modulus_size, key.n);
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	res = crypto_ops.acipher.rsassa_verify(shdr->algo, &key, -1,
+				SHDR_GET_HASH(shdr), shdr->hash_size,
+				SHDR_GET_SIG(shdr), shdr->sig_size);
+out:
+	crypto_ops.acipher.free_rsa_public_key(&key);
+	if (res != TEE_SUCCESS)
+		return TEE_ERROR_SECURITY;
+	return TEE_SUCCESS;
+}
+
+static uint32_t elf_flags_to_mattr(uint32_t flags, bool init_attrs)
+{
+	uint32_t mattr = 0;
+
+	if (init_attrs)
+		mattr = TEE_MATTR_PRW;
+	else {
+		if (flags & PF_X)
+			mattr |= TEE_MATTR_UX;
+		if (flags & PF_W)
+			mattr |= TEE_MATTR_UW;
+		if (flags & PF_R)
+			mattr |= TEE_MATTR_UR;
+	}
+
+	return mattr;
+}
+
+#ifdef CFG_PAGED_USER_TA
+static TEE_Result config_initial_paging(struct user_ta_ctx *utc)
+{
+	size_t n;
+
+	for (n = 0; n < ARRAY_SIZE(utc->mmu->regions); n++) {
+		if (!utc->mmu->regions[n].size)
+			continue;
+		if (!tee_pager_add_uta_area(utc, utc->mmu->regions[n].va,
+					    utc->mmu->regions[n].size))
+			return TEE_ERROR_GENERIC;
+	}
+	return TEE_SUCCESS;
+}
+
+static TEE_Result config_final_paging(struct user_ta_ctx *utc)
+{
+	size_t n;
+	uint32_t flags;
+
+	tee_pager_assign_uta_tables(utc);
+
+	for (n = 0; n < ARRAY_SIZE(utc->mmu->regions); n++) {
+		if (!utc->mmu->regions[n].size)
+			continue;
+		flags = utc->mmu->regions[n].attr &
+			(TEE_MATTR_PRW | TEE_MATTR_URWX);
+		if (!tee_pager_set_uta_area_attr(utc, utc->mmu->regions[n].va,
+						 utc->mmu->regions[n].size,
+						 flags))
+			return TEE_ERROR_GENERIC;
+	}
+	return TEE_SUCCESS;
+}
+#else /*!CFG_PAGED_USER_TA*/
+static TEE_Result config_initial_paging(struct user_ta_ctx *utc __unused)
+{
+	return TEE_SUCCESS;
+}
+
+static TEE_Result config_final_paging(struct user_ta_ctx *utc)
+{
+	void *va = (void *)utc->mmu->ta_private_vmem_start;
+	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);
+	return TEE_SUCCESS;
+}
+#endif /*!CFG_PAGED_USER_TA*/
+
+static TEE_Result load_elf_segments(struct user_ta_ctx *utc,
+			struct elf_load_state *elf_state, bool init_attrs)
+{
+	TEE_Result res;
+	uint32_t mattr;
+	size_t idx = 0;
+
+	tee_mmu_map_clear(utc);
+
+	/*
+	 * Add stack segment
+	 */
+	tee_mmu_map_stack(utc, utc->mobj_stack);
+
+	/*
+	 * Add code segment
+	 */
+	while (true) {
+		vaddr_t offs;
+		size_t size;
+		uint32_t flags;
+
+		res = elf_load_get_next_segment(elf_state, &idx, &offs, &size,
+						&flags);
+		if (res == TEE_ERROR_ITEM_NOT_FOUND)
+			break;
+		if (res != TEE_SUCCESS)
+			return res;
+
+		mattr = elf_flags_to_mattr(flags, init_attrs);
+		res = tee_mmu_map_add_segment(utc, utc->mobj_code, offs, size,
+					      mattr);
+		if (res != TEE_SUCCESS)
+			return res;
+	}
+
+	if (init_attrs)
+		return config_initial_paging(utc);
+	else
+		return config_final_paging(utc);
+}
+
+static struct mobj *alloc_ta_mem(size_t size)
+{
+#ifdef CFG_PAGED_USER_TA
+	return mobj_paged_alloc(size);
+#else
+	return mobj_mm_alloc(mobj_sec_ddr, size, &tee_mm_sec_ddr);
+#endif
+}
+
+static TEE_Result load_elf(struct user_ta_ctx *utc, struct shdr *shdr,
+			const struct shdr *nmem_shdr)
+{
+	TEE_Result res;
+	size_t hash_ctx_size;
+	void *hash_ctx = NULL;
+	uint32_t hash_algo;
+	uint8_t *nwdata = (uint8_t *)nmem_shdr + SHDR_GET_SIZE(shdr);
+	size_t nwdata_len = shdr->img_size;
+	void *digest = NULL;
+	struct elf_load_state *elf_state = NULL;
+	struct ta_head *ta_head;
+	void *p;
+	size_t vasize;
+
+	if (!tee_vbuf_is_non_sec(nwdata, nwdata_len))
+		return TEE_ERROR_SECURITY;
+
+	if (!crypto_ops.hash.get_ctx_size || !crypto_ops.hash.init ||
+	    !crypto_ops.hash.update || !crypto_ops.hash.final) {
+		res = TEE_ERROR_NOT_IMPLEMENTED;
+		goto out;
+	}
+	hash_algo = TEE_DIGEST_HASH_TO_ALGO(shdr->algo);
+	res = crypto_ops.hash.get_ctx_size(hash_algo, &hash_ctx_size);
+	if (res != TEE_SUCCESS)
+		goto out;
+	hash_ctx = malloc(hash_ctx_size);
+	if (!hash_ctx) {
+		res = TEE_ERROR_OUT_OF_MEMORY;
+		goto out;
+	}
+	res = crypto_ops.hash.init(hash_ctx, hash_algo);
+	if (res != TEE_SUCCESS)
+		goto out;
+	res = crypto_ops.hash.update(hash_ctx, hash_algo,
+				     (uint8_t *)shdr, sizeof(struct shdr));
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	res = elf_load_init(hash_ctx, hash_algo, nwdata, nwdata_len,
+			    &elf_state);
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	res = elf_load_head(elf_state, sizeof(struct ta_head), &p, &vasize,
+			    &utc->is_32bit);
+	if (res != TEE_SUCCESS)
+		goto out;
+	ta_head = p;
+
+	utc->mobj_code = alloc_ta_mem(vasize);
+	if (!utc->mobj_code) {
+		res = TEE_ERROR_OUT_OF_MEMORY;
+		goto out;
+	}
+
+	/* Currently all TA must execute from DDR */
+	if (!(ta_head->flags & TA_FLAG_EXEC_DDR)) {
+		res = TEE_ERROR_BAD_FORMAT;
+		goto out;
+	}
+	/* Temporary assignment to setup memory mapping */
+	utc->ctx.flags = TA_FLAG_USER_MODE | TA_FLAG_EXEC_DDR;
+
+	/* Ensure proper aligment of stack */
+	utc->mobj_stack = alloc_ta_mem(ROUNDUP(ta_head->stack_size,
+					       STACK_ALIGNMENT));
+	if (!utc->mobj_stack) {
+		res = TEE_ERROR_OUT_OF_MEMORY;
+		goto out;
+	}
+
+	/*
+	 * Map physical memory into TA virtual memory
+	 */
+
+	res = tee_mmu_init(utc);
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	res = load_elf_segments(utc, elf_state, true /* init attrs */);
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	tee_mmu_set_ctx(&utc->ctx);
+
+	res = elf_load_body(elf_state, tee_mmu_get_load_addr(&utc->ctx));
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	digest = malloc(shdr->hash_size);
+	if (!digest) {
+		res = TEE_ERROR_OUT_OF_MEMORY;
+		goto out;
+	}
+
+	res = crypto_ops.hash.final(hash_ctx, hash_algo, digest,
+				    shdr->hash_size);
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	if (memcmp(digest, SHDR_GET_HASH(shdr), shdr->hash_size) != 0) {
+		res = TEE_ERROR_SECURITY;
+		goto out;
+	}
+
+	/*
+	 * Replace the init attributes with attributes used when the TA is
+	 * running.
+	 */
+	res = load_elf_segments(utc, elf_state, false /* final attrs */);
+	if (res != TEE_SUCCESS)
+		goto out;
+
+out:
+	elf_load_final(elf_state);
+	free(digest);
+	free(hash_ctx);
+	return res;
+}
+
+/*-----------------------------------------------------------------------------
+ * Loads TA header and hashes.
+ * Verifies the TA signature.
+ * Returns context ptr and TEE_Result.
+ *---------------------------------------------------------------------------*/
+static TEE_Result ta_load(const TEE_UUID *uuid, const struct shdr *signed_ta,
+			struct tee_ta_ctx **ta_ctx)
+{
+	TEE_Result res;
+	/* man_flags: mandatory flags */
+	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_INSTANCE_KEEP_ALIVE | TA_FLAG_CACHE_MAINTENANCE;
+	struct user_ta_ctx *utc = NULL;
+	struct shdr *sec_shdr = NULL;
+	struct ta_head *ta_head;
+
+	res = load_header(signed_ta, &sec_shdr);
+	if (res != TEE_SUCCESS)
+		goto error_return;
+
+	res = check_shdr(sec_shdr);
+	if (res != TEE_SUCCESS)
+		goto error_return;
+
+	/*
+	 * ------------------------------------------------------------------
+	 * 2nd step: Register context
+	 * Alloc and init the ta context structure, alloc physical/virtual
+	 * memories to store/map the TA.
+	 * ------------------------------------------------------------------
+	 */
+
+	/*
+	 * Register context
+	 */
+
+	/* code below must be protected by mutex (multi-threaded) */
+	utc = calloc(1, sizeof(struct user_ta_ctx));
+	if (!utc) {
+		res = TEE_ERROR_OUT_OF_MEMORY;
+		goto error_return;
+	}
+	TAILQ_INIT(&utc->open_sessions);
+	TAILQ_INIT(&utc->cryp_states);
+	TAILQ_INIT(&utc->objects);
+	TAILQ_INIT(&utc->storage_enums);
+#if defined(CFG_SE_API)
+	utc->se_service = NULL;
+#endif
+
+	res = load_elf(utc, sec_shdr, signed_ta);
+	if (res != TEE_SUCCESS)
+		goto error_return;
+
+	utc->load_addr = tee_mmu_get_load_addr(&utc->ctx);
+	ta_head = (struct ta_head *)(vaddr_t)utc->load_addr;
+
+	if (memcmp(&ta_head->uuid, uuid, sizeof(TEE_UUID)) != 0) {
+		res = TEE_ERROR_SECURITY;
+		goto error_return;
+	}
+
+	/* check input flags bitmask consistency and save flags */
+	if ((ta_head->flags & opt_flags) != ta_head->flags ||
+	    (ta_head->flags & man_flags) != man_flags) {
+		EMSG("TA flag issue: flags=%x opt=%X man=%X",
+		     ta_head->flags, opt_flags, man_flags);
+		res = TEE_ERROR_BAD_FORMAT;
+		goto error_return;
+	}
+
+	utc->ctx.flags = ta_head->flags;
+	utc->ctx.uuid = ta_head->uuid;
+	utc->entry_func = ta_head->entry.ptr64;
+
+	utc->ctx.ref_count = 1;
+
+	condvar_init(&utc->ctx.busy_cv);
+	TAILQ_INSERT_TAIL(&tee_ctxes, &utc->ctx, link);
+	*ta_ctx = &utc->ctx;
+
+	DMSG("ELF load address 0x%x", utc->load_addr);
+
+	tee_mmu_set_ctx(NULL);
+	/* end thread protection (multi-threaded) */
+
+	free(sec_shdr);
+	return TEE_SUCCESS;
+
+error_return:
+	free(sec_shdr);
+	tee_mmu_set_ctx(NULL);
+	if (utc) {
+		pgt_flush_ctx(&utc->ctx);
+		tee_pager_rem_uta_areas(utc);
+		tee_mmu_final(utc);
+		mobj_free(utc->mobj_code);
+		mobj_free(utc->mobj_stack);
+		free(utc);
+	}
+	return res;
+}
+
+static void init_utee_param(struct utee_params *up,
+			const struct tee_ta_param *p, void *va[TEE_NUM_PARAMS])
+{
+	size_t n;
+
+	up->types = p->types;
+	for (n = 0; n < TEE_NUM_PARAMS; n++) {
+		uintptr_t a;
+		uintptr_t b;
+
+		switch (TEE_PARAM_TYPE_GET(p->types, n)) {
+		case TEE_PARAM_TYPE_MEMREF_INPUT:
+		case TEE_PARAM_TYPE_MEMREF_OUTPUT:
+		case TEE_PARAM_TYPE_MEMREF_INOUT:
+			a = (uintptr_t)va[n];
+			b = p->u[n].mem.size;
+			break;
+		case TEE_PARAM_TYPE_VALUE_INPUT:
+		case TEE_PARAM_TYPE_VALUE_INOUT:
+			a = p->u[n].val.a;
+			b = p->u[n].val.b;
+			break;
+		default:
+			a = 0;
+			b = 0;
+			break;
+		}
+		/* See comment for struct utee_params in utee_types.h */
+		up->vals[n * 2] = a;
+		up->vals[n * 2 + 1] = b;
+	}
+}
+
+static void update_from_utee_param(struct tee_ta_param *p,
+			const struct utee_params *up)
+{
+	size_t n;
+
+	for (n = 0; n < TEE_NUM_PARAMS; n++) {
+		switch (TEE_PARAM_TYPE_GET(p->types, n)) {
+		case TEE_PARAM_TYPE_MEMREF_OUTPUT:
+		case TEE_PARAM_TYPE_MEMREF_INOUT:
+			/* See comment for struct utee_params in utee_types.h */
+			p->u[n].mem.size = up->vals[n * 2 + 1];
+			break;
+		case TEE_PARAM_TYPE_VALUE_OUTPUT:
+		case TEE_PARAM_TYPE_VALUE_INOUT:
+			/* See comment for struct utee_params in utee_types.h */
+			p->u[n].val.a = up->vals[n * 2];
+			p->u[n].val.b = up->vals[n * 2 + 1];
+			break;
+		default:
+			break;
+		}
+	}
+}
+
+static void clear_vfp_state(struct user_ta_ctx *utc __unused)
+{
+#ifdef CFG_WITH_VFP
+	thread_user_clear_vfp(&utc->vfp);
+#endif
+}
+
+static TEE_Result user_ta_enter(TEE_ErrorOrigin *err,
+			struct tee_ta_session *session,
+			enum utee_entry_func func, uint32_t cmd,
+			struct tee_ta_param *param)
+{
+	TEE_Result res;
+	struct utee_params *usr_params;
+	uaddr_t usr_stack;
+	struct user_ta_ctx *utc = to_user_ta_ctx(session->ctx);
+	TEE_ErrorOrigin serr = TEE_ORIGIN_TEE;
+	struct tee_ta_session *s __maybe_unused;
+	void *param_va[TEE_NUM_PARAMS] = { NULL };
+
+	if (!(utc->ctx.flags & TA_FLAG_EXEC_DDR))
+		panic("TA does not exec in DDR");
+
+	/* Map user space memory */
+	res = tee_mmu_map_param(utc, param, param_va);
+	if (res != TEE_SUCCESS)
+		goto cleanup_return;
+
+	/* Switch to user ctx */
+	tee_ta_push_current_session(session);
+
+	/* Make room for usr_params at top of stack */
+	usr_stack = (uaddr_t)utc->mmu->regions[0].va + utc->mobj_stack->size;
+	usr_stack -= ROUNDUP(sizeof(struct utee_params), STACK_ALIGNMENT);
+	usr_params = (struct utee_params *)usr_stack;
+	init_utee_param(usr_params, param, param_va);
+
+	res = thread_enter_user_mode(func, tee_svc_kaddr_to_uref(session),
+				     (vaddr_t)usr_params, cmd, usr_stack,
+				     utc->entry_func, utc->is_32bit,
+				     &utc->ctx.panicked, &utc->ctx.panic_code);
+
+	clear_vfp_state(utc);
+	/*
+	 * According to GP spec the origin should allways be set to the
+	 * TA after TA execution
+	 */
+	serr = TEE_ORIGIN_TRUSTED_APP;
+
+	if (utc->ctx.panicked) {
+		DMSG("tee_user_ta_enter: TA panicked with code 0x%x\n",
+		     utc->ctx.panic_code);
+		serr = TEE_ORIGIN_TEE;
+		res = TEE_ERROR_TARGET_DEAD;
+	}
+
+	/* Copy out value results */
+	update_from_utee_param(param, usr_params);
+
+	s = tee_ta_pop_current_session();
+	assert(s == session);
+cleanup_return:
+
+	/*
+	 * Clear the cancel state now that the user TA has returned. The next
+	 * time the TA will be invoked will be with a new operation and should
+	 * not have an old cancellation pending.
+	 */
+	session->cancel = false;
+
+	/*
+	 * Can't update *err until now since it may point to an address
+	 * mapped for the user mode TA.
+	 */
+	*err = serr;
+
+	return res;
+}
+
+/*
+ * Load a TA via RPC with UUID defined by input param uuid. The virtual
+ * address of the TA is recieved in out parameter ta
+ *
+ * Function is not thread safe
+ */
+static TEE_Result rpc_load(const TEE_UUID *uuid, struct shdr **ta,
+			uint64_t *cookie_ta)
+{
+	TEE_Result res;
+	struct optee_msg_param params[2];
+	paddr_t phta = 0;
+	uint64_t cta = 0;
+
+
+	if (!uuid || !ta || !cookie_ta)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	memset(params, 0, sizeof(params));
+	params[0].attr = OPTEE_MSG_ATTR_TYPE_VALUE_INPUT;
+	tee_uuid_to_octets((void *)&params[0].u.value, uuid);
+	params[1].attr = OPTEE_MSG_ATTR_TYPE_TMEM_OUTPUT;
+	params[1].u.tmem.buf_ptr = 0;
+	params[1].u.tmem.size = 0;
+	params[1].u.tmem.shm_ref = 0;
+
+	res = thread_rpc_cmd(OPTEE_MSG_RPC_CMD_LOAD_TA, 2, params);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	thread_rpc_alloc_payload(params[1].u.tmem.size, &phta, &cta);
+	if (!phta)
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	*ta = phys_to_virt(phta, MEM_AREA_NSEC_SHM);
+	if (!*ta) {
+		res = TEE_ERROR_GENERIC;
+		goto out;
+	}
+	*cookie_ta = cta;
+
+	params[0].attr = OPTEE_MSG_ATTR_TYPE_VALUE_INPUT;
+	tee_uuid_to_octets((void *)&params[0].u.value, uuid);
+	params[1].attr = OPTEE_MSG_ATTR_TYPE_TMEM_OUTPUT;
+	params[1].u.tmem.buf_ptr = phta;
+	params[1].u.tmem.shm_ref = cta;
+	/* Note that params[1].u.tmem.size is already assigned */
+
+	res = thread_rpc_cmd(OPTEE_MSG_RPC_CMD_LOAD_TA, 2, params);
+out:
+	if (res != TEE_SUCCESS)
+		thread_rpc_free_payload(cta);
+	return res;
+}
+
+static TEE_Result init_session_with_signed_ta(const TEE_UUID *uuid,
+				const struct shdr *signed_ta,
+				struct tee_ta_session *s)
+{
+	TEE_Result res;
+
+	DMSG("   Load dynamic TA");
+	/* load and verify */
+	res = ta_load(uuid, signed_ta, &s->ctx);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	DMSG("      dyn TA : %pUl", (void *)&s->ctx->uuid);
+
+	return res;
+}
+
+static TEE_Result user_ta_enter_open_session(struct tee_ta_session *s,
+			struct tee_ta_param *param, TEE_ErrorOrigin *eo)
+{
+	return user_ta_enter(eo, s, UTEE_ENTRY_FUNC_OPEN_SESSION, 0, param);
+}
+
+static TEE_Result user_ta_enter_invoke_cmd(struct tee_ta_session *s,
+			uint32_t cmd, struct tee_ta_param *param,
+			TEE_ErrorOrigin *eo)
+{
+	return user_ta_enter(eo, s, UTEE_ENTRY_FUNC_INVOKE_COMMAND, cmd, param);
+}
+
+static void user_ta_enter_close_session(struct tee_ta_session *s)
+{
+	TEE_ErrorOrigin eo;
+	struct tee_ta_param param = { 0 };
+
+	user_ta_enter(&eo, s, UTEE_ENTRY_FUNC_CLOSE_SESSION, 0, &param);
+}
+
+static void user_ta_dump_state(struct tee_ta_ctx *ctx)
+{
+	struct user_ta_ctx *utc __maybe_unused = to_user_ta_ctx(ctx);
+	size_t n;
+
+	EMSG_RAW("- load addr : 0x%x    ctx-idr: %d",
+		 utc->load_addr, utc->context);
+	EMSG_RAW("- stack: 0x%" PRIxVA " %zu",
+		 utc->mmu->regions[0].va, utc->mobj_stack->size);
+	for (n = 0; n < ARRAY_SIZE(utc->mmu->regions); n++) {
+		paddr_t pa = 0;
+
+		if (utc->mmu->regions[n].mobj)
+			mobj_get_pa(utc->mmu->regions[n].mobj,
+				    utc->mmu->regions[n].offset, 0, &pa);
+
+		EMSG_RAW("sect %zu : va %#" PRIxVA " pa %#" PRIxPA " %#zx",
+			 n, utc->mmu->regions[n].va, pa,
+			 utc->mmu->regions[n].size);
+	}
+}
+KEEP_PAGER(user_ta_dump_state);
+
+static void user_ta_ctx_destroy(struct tee_ta_ctx *ctx)
+{
+	struct user_ta_ctx *utc = to_user_ta_ctx(ctx);
+
+	tee_pager_rem_uta_areas(utc);
+
+	/*
+	 * Clean all traces of the TA, both RO and RW data.
+	 * No L2 cache maintenance to avoid sync problems
+	 */
+	if (ctx->flags & TA_FLAG_EXEC_DDR) {
+		void *va;
+
+		if (utc->mobj_code) {
+			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);
+			}
+		}
+
+		if (utc->mobj_stack) {
+			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);
+			}
+		}
+	}
+
+	/*
+	 * Close sessions opened by this TA
+	 * Note that tee_ta_close_session() removes the item
+	 * from the utc->open_sessions list.
+	 */
+	while (!TAILQ_EMPTY(&utc->open_sessions)) {
+		tee_ta_close_session(TAILQ_FIRST(&utc->open_sessions),
+				     &utc->open_sessions, KERN_IDENTITY);
+	}
+
+	tee_mmu_final(utc);
+	mobj_free(utc->mobj_code);
+	mobj_free(utc->mobj_stack);
+
+	/* Free cryp states created by this TA */
+	tee_svc_cryp_free_states(utc);
+	/* Close cryp objects opened by this TA */
+	tee_obj_close_all(utc);
+	/* Free emums created by this TA */
+	tee_svc_storage_close_all_enum(utc);
+	free(utc);
+}
+
+static uint32_t user_ta_get_instance_id(struct tee_ta_ctx *ctx)
+{
+	return to_user_ta_ctx(ctx)->context;
+}
+
+static const struct tee_ta_ops user_ta_ops __rodata_unpaged = {
+	.enter_open_session = user_ta_enter_open_session,
+	.enter_invoke_cmd = user_ta_enter_invoke_cmd,
+	.enter_close_session = user_ta_enter_close_session,
+	.dump_state = user_ta_dump_state,
+	.destroy = user_ta_ctx_destroy,
+	.get_instance_id = user_ta_get_instance_id,
+};
+
+TEE_Result tee_ta_init_user_ta_session(const TEE_UUID *uuid,
+			struct tee_ta_session *s)
+{
+	TEE_Result res;
+	struct shdr *ta = NULL;
+	uint64_t cookie_ta = 0;
+
+
+	/* Request TA from tee-supplicant */
+	res = rpc_load(uuid, &ta, &cookie_ta);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = init_session_with_signed_ta(uuid, ta, s);
+	/*
+	 * Free normal world shared memory now that the TA either has been
+	 * copied into secure memory or the TA failed to be initialized.
+	 */
+	thread_rpc_free_payload(cookie_ta);
+
+	if (res == TEE_SUCCESS)
+		s->ctx->ops = &user_ta_ops;
+	return res;
+}
diff --git a/core/arch/arm/kernel/vfp.c b/core/arch/arm/kernel/vfp.c
new file mode 100644
index 0000000..9903642
--- /dev/null
+++ b/core/arch/arm/kernel/vfp.c
@@ -0,0 +1,149 @@
+/*
+ * 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 <arm.h>
+#include <assert.h>
+#include <kernel/vfp.h>
+#include "vfp_private.h"
+
+#ifdef ARM32
+bool vfp_is_enabled(void)
+{
+	return !!(vfp_read_fpexc() & FPEXC_EN);
+}
+
+void vfp_enable(void)
+{
+	vfp_write_fpexc(vfp_read_fpexc() | FPEXC_EN);
+}
+
+void vfp_disable(void)
+{
+	vfp_write_fpexc(vfp_read_fpexc() & ~FPEXC_EN);
+}
+
+void vfp_lazy_save_state_init(struct vfp_state *state)
+{
+	uint32_t fpexc = vfp_read_fpexc();
+
+	state->fpexc = fpexc;
+	vfp_write_fpexc(fpexc & ~FPEXC_EN);
+}
+
+void vfp_lazy_save_state_final(struct vfp_state *state)
+{
+	if (state->fpexc & FPEXC_EN) {
+		uint32_t fpexc = vfp_read_fpexc();
+
+		assert(!(fpexc & FPEXC_EN));
+		vfp_write_fpexc(fpexc | FPEXC_EN);
+		state->fpscr = vfp_read_fpscr();
+		vfp_save_extension_regs(state->reg);
+		vfp_write_fpexc(fpexc);
+	}
+}
+
+void vfp_lazy_restore_state(struct vfp_state *state, bool full_state)
+{
+
+	if (full_state) {
+		/*
+		 * Only restore VFP registers if they have been touched as they
+		 * otherwise are intact.
+		 */
+
+		/* FPEXC is restored to what's in state->fpexc below */
+		vfp_write_fpexc(vfp_read_fpexc() | FPEXC_EN);
+
+		vfp_write_fpscr(state->fpscr);
+		vfp_restore_extension_regs(state->reg);
+	}
+	vfp_write_fpexc(state->fpexc);
+}
+#endif /* ARM32 */
+
+#ifdef ARM64
+bool vfp_is_enabled(void)
+{
+	return (CPACR_EL1_FPEN(read_cpacr_el1()) & CPACR_EL1_FPEN_EL0EL1);
+}
+
+void vfp_enable(void)
+{
+	uint32_t val = read_cpacr_el1();
+
+	val |= (CPACR_EL1_FPEN_EL0EL1 << CPACR_EL1_FPEN_SHIFT);
+	write_cpacr_el1(val);
+	isb();
+}
+
+void vfp_disable(void)
+{
+	uint32_t val = read_cpacr_el1();
+
+	val &= ~(CPACR_EL1_FPEN_MASK << CPACR_EL1_FPEN_SHIFT);
+	write_cpacr_el1(val);
+	isb();
+}
+
+void vfp_lazy_save_state_init(struct vfp_state *state)
+{
+	state->cpacr_el1 = read_cpacr_el1();
+	vfp_disable();
+}
+
+void vfp_lazy_save_state_final(struct vfp_state *state)
+{
+	if ((CPACR_EL1_FPEN(state->cpacr_el1) & CPACR_EL1_FPEN_EL0EL1) ||
+	    state->force_save) {
+		assert(!vfp_is_enabled());
+		vfp_enable();
+		state->fpcr = read_fpcr();
+		state->fpsr = read_fpsr();
+		vfp_save_extension_regs(state->reg);
+		vfp_disable();
+	}
+}
+
+void vfp_lazy_restore_state(struct vfp_state *state, bool full_state)
+{
+	if (full_state) {
+		/*
+		 * Only restore VFP registers if they have been touched as they
+		 * otherwise are intact.
+		 */
+
+		/* CPACR_EL1 is restored to what's in state->cpacr_el1 below */
+		vfp_enable();
+		write_fpcr(state->fpcr);
+		write_fpsr(state->fpsr);
+		vfp_restore_extension_regs(state->reg);
+	}
+	write_cpacr_el1(state->cpacr_el1);
+	isb();
+}
+#endif /* ARM64 */
diff --git a/core/arch/arm/kernel/vfp_a32.S b/core/arch/arm/kernel/vfp_a32.S
new file mode 100644
index 0000000..6cc3e77
--- /dev/null
+++ b/core/arch/arm/kernel/vfp_a32.S
@@ -0,0 +1,81 @@
+/*
+ * Copyright (c) 2015-2016, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <asm.S>
+#include <kernel/unwind.h>
+
+	.section .text.vfp_asm
+
+/* void vfp_save_extension_regs(uint64_t regs[VFP_NUM_REGS]); */
+FUNC vfp_save_extension_regs , :
+UNWIND(	.fnstart)
+	vstm	r0!, {d0-d15}
+	vstm	r0, {d16-d31}
+	bx	lr
+UNWIND(	.fnend)
+END_FUNC vfp_save_extension_regs
+
+/* void vfp_restore_extension_regs(uint64_t regs[VFP_NUM_REGS]); */
+FUNC vfp_restore_extension_regs , :
+UNWIND(	.fnstart)
+	vldm	r0!, {d0-d15}
+	vldm	r0, {d16-d31}
+	bx	lr
+UNWIND(	.fnend)
+END_FUNC vfp_restore_extension_regs
+
+/* void vfp_write_fpexc(uint32_t fpexc) */
+FUNC vfp_write_fpexc , :
+UNWIND(	.fnstart)
+	vmsr	fpexc, r0
+	bx	lr
+UNWIND(	.fnend)
+END_FUNC vfp_write_fpexc
+
+/* uint32_t vfp_read_fpexc(void) */
+FUNC vfp_read_fpexc , :
+UNWIND(	.fnstart)
+	vmrs	r0, fpexc
+	bx	lr
+UNWIND(	.fnend)
+END_FUNC vfp_read_fpexc
+
+/* void vfp_write_fpscr(uint32_t fpscr) */
+FUNC vfp_write_fpscr , :
+UNWIND(	.fnstart)
+	vmsr	fpscr, r0
+	bx	lr
+UNWIND(	.fnend)
+END_FUNC vfp_write_fpscr
+
+/* uint32_t vfp_read_fpscr(void) */
+FUNC vfp_read_fpscr , :
+UNWIND(	.fnstart)
+	vmrs	r0, fpscr
+	bx	lr
+UNWIND(	.fnend)
+END_FUNC vfp_read_fpscr
diff --git a/core/arch/arm/kernel/vfp_a64.S b/core/arch/arm/kernel/vfp_a64.S
new file mode 100644
index 0000000..53210c5
--- /dev/null
+++ b/core/arch/arm/kernel/vfp_a64.S
@@ -0,0 +1,72 @@
+/*
+ * 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 <asm.S>
+
+	.section .text.vfp_asm
+
+/* void vfp_save_extension_regs(struct vfp_reg regs[VFP_NUM_REGS]); */
+FUNC vfp_save_extension_regs , :
+	stp	q0, q1, [x0, #16 * 0]
+	stp	q2, q3, [x0, #16 * 2]
+	stp	q4, q5, [x0, #16 * 4]
+	stp	q6, q7, [x0, #16 * 6]
+	stp	q8, q9, [x0, #16 * 8]
+	stp	q10, q11, [x0, #16 * 10]
+	stp	q12, q13, [x0, #16 * 12]
+	stp	q14, q15, [x0, #16 * 14]
+	stp	q16, q17, [x0, #16 * 16]
+	stp	q18, q19, [x0, #16 * 18]
+	stp	q20, q21, [x0, #16 * 20]
+	stp	q22, q23, [x0, #16 * 22]
+	stp	q24, q25, [x0, #16 * 24]
+	stp	q26, q27, [x0, #16 * 26]
+	stp	q28, q29, [x0, #16 * 28]
+	stp	q30, q31, [x0, #16 * 30]
+	ret
+END_FUNC vfp_save_extension_regs
+
+/* void vfp_restore_extension_regs(struct vfp_reg regs[VFP_NUM_REGS]); */
+FUNC vfp_restore_extension_regs , :
+	ldp	q0, q1, [x0, #16 * 0]
+	ldp	q2, q3, [x0, #16 * 2]
+	ldp	q4, q5, [x0, #16 * 4]
+	ldp	q6, q7, [x0, #16 * 6]
+	ldp	q8, q9, [x0, #16 * 8]
+	ldp	q10, q11, [x0, #16 * 10]
+	ldp	q12, q13, [x0, #16 * 12]
+	ldp	q14, q15, [x0, #16 * 14]
+	ldp	q16, q17, [x0, #16 * 16]
+	ldp	q18, q19, [x0, #16 * 18]
+	ldp	q20, q21, [x0, #16 * 20]
+	ldp	q22, q23, [x0, #16 * 22]
+	ldp	q24, q25, [x0, #16 * 24]
+	ldp	q26, q27, [x0, #16 * 26]
+	ldp	q28, q29, [x0, #16 * 28]
+	ldp	q30, q31, [x0, #16 * 30]
+	ret
+END_FUNC vfp_restore_extension_regs
diff --git a/core/arch/arm/kernel/vfp_private.h b/core/arch/arm/kernel/vfp_private.h
new file mode 100644
index 0000000..0c0ffba
--- /dev/null
+++ b/core/arch/arm/kernel/vfp_private.h
@@ -0,0 +1,53 @@
+/*
+ * 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.
+ */
+
+#ifndef VFP_PRIVATE
+#define VFP_PRIVATE
+
+#include <kernel/vfp.h>
+
+void vfp_save_extension_regs(struct vfp_reg regs[VFP_NUM_REGS]);
+void vfp_restore_extension_regs(struct vfp_reg regs[VFP_NUM_REGS]);
+void vfp_clear_extension_regs(void);
+
+#ifdef ARM32
+
+#define FPEXC_EN	(1 << 30)
+
+/*
+ * These functions can't be implemented in inline assembly when compiling
+ * for thumb mode, to make it easy always implement then in ARM assembly as
+ * ordinary functions.
+ */
+void vfp_write_fpexc(uint32_t fpexc);
+uint32_t vfp_read_fpexc(void);
+void vfp_write_fpscr(uint32_t fpscr);
+uint32_t vfp_read_fpscr(void);
+
+#endif /* ARM32 */
+
+#endif /*VFP_PRIVATE*/
diff --git a/core/arch/arm/kernel/wait_queue.c b/core/arch/arm/kernel/wait_queue.c
new file mode 100644
index 0000000..a96e0fe
--- /dev/null
+++ b/core/arch/arm/kernel/wait_queue.c
@@ -0,0 +1,225 @@
+/*
+ * Copyright (c) 2015-2016, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+#include <compiler.h>
+#include <types_ext.h>
+#include <tee_api_defines.h>
+#include <string.h>
+#include <optee_msg.h>
+#include <kernel/spinlock.h>
+#include <kernel/wait_queue.h>
+#include <kernel/thread.h>
+#include <trace.h>
+
+static unsigned wq_spin_lock;
+
+
+void wq_init(struct wait_queue *wq)
+{
+	*wq = (struct wait_queue)WAIT_QUEUE_INITIALIZER;
+}
+
+static void wq_rpc(uint32_t func, int id, const void *sync_obj __maybe_unused,
+			const char *fname, int lineno __maybe_unused)
+{
+	uint32_t ret;
+	struct optee_msg_param params;
+	const char *cmd_str __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);
+	else
+		DMSG("%s thread %u %p", cmd_str, id, sync_obj);
+
+	memset(&params, 0, sizeof(params));
+	params.attr = OPTEE_MSG_ATTR_TYPE_VALUE_INPUT;
+	params.u.value.a = func;
+	params.u.value.b = id;
+
+	ret = thread_rpc_cmd(OPTEE_MSG_RPC_CMD_WAIT_QUEUE, 1, &params);
+	if (ret != TEE_SUCCESS)
+		DMSG("%s thread %u ret 0x%x", cmd_str, id, ret);
+}
+
+static void slist_add_tail(struct wait_queue *wq, struct wait_queue_elem *wqe)
+{
+	struct wait_queue_elem *wqe_iter;
+
+	/* Add elem to end of wait queue */
+	wqe_iter = SLIST_FIRST(wq);
+	if (wqe_iter) {
+		while (SLIST_NEXT(wqe_iter, link))
+			wqe_iter = SLIST_NEXT(wqe_iter, link);
+		SLIST_INSERT_AFTER(wqe_iter, wqe, link);
+	} else
+		SLIST_INSERT_HEAD(wq, wqe, link);
+}
+
+void wq_wait_init_condvar(struct wait_queue *wq, struct wait_queue_elem *wqe,
+		struct condvar *cv)
+{
+	uint32_t old_itr_status;
+
+	wqe->handle = thread_get_id();
+	wqe->done = false;
+	wqe->cv = cv;
+
+	old_itr_status = thread_mask_exceptions(THREAD_EXCP_ALL);
+	cpu_spin_lock(&wq_spin_lock);
+
+	slist_add_tail(wq, wqe);
+
+	cpu_spin_unlock(&wq_spin_lock);
+	thread_unmask_exceptions(old_itr_status);
+}
+
+void wq_wait_final(struct wait_queue *wq, struct wait_queue_elem *wqe,
+			const void *sync_obj, 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);
+
+		old_itr_status = thread_mask_exceptions(THREAD_EXCP_ALL);
+		cpu_spin_lock(&wq_spin_lock);
+
+		done = wqe->done;
+		if (done)
+			SLIST_REMOVE(wq, wqe, wait_queue_elem, link);
+
+		cpu_spin_unlock(&wq_spin_lock);
+		thread_unmask_exceptions(old_itr_status);
+	} while (!done);
+}
+
+void wq_wake_one(struct wait_queue *wq, const void *sync_obj,
+			const char *fname, int lineno)
+{
+	uint32_t old_itr_status;
+	struct wait_queue_elem *wqe;
+	int handle = -1;
+	bool do_wakeup = false;
+
+	old_itr_status = thread_mask_exceptions(THREAD_EXCP_ALL);
+	cpu_spin_lock(&wq_spin_lock);
+
+	SLIST_FOREACH(wqe, wq, link) {
+		if (!wqe->cv) {
+			do_wakeup = !wqe->done;
+			wqe->done = true;
+			handle = wqe->handle;
+			break;
+		}
+	}
+
+	cpu_spin_unlock(&wq_spin_lock);
+	thread_unmask_exceptions(old_itr_status);
+
+	if (do_wakeup)
+		wq_rpc(OPTEE_MSG_RPC_WAIT_QUEUE_WAKEUP, handle,
+		       sync_obj, fname, lineno);
+}
+
+void wq_promote_condvar(struct wait_queue *wq, struct condvar *cv,
+			bool only_one, const void *sync_obj __unused,
+			const char *fname, int lineno __maybe_unused)
+{
+	uint32_t old_itr_status;
+	struct wait_queue_elem *wqe;
+
+	if (!cv)
+		return;
+
+	old_itr_status = thread_mask_exceptions(THREAD_EXCP_ALL);
+	cpu_spin_lock(&wq_spin_lock);
+
+	/*
+	 * Find condvar waiter(s) and promote each to an active waiter.
+	 * This is a bit unfair to eventual other active waiters as a
+	 * condvar waiter is added the the queue when waiting for the
+	 * condvar.
+	 */
+	SLIST_FOREACH(wqe, wq, link) {
+		if (wqe->cv == cv) {
+			if (fname)
+				FMSG("promote thread %u %p %s:%d",
+				     wqe->handle, (void *)cv->m, fname, lineno);
+			else
+				FMSG("promote thread %u %p",
+				     wqe->handle, (void *)cv->m);
+
+			wqe->cv = NULL;
+			if (only_one)
+				break;
+		}
+	}
+
+	cpu_spin_unlock(&wq_spin_lock);
+	thread_unmask_exceptions(old_itr_status);
+}
+
+bool wq_have_condvar(struct wait_queue *wq, struct condvar *cv)
+{
+	uint32_t old_itr_status;
+	struct wait_queue_elem *wqe;
+	bool rc = false;
+
+	old_itr_status = thread_mask_exceptions(THREAD_EXCP_ALL);
+	cpu_spin_lock(&wq_spin_lock);
+
+	SLIST_FOREACH(wqe, wq, link) {
+		if (wqe->cv == cv) {
+			rc = true;
+			break;
+		}
+	}
+
+	cpu_spin_unlock(&wq_spin_lock);
+	thread_unmask_exceptions(old_itr_status);
+
+	return rc;
+}
+
+bool wq_is_empty(struct wait_queue *wq)
+{
+	uint32_t old_itr_status;
+	bool ret;
+
+	old_itr_status = thread_mask_exceptions(THREAD_EXCP_ALL);
+	cpu_spin_lock(&wq_spin_lock);
+
+	ret = SLIST_EMPTY(wq);
+
+	cpu_spin_unlock(&wq_spin_lock);
+	thread_unmask_exceptions(old_itr_status);
+
+	return ret;
+}
diff --git a/core/arch/arm/mm/core_mmu.c b/core/arch/arm/mm/core_mmu.c
new file mode 100644
index 0000000..62dda73
--- /dev/null
+++ b/core/arch/arm/mm/core_mmu.c
@@ -0,0 +1,1177 @@
+/*
+ * Copyright (c) 2016, Linaro Limited
+ * 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.
+ */
+
+/*
+ * This core mmu supports static section mapping (1MByte) and finer mapping
+ * with 4k pages.
+ *       It should also allow core to map/unmap (and va/pa) at run-time.
+ */
+
+#include <arm.h>
+#include <assert.h>
+#include <kernel/generic_boot.h>
+#include <kernel/panic.h>
+#include <kernel/tee_l2cc_mutex.h>
+#include <kernel/tee_misc.h>
+#include <kernel/tee_ta_manager.h>
+#include <kernel/thread.h>
+#include <kernel/tz_ssvce.h>
+#include <kernel/tz_ssvce_pl310.h>
+#include <mm/core_memprot.h>
+#include <mm/core_mmu.h>
+#include <mm/mobj.h>
+#include <mm/pgt_cache.h>
+#include <mm/tee_mmu.h>
+#include <mm/tee_pager.h>
+#include <platform_config.h>
+#include <stdlib.h>
+#include <trace.h>
+#include <util.h>
+
+#include "core_mmu_private.h"
+
+#define MAX_MMAP_REGIONS	10
+#define RES_VASPACE_SIZE	(CORE_MMU_PGDIR_SIZE * 10)
+
+/*
+ * These variables are initialized before .bss is cleared. To avoid
+ * resetting them when .bss is cleared we're storing them in .data instead,
+ * even if they initially are zero.
+ */
+
+/* Default NSec shared memory allocated from NSec world */
+unsigned long default_nsec_shm_size __early_bss;
+unsigned long default_nsec_shm_paddr __early_bss;
+
+static struct tee_mmap_region
+	static_memory_map[MAX_MMAP_REGIONS + 1] __early_bss;
+static bool mem_map_inited __early_bss;
+
+static struct tee_mmap_region *map_tee_ram __early_bss;
+static struct tee_mmap_region *map_ta_ram __early_bss;
+static struct tee_mmap_region *map_nsec_shm __early_bss;
+
+/* Define the platform's memory layout. */
+struct memaccess_area {
+	paddr_t paddr;
+	size_t size;
+};
+#define MEMACCESS_AREA(a, s) { .paddr = a, .size = s }
+
+static struct memaccess_area ddr[] = {
+	MEMACCESS_AREA(DRAM0_BASE, DRAM0_SIZE),
+#ifdef DRAM1_BASE
+	MEMACCESS_AREA(DRAM1_BASE, DRAM1_SIZE),
+#endif
+};
+
+static struct memaccess_area secure_only[] = {
+#ifdef TZSRAM_BASE
+	MEMACCESS_AREA(TZSRAM_BASE, TZSRAM_SIZE),
+#endif
+	MEMACCESS_AREA(TZDRAM_BASE, TZDRAM_SIZE),
+};
+
+static struct memaccess_area nsec_shared[] = {
+	MEMACCESS_AREA(CFG_SHMEM_START, CFG_SHMEM_SIZE),
+};
+
+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);
+#ifdef DEVICE0_PA_BASE
+register_phys_mem(DEVICE0_TYPE, DEVICE0_PA_BASE, DEVICE0_SIZE);
+#endif
+#ifdef DEVICE1_PA_BASE
+register_phys_mem(DEVICE1_TYPE, DEVICE1_PA_BASE, DEVICE1_SIZE);
+#endif
+#ifdef DEVICE2_PA_BASE
+register_phys_mem(DEVICE2_TYPE, DEVICE2_PA_BASE, DEVICE2_SIZE);
+#endif
+#ifdef DEVICE3_PA_BASE
+register_phys_mem(DEVICE3_TYPE, DEVICE3_PA_BASE, DEVICE3_SIZE);
+#endif
+#ifdef DEVICE4_PA_BASE
+register_phys_mem(DEVICE4_TYPE, DEVICE4_PA_BASE, DEVICE4_SIZE);
+#endif
+#ifdef DEVICE5_PA_BASE
+register_phys_mem(DEVICE5_TYPE, DEVICE5_PA_BASE, DEVICE5_SIZE);
+#endif
+#ifdef DEVICE6_PA_BASE
+register_phys_mem(DEVICE6_TYPE, DEVICE6_PA_BASE, DEVICE6_SIZE);
+#endif
+
+static bool _pbuf_intersects(struct memaccess_area *a, size_t alen,
+			     paddr_t pa, size_t size)
+{
+	size_t n;
+
+	for (n = 0; n < alen; n++)
+		if (core_is_buffer_intersect(pa, size, a[n].paddr, a[n].size))
+			return true;
+	return false;
+}
+#define pbuf_intersects(a, pa, size) \
+	_pbuf_intersects((a), ARRAY_SIZE(a), (pa), (size))
+
+static bool _pbuf_is_inside(struct memaccess_area *a, size_t alen,
+			    paddr_t pa, size_t size)
+{
+	size_t n;
+
+	for (n = 0; n < alen; n++)
+		if (core_is_buffer_inside(pa, size, a[n].paddr, a[n].size))
+			return true;
+	return false;
+}
+#define pbuf_is_inside(a, pa, size) \
+	_pbuf_is_inside((a), ARRAY_SIZE(a), (pa), (size))
+
+static bool pa_is_in_map(struct tee_mmap_region *map, paddr_t pa)
+{
+	if (!map)
+		return false;
+	return (pa >= map->pa && pa <= (map->pa + map->size - 1));
+}
+
+static bool va_is_in_map(struct tee_mmap_region *map, vaddr_t va)
+{
+	if (!map)
+		return false;
+	return (va >= map->va && va <= (map->va + map->size - 1));
+}
+
+/* check if target buffer fits in a core default map area */
+static bool pbuf_inside_map_area(unsigned long p, size_t l,
+				 struct tee_mmap_region *map)
+{
+	return core_is_buffer_inside(p, l, map->pa, map->size);
+}
+
+static struct tee_mmap_region *find_map_by_type(enum teecore_memtypes type)
+{
+	struct tee_mmap_region *map;
+
+	for (map = static_memory_map; map->type != MEM_AREA_NOTYPE; map++)
+		if (map->type == type)
+			return map;
+	return NULL;
+}
+
+static struct tee_mmap_region *find_map_by_type_and_pa(
+			enum teecore_memtypes type, paddr_t pa)
+{
+	struct tee_mmap_region *map;
+
+	for (map = static_memory_map; map->type != MEM_AREA_NOTYPE; map++) {
+		if (map->type != type)
+			continue;
+		if (pa_is_in_map(map, pa))
+			return map;
+	}
+	return NULL;
+}
+
+static struct tee_mmap_region *find_map_by_va(void *va)
+{
+	struct tee_mmap_region *map = static_memory_map;
+	unsigned long a = (unsigned long)va;
+
+	while (map->type != MEM_AREA_NOTYPE) {
+		if ((a >= map->va) && (a <= (map->va - 1 + map->size)))
+			return map;
+		map++;
+	}
+	return NULL;
+}
+
+static struct tee_mmap_region *find_map_by_pa(unsigned long pa)
+{
+	struct tee_mmap_region *map = static_memory_map;
+
+	while (map->type != MEM_AREA_NOTYPE) {
+		if ((pa >= map->pa) && (pa < (map->pa + map->size)))
+			return map;
+		map++;
+	}
+	return NULL;
+}
+
+extern const struct core_mmu_phys_mem __start_phys_mem_map_section;
+extern const struct core_mmu_phys_mem __end_phys_mem_map_section;
+
+static void add_phys_mem(struct tee_mmap_region *memory_map, size_t num_elems,
+			 const struct core_mmu_phys_mem *mem, size_t *last)
+{
+	size_t n = 0;
+	paddr_t pa;
+	size_t size;
+
+	/*
+	 * When all entries are added we'd like to have it in a sorted
+	 * array first based on memory type and secondly on physical
+	 * address. If some ranges of memory of the same type overlaps of
+	 * are next to each others they are coalesced into one entry. This
+	 * makes it easier later when building the translation tables.
+	 *
+	 * Note that it's valid to have the same physical memory as several
+	 * different memory types, for instance the same device memory
+	 * mapped as both secure and non-secure. This will probably not
+	 * happen often in practice.
+	 */
+	DMSG("%s %d 0x%08" PRIxPA " size 0x%08zx",
+	     mem->name, mem->type, mem->addr, mem->size);
+	while (true) {
+		if (n >= (num_elems - 1)) {
+			EMSG("Out of entries (%zu) in memory_map", num_elems);
+			panic();
+		}
+		if (n == *last)
+			break;
+		pa = memory_map[n].pa;
+		size = memory_map[n].size;
+		if (mem->addr >= pa && mem->addr <= (pa + (size - 1)) &&
+		    mem->type == memory_map[n].type) {
+			DMSG("Physical mem map overlaps 0x%" PRIxPA, mem->addr);
+			memory_map[n].pa = MIN(pa, mem->addr);
+			memory_map[n].size = MAX(size, mem->size) +
+					     (pa - memory_map[n].pa);
+			return;
+		}
+		if (mem->type < memory_map[n].type ||
+		    (mem->type == memory_map[n].type && mem->addr < pa))
+			break; /* found the spot where to inseart this memory */
+		n++;
+	}
+
+	memmove(memory_map + n + 1, memory_map + n,
+		sizeof(struct tee_mmap_region) * (*last - n));
+	(*last)++;
+	memset(memory_map + n, 0, sizeof(memory_map[0]));
+	memory_map[n].type = mem->type;
+	memory_map[n].pa = mem->addr;
+	memory_map[n].size = mem->size;
+}
+
+static void add_va_space(struct tee_mmap_region *memory_map, size_t num_elems,
+			 unsigned int type, size_t size, size_t *last) {
+	size_t n = 0;
+
+	DMSG("type %d size 0x%08zx", type, size);
+	while (true) {
+		if (n >= (num_elems - 1)) {
+			EMSG("Out of entries (%zu) in memory_map", num_elems);
+			panic();
+		}
+		if (n == *last)
+			break;
+		if (type < memory_map[n].type)
+			break;
+		n++;
+	}
+
+	memmove(memory_map + n + 1, memory_map + n,
+		sizeof(struct tee_mmap_region) * (*last - n));
+	(*last)++;
+	memset(memory_map + n, 0, sizeof(memory_map[0]));
+	memory_map[n].type = type;
+	memory_map[n].size = size;
+}
+
+uint32_t core_mmu_type_to_attr(enum teecore_memtypes t)
+{
+	const uint32_t attr = TEE_MATTR_VALID_BLOCK | TEE_MATTR_PRW |
+			      TEE_MATTR_GLOBAL;
+	const uint32_t cached = TEE_MATTR_CACHE_CACHED << TEE_MATTR_CACHE_SHIFT;
+	const uint32_t noncache = TEE_MATTR_CACHE_NONCACHE <<
+				  TEE_MATTR_CACHE_SHIFT;
+
+	switch (t) {
+	case MEM_AREA_TEE_RAM:
+		return attr | TEE_MATTR_SECURE | TEE_MATTR_PX | cached;
+	case MEM_AREA_TA_RAM:
+		return attr | TEE_MATTR_SECURE | cached;
+	case MEM_AREA_NSEC_SHM:
+		return attr | cached;
+	case MEM_AREA_IO_NSEC:
+		return attr | noncache;
+	case MEM_AREA_IO_SEC:
+		return attr | TEE_MATTR_SECURE | noncache;
+	case MEM_AREA_RAM_NSEC:
+		return attr | cached;
+	case MEM_AREA_RAM_SEC:
+		return attr | TEE_MATTR_SECURE | cached;
+	case MEM_AREA_RES_VASPACE:
+		return 0;
+	default:
+		panic("invalid type");
+	}
+}
+
+static void init_mem_map(struct tee_mmap_region *memory_map, size_t num_elems)
+{
+	const struct core_mmu_phys_mem *mem;
+	struct tee_mmap_region *map;
+	size_t last = 0;
+	vaddr_t va;
+	size_t n;
+
+	for (mem = &__start_phys_mem_map_section;
+	     mem < &__end_phys_mem_map_section; mem++) {
+		struct core_mmu_phys_mem m = *mem;
+
+		if (m.type == MEM_AREA_IO_NSEC || m.type == MEM_AREA_IO_SEC) {
+			m.addr = ROUNDDOWN(m.addr, CORE_MMU_PGDIR_SIZE);
+			m.size = ROUNDUP(m.size + (mem->addr - m.addr),
+					 CORE_MMU_PGDIR_SIZE);
+		}
+		add_phys_mem(memory_map, num_elems, &m, &last);
+	}
+
+	add_va_space(memory_map, num_elems, MEM_AREA_RES_VASPACE,
+		     RES_VASPACE_SIZE, &last);
+
+	memory_map[last].type = MEM_AREA_NOTYPE;
+
+	/*
+	 * Assign region sizes, note that MEM_AREA_TEE_RAM always uses
+	 * SMALL_PAGE_SIZE if paging is enabled.
+	 */
+	for (map = memory_map; map->type != MEM_AREA_NOTYPE; map++) {
+		paddr_t mask = map->pa | map->size;
+
+		if (!(mask & CORE_MMU_PGDIR_MASK))
+			map->region_size = CORE_MMU_PGDIR_SIZE;
+		else if (!(mask & SMALL_PAGE_MASK))
+			map->region_size = SMALL_PAGE_SIZE;
+		else
+			panic("Impossible memory alignment");
+	}
+
+	/*
+	 * bootcfg_memory_map is sorted in order first by type and last by
+	 * address. This puts TEE_RAM first and TA_RAM second
+	 *
+	 */
+	map = memory_map;
+	assert(map->type == MEM_AREA_TEE_RAM);
+	map->va = map->pa;
+#ifdef CFG_WITH_PAGER
+	map->region_size = SMALL_PAGE_SIZE,
+#endif
+	map->attr = core_mmu_type_to_attr(map->type);
+
+
+	if (core_mmu_place_tee_ram_at_top(map->pa)) {
+		va = map->va;
+		map++;
+		while (map->type != MEM_AREA_NOTYPE) {
+			map->attr = core_mmu_type_to_attr(map->type);
+			va -= map->size;
+			map->va = va;
+			map++;
+		}
+		/*
+		 * The memory map should be sorted by virtual address
+		 * when this function returns. As we're assigning va in
+		 * the oposite direction we need to reverse the list.
+		 */
+		for (n = 0; n < last / 2; n++) {
+			struct tee_mmap_region r;
+
+			r = memory_map[last - n - 1];
+			memory_map[last - n - 1] = memory_map[n];
+			memory_map[n] = r;
+		}
+	} else {
+		va = ROUNDUP(map->va + map->size, CORE_MMU_PGDIR_SIZE);
+		map++;
+		while (map->type != MEM_AREA_NOTYPE) {
+			map->attr = core_mmu_type_to_attr(map->type);
+			map->va = va;
+			va += map->size;
+			map++;
+		}
+	}
+
+	for (map = memory_map; map->type != MEM_AREA_NOTYPE; map++) {
+		vaddr_t __maybe_unused vstart;
+
+		vstart = map->va + ((vaddr_t)map->pa & (map->region_size - 1));
+		DMSG("type va %d 0x%08" PRIxVA "..0x%08" PRIxVA
+		     " pa 0x%08" PRIxPA "..0x%08" PRIxPA " size %#zx",
+		     map->type, vstart, vstart + map->size - 1,
+		     (paddr_t)map->pa, (paddr_t)map->pa + map->size - 1,
+		     map->size);
+	}
+}
+
+/*
+ * core_init_mmu_map - init tee core default memory mapping
+ *
+ * this routine sets the static default tee core mapping.
+ *
+ * If an error happend: core_init_mmu_map is expected to reset.
+ */
+void core_init_mmu_map(void)
+{
+	struct tee_mmap_region *map;
+	size_t n;
+
+	for (n = 0; n < ARRAY_SIZE(secure_only); n++) {
+		if (pbuf_intersects(nsec_shared, secure_only[n].paddr,
+				    secure_only[n].size))
+			panic("Invalid memory access config: sec/nsec");
+	}
+
+	if (!mem_map_inited)
+		init_mem_map(static_memory_map, ARRAY_SIZE(static_memory_map));
+
+	map = static_memory_map;
+	while (map->type != MEM_AREA_NOTYPE) {
+		switch (map->type) {
+		case MEM_AREA_TEE_RAM:
+			if (!pbuf_is_inside(secure_only, map->pa, map->size))
+				panic("TEE_RAM can't fit in secure_only");
+
+			map_tee_ram = map;
+			break;
+		case MEM_AREA_TA_RAM:
+			if (!pbuf_is_inside(secure_only, map->pa, map->size))
+				panic("TA_RAM can't fit in secure_only");
+			map_ta_ram = map;
+			break;
+		case MEM_AREA_NSEC_SHM:
+			if (!pbuf_is_inside(nsec_shared, map->pa, map->size))
+				panic("NS_SHM can't fit in nsec_shared");
+			map_nsec_shm = map;
+			break;
+		case MEM_AREA_IO_SEC:
+		case MEM_AREA_IO_NSEC:
+		case MEM_AREA_RAM_SEC:
+		case MEM_AREA_RAM_NSEC:
+		case MEM_AREA_RES_VASPACE:
+			break;
+		default:
+			EMSG("Uhandled memtype %d", map->type);
+			panic();
+		}
+		map++;
+	}
+
+	/* Check that we have the mandatory memory areas defined */
+	if (!map_tee_ram || !map_ta_ram || !map_nsec_shm)
+		panic("mandatory area(s) not found");
+
+	core_init_mmu_tables(static_memory_map);
+}
+
+/* routines to retrieve shared mem configuration */
+bool core_mmu_is_shm_cached(void)
+{
+	if (!map_nsec_shm)
+		return false;
+	return map_nsec_shm->attr >> TEE_MATTR_CACHE_SHIFT ==
+	       TEE_MATTR_CACHE_CACHED;
+}
+
+bool core_mmu_mattr_is_ok(uint32_t mattr)
+{
+	/*
+	 * Keep in sync with core_mmu_lpae.c:mattr_to_desc and
+	 * core_mmu_v7.c:mattr_to_texcb
+	 */
+
+	switch ((mattr >> TEE_MATTR_CACHE_SHIFT) & TEE_MATTR_CACHE_MASK) {
+	case TEE_MATTR_CACHE_NONCACHE:
+	case TEE_MATTR_CACHE_CACHED:
+		return true;
+	default:
+		return false;
+	}
+}
+
+/*
+ * test attributes of target physical buffer
+ *
+ * Flags: pbuf_is(SECURE, NOT_SECURE, RAM, IOMEM, KEYVAULT).
+ *
+ */
+bool core_pbuf_is(uint32_t attr, paddr_t pbuf, size_t len)
+{
+	struct tee_mmap_region *map;
+
+	/* Empty buffers complies with anything */
+	if (len == 0)
+		return true;
+
+	switch (attr) {
+	case CORE_MEM_SEC:
+		return pbuf_is_inside(secure_only, pbuf, len);
+	case CORE_MEM_NON_SEC:
+		return pbuf_is_inside(nsec_shared, pbuf, len);
+	case CORE_MEM_TEE_RAM:
+		return pbuf_inside_map_area(pbuf, len, map_tee_ram);
+	case CORE_MEM_TA_RAM:
+		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_EXTRAM:
+		return pbuf_is_inside(ddr, pbuf, len);
+	case CORE_MEM_CACHED:
+		map = find_map_by_pa(pbuf);
+		if (map == NULL || !pbuf_inside_map_area(pbuf, len, map))
+			return false;
+		return map->attr >> TEE_MATTR_CACHE_SHIFT ==
+		       TEE_MATTR_CACHE_CACHED;
+	default:
+		return false;
+	}
+}
+
+/* test attributes of target virtual buffer (in core mapping) */
+bool core_vbuf_is(uint32_t attr, const void *vbuf, size_t len)
+{
+	paddr_t p;
+
+	/* Empty buffers complies with anything */
+	if (len == 0)
+		return true;
+
+	p = virt_to_phys((void *)vbuf);
+	if (!p)
+		return false;
+
+	return core_pbuf_is(attr, p, len);
+}
+
+
+/* core_va2pa - teecore exported service */
+int core_va2pa_helper(void *va, paddr_t *pa)
+{
+	struct tee_mmap_region *map;
+
+	map = find_map_by_va(va);
+	if (!va_is_in_map(map, (vaddr_t)va))
+		return -1;
+
+	*pa = ((uintptr_t)va & (map->region_size - 1)) |
+	    ((map->pa + (uintptr_t)va - map->va) & ~(map->region_size - 1));
+	return 0;
+}
+
+static void *map_pa2va(struct tee_mmap_region *map, paddr_t pa)
+{
+	if (!pa_is_in_map(map, pa))
+		return NULL;
+	return (void *)((pa & (map->region_size - 1)) |
+		((map->va + pa - map->pa) & ~((vaddr_t)map->region_size - 1)));
+}
+
+/*
+ * teecore gets some memory area definitions
+ */
+void core_mmu_get_mem_by_type(unsigned int type, vaddr_t *s, vaddr_t *e)
+{
+	struct tee_mmap_region *map = find_map_by_type(type);
+
+	if (map) {
+		*s = map->va;
+		*e = map->va + map->size;
+	} else {
+		*s = 0;
+		*e = 0;
+	}
+}
+
+enum teecore_memtypes core_mmu_get_type_by_pa(paddr_t pa)
+{
+	struct tee_mmap_region *map = find_map_by_pa(pa);
+
+	if (!map)
+		return MEM_AREA_NOTYPE;
+	return map->type;
+}
+
+int core_tlb_maintenance(int op, unsigned int a)
+{
+	/*
+	 * We're doing TLB invalidation because we've changed mapping.
+	 * The dsb() makes sure that written data is visible.
+	 */
+	dsb();
+
+	switch (op) {
+	case TLBINV_UNIFIEDTLB:
+		secure_mmu_unifiedtlbinvall();
+		break;
+	case TLBINV_CURRENT_ASID:
+		secure_mmu_unifiedtlbinv_curasid();
+		break;
+	case TLBINV_BY_ASID:
+		secure_mmu_unifiedtlbinv_byasid(a);
+		break;
+	case TLBINV_BY_MVA:
+		EMSG("TLB_INV_SECURE_MVA is not yet supported!");
+		while (1)
+			;
+		secure_mmu_unifiedtlbinvbymva(a);
+		break;
+	default:
+		return 1;
+	}
+	return 0;
+}
+
+unsigned int cache_maintenance_l1(int op, void *va, size_t len)
+{
+	switch (op) {
+	case DCACHE_CLEAN:
+		arm_cl1_d_cleanbysetway();
+		break;
+	case DCACHE_AREA_CLEAN:
+		if (len)
+			arm_cl1_d_cleanbyva(va, (char *)va + len - 1);
+		break;
+	case DCACHE_INVALIDATE:
+		arm_cl1_d_invbysetway();
+		break;
+	case DCACHE_AREA_INVALIDATE:
+		if (len)
+			arm_cl1_d_invbyva(va, (char *)va + len - 1);
+		break;
+	case ICACHE_INVALIDATE:
+		arm_cl1_i_inv_all();
+		break;
+	case ICACHE_AREA_INVALIDATE:
+		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;
+	case DCACHE_AREA_CLEAN_INV:
+		if (len)
+			arm_cl1_d_cleaninvbyva(va, (char *)va + len - 1);
+		break;
+	default:
+		return TEE_ERROR_NOT_IMPLEMENTED;
+	}
+	return TEE_SUCCESS;
+}
+
+#ifdef CFG_PL310
+unsigned int cache_maintenance_l2(int op, paddr_t pa, size_t len)
+{
+	unsigned int ret = TEE_SUCCESS;
+	uint32_t exceptions = thread_mask_exceptions(THREAD_EXCP_IRQ);
+
+	tee_l2cc_mutex_lock();
+	switch (op) {
+	case L2CACHE_INVALIDATE:
+		arm_cl2_invbyway(pl310_base());
+		break;
+	case L2CACHE_AREA_INVALIDATE:
+		if (len)
+			arm_cl2_invbypa(pl310_base(), pa, pa + len - 1);
+		break;
+	case L2CACHE_CLEAN:
+		arm_cl2_cleanbyway(pl310_base());
+		break;
+	case L2CACHE_AREA_CLEAN:
+		if (len)
+			arm_cl2_cleanbypa(pl310_base(), pa, pa + len - 1);
+		break;
+	case L2CACHE_CLEAN_INV:
+		arm_cl2_cleaninvbyway(pl310_base());
+		break;
+	case L2CACHE_AREA_CLEAN_INV:
+		if (len)
+			arm_cl2_cleaninvbypa(pl310_base(), pa, pa + len - 1);
+		break;
+	default:
+		ret = TEE_ERROR_NOT_IMPLEMENTED;
+	}
+
+	tee_l2cc_mutex_unlock();
+	thread_set_exceptions(exceptions);
+	return ret;
+}
+#endif /*CFG_PL310*/
+
+void core_mmu_set_entry(struct core_mmu_table_info *tbl_info, unsigned idx,
+			paddr_t pa, uint32_t attr)
+{
+	assert(idx < tbl_info->num_entries);
+	core_mmu_set_entry_primitive(tbl_info->table, tbl_info->level,
+				     idx, pa, attr);
+}
+
+void core_mmu_get_entry(struct core_mmu_table_info *tbl_info, unsigned idx,
+			paddr_t *pa, uint32_t *attr)
+{
+	assert(idx < tbl_info->num_entries);
+	core_mmu_get_entry_primitive(tbl_info->table, tbl_info->level,
+				     idx, pa, attr);
+}
+
+static void set_region(struct core_mmu_table_info *tbl_info,
+		struct tee_mmap_region *region)
+{
+	unsigned end;
+	unsigned idx;
+	paddr_t pa;
+
+	/* va, len and pa should be block aligned */
+	assert(!core_mmu_get_block_offset(tbl_info, region->va));
+	assert(!core_mmu_get_block_offset(tbl_info, region->size));
+	assert(!core_mmu_get_block_offset(tbl_info, region->pa));
+
+	idx = core_mmu_va2idx(tbl_info, region->va);
+	end = core_mmu_va2idx(tbl_info, region->va + region->size);
+	pa = region->pa;
+
+	while (idx < end) {
+		core_mmu_set_entry(tbl_info, idx, pa, region->attr);
+		idx++;
+		pa += 1 << tbl_info->shift;
+	}
+}
+
+#ifdef CFG_SMALL_PAGE_USER_TA
+static void set_pg_region(struct core_mmu_table_info *dir_info,
+			struct tee_ta_region *region, struct pgt **pgt,
+			struct core_mmu_table_info *pg_info)
+{
+	struct tee_mmap_region r = {
+		.va = region->va,
+		.size = region->size,
+		.attr = region->attr,
+	};
+	vaddr_t end = r.va + r.size;
+	uint32_t pgt_attr = (r.attr & TEE_MATTR_SECURE) | TEE_MATTR_TABLE;
+
+	while (r.va < end) {
+		if (!pg_info->table ||
+		     r.va >= (pg_info->va_base + CORE_MMU_PGDIR_SIZE)) {
+			/*
+			 * We're assigning a new translation table.
+			 */
+			unsigned int idx;
+
+			assert(*pgt); /* We should have alloced enough */
+
+			/* Virtual addresses must grow */
+			assert(r.va > pg_info->va_base);
+
+			idx = core_mmu_va2idx(dir_info, r.va);
+			pg_info->table = (*pgt)->tbl;
+			pg_info->va_base = core_mmu_idx2va(dir_info, idx);
+#ifdef CFG_PAGED_USER_TA
+			assert((*pgt)->vabase == pg_info->va_base);
+#endif
+			*pgt = SLIST_NEXT(*pgt, link);
+
+			core_mmu_set_entry(dir_info, idx,
+					   virt_to_phys(pg_info->table),
+					   pgt_attr);
+		}
+
+		r.size = MIN(CORE_MMU_PGDIR_SIZE - (r.va - pg_info->va_base),
+			     end - r.va);
+		if (!mobj_is_paged(region->mobj)) {
+			size_t granule = BIT(pg_info->shift);
+			size_t offset = r.va - region->va + region->offset;
+
+			if (mobj_get_pa(region->mobj, offset, granule,
+					&r.pa) != TEE_SUCCESS)
+				panic("Failed to get PA of unpaged mobj");
+			set_region(pg_info, &r);
+		}
+		r.va += r.size;
+	}
+}
+
+void core_mmu_populate_user_map(struct core_mmu_table_info *dir_info,
+				struct user_ta_ctx *utc)
+{
+	struct core_mmu_table_info pg_info;
+	struct pgt_cache *pgt_cache = &thread_get_tsd()->pgt_cache;
+	struct pgt *pgt;
+	size_t n;
+
+	/* Find the last valid entry */
+	n = ARRAY_SIZE(utc->mmu->regions);
+	while (true) {
+		n--;
+		if (utc->mmu->regions[n].size)
+			break;
+		if (!n)
+			return;	/* Nothing to map */
+	}
+
+	/*
+	 * Allocate all page tables in advance.
+	 */
+	pgt_alloc(pgt_cache, &utc->ctx, utc->mmu->regions[0].va,
+		  utc->mmu->regions[n].va + utc->mmu->regions[n].size - 1);
+	pgt = SLIST_FIRST(pgt_cache);
+
+	core_mmu_set_info_table(&pg_info, dir_info->level + 1, 0, NULL);
+
+	for (n = 0; n < ARRAY_SIZE(utc->mmu->regions); n++)
+		mobj_update_mapping(utc->mmu->regions[n].mobj, utc,
+				    utc->mmu->regions[n].va);
+
+	for (n = 0; n < ARRAY_SIZE(utc->mmu->regions); n++) {
+		if (!utc->mmu->regions[n].size)
+			continue;
+		set_pg_region(dir_info, utc->mmu->regions + n, &pgt, &pg_info);
+	}
+}
+
+#else
+void core_mmu_populate_user_map(struct core_mmu_table_info *dir_info,
+				struct user_ta_ctx *utc)
+{
+	unsigned n;
+	struct tee_mmap_region r;
+	size_t offset;
+	size_t granule = BIT(dir_info->shift);
+
+	memset(&r, 0, sizeof(r));
+	for (n = 0; n < ARRAY_SIZE(utc->mmu->regions); n++) {
+		if (!utc->mmu->regions[n].size)
+			continue;
+
+		offset = utc->mmu->regions[n].offset;
+		r.va = utc->mmu->regions[n].va;
+		r.size = utc->mmu->regions[n].size;
+		r.attr = utc->mmu->regions[n].attr;
+
+		if (mobj_get_pa(utc->mmu->regions[n].mobj, offset, granule,
+				&r.pa) != TEE_SUCCESS)
+			panic("Failed to get PA of unpaged mobj");
+
+		set_region(dir_info, &r);
+	}
+}
+#endif
+
+bool core_mmu_add_mapping(enum teecore_memtypes type, paddr_t addr, size_t len)
+{
+	struct core_mmu_table_info tbl_info;
+	struct tee_mmap_region *map;
+	size_t n;
+	size_t granule;
+	paddr_t p;
+	size_t l;
+
+	if (!len)
+		return true;
+
+	/* Check if the memory is already mapped */
+	map = find_map_by_type_and_pa(type, addr);
+	if (map && pbuf_inside_map_area(addr, len, map))
+		return true;
+
+	/* Find the reserved va space used for late mappings */
+	map = find_map_by_type(MEM_AREA_RES_VASPACE);
+	if (!map)
+		return false;
+
+	if (!core_mmu_find_table(map->va, UINT_MAX, &tbl_info))
+		return false;
+
+	granule = 1 << tbl_info.shift;
+	p = ROUNDDOWN(addr, granule);
+	l = ROUNDUP(len + addr - p, granule);
+	/*
+	 * Something is wrong, we can't fit the va range into the selected
+	 * table. The reserved va range is possibly missaligned with
+	 * granule.
+	 */
+	if (core_mmu_va2idx(&tbl_info, map->va + len) >= tbl_info.num_entries)
+		return false;
+
+	/* Find end of the memory map */
+	n = 0;
+	while (static_memory_map[n].type != MEM_AREA_NOTYPE)
+		n++;
+
+	if (n < (ARRAY_SIZE(static_memory_map) - 1)) {
+		/* There's room for another entry */
+		static_memory_map[n].va = map->va;
+		static_memory_map[n].size = l;
+		static_memory_map[n + 1].type = MEM_AREA_NOTYPE;
+		map->va += l;
+		map->size -= l;
+		map = static_memory_map + n;
+	} else {
+		/*
+		 * There isn't room for another entry, steal the reserved
+		 * entry as it's not useful for anything else any longer.
+		 */
+		map->size = l;
+	}
+	map->type = type;
+	map->region_size = granule;
+	map->attr = core_mmu_type_to_attr(type);
+	map->pa = p;
+
+	set_region(&tbl_info, map);
+	return true;
+}
+
+static bool arm_va2pa_helper(void *va, paddr_t *pa)
+{
+	uint32_t exceptions = thread_mask_exceptions(THREAD_EXCP_ALL);
+	paddr_t par;
+	paddr_t par_pa_mask;
+	bool ret = false;
+
+#ifdef ARM32
+	write_ats1cpr((vaddr_t)va);
+	isb();
+#ifdef CFG_WITH_LPAE
+	par = read_par64();
+	par_pa_mask = PAR64_PA_MASK;
+#else
+	par = read_par32();
+	par_pa_mask = PAR32_PA_MASK;
+#endif
+#endif /*ARM32*/
+
+#ifdef ARM64
+	write_at_s1e1r((vaddr_t)va);
+	isb();
+	par = read_par_el1();
+	par_pa_mask = PAR_PA_MASK;
+#endif
+	if (par & PAR_F)
+		goto out;
+	*pa = (par & (par_pa_mask << PAR_PA_SHIFT)) |
+		((vaddr_t)va & ((1 << PAR_PA_SHIFT) - 1));
+
+	ret = true;
+out:
+	thread_unmask_exceptions(exceptions);
+	return ret;
+}
+
+#ifdef CFG_WITH_PAGER
+static vaddr_t get_linear_map_end(void)
+{
+	/* this is synced with the generic linker file kern.ld.S */
+	return (vaddr_t)__heap2_end;
+}
+#endif
+
+#if defined(CFG_TEE_CORE_DEBUG)
+static void check_pa_matches_va(void *va, paddr_t pa)
+{
+	TEE_Result res;
+	vaddr_t v = (vaddr_t)va;
+	paddr_t p = 0;
+
+	if (core_mmu_user_va_range_is_defined()) {
+		vaddr_t user_va_base;
+		size_t user_va_size;
+
+		core_mmu_get_user_va_range(&user_va_base, &user_va_size);
+		if (v >= user_va_base &&
+		    v <= (user_va_base - 1 + user_va_size)) {
+			if (!core_mmu_user_mapping_is_active()) {
+				if (pa)
+					panic("issue in linear address space");
+				return;
+			}
+
+			res = tee_mmu_user_va2pa_helper(
+				to_user_ta_ctx(tee_mmu_get_ctx()), va, &p);
+			if (res == TEE_SUCCESS && pa != p)
+				panic("bad pa");
+			if (res != TEE_SUCCESS && pa)
+				panic("false pa");
+			return;
+		}
+	}
+#ifdef CFG_WITH_PAGER
+	if (v >= CFG_TEE_LOAD_ADDR && v < get_linear_map_end()) {
+		if (v != pa)
+			panic("issue in linear address space");
+		return;
+	}
+	if (v >= (CFG_TEE_LOAD_ADDR & ~CORE_MMU_PGDIR_MASK) &&
+	    v <= (CFG_TEE_LOAD_ADDR | CORE_MMU_PGDIR_MASK)) {
+		struct core_mmu_table_info *ti = &tee_pager_tbl_info;
+		uint32_t a;
+
+		/*
+		 * Lookups in the page table managed by the pager is
+		 * dangerous for addresses in the paged area as those pages
+		 * changes all the time. But some ranges are safe,
+		 * rw-locked areas when the page is populated for instance.
+		 */
+		core_mmu_get_entry(ti, core_mmu_va2idx(ti, v), &p, &a);
+		if (a & TEE_MATTR_VALID_BLOCK) {
+			paddr_t mask = ((1 << ti->shift) - 1);
+
+			p |= v & mask;
+			if (pa != p)
+				panic();
+		} else
+			if (pa)
+				panic();
+		return;
+	}
+#endif
+	if (!core_va2pa_helper(va, &p)) {
+		if (pa != p)
+			panic();
+	} else {
+		if (pa)
+			panic();
+	}
+}
+#else
+static void check_pa_matches_va(void *va __unused, paddr_t pa __unused)
+{
+}
+#endif
+
+paddr_t virt_to_phys(void *va)
+{
+	paddr_t pa;
+
+	if (!arm_va2pa_helper(va, &pa))
+		pa = 0;
+	check_pa_matches_va(va, pa);
+	return pa;
+}
+
+#if defined(CFG_TEE_CORE_DEBUG)
+static void check_va_matches_pa(paddr_t pa, void *va)
+{
+	if (va && virt_to_phys(va) != pa)
+		panic();
+}
+#else
+static void check_va_matches_pa(paddr_t pa __unused, void *va __unused)
+{
+}
+#endif
+
+static void *phys_to_virt_ta_vaspace(paddr_t pa)
+{
+	TEE_Result res;
+	void *va = NULL;
+
+	if (!core_mmu_user_mapping_is_active())
+		return NULL;
+
+	res = tee_mmu_user_pa2va_helper(to_user_ta_ctx(tee_mmu_get_ctx()),
+					pa, &va);
+	if (res != TEE_SUCCESS)
+		return NULL;
+	return va;
+}
+
+#ifdef CFG_WITH_PAGER
+static void *phys_to_virt_tee_ram(paddr_t pa)
+{
+	struct core_mmu_table_info *ti = &tee_pager_tbl_info;
+	unsigned idx;
+	unsigned end_idx;
+	uint32_t a;
+	paddr_t p;
+
+	if (pa >= CFG_TEE_LOAD_ADDR && pa < get_linear_map_end())
+		return (void *)(vaddr_t)pa;
+
+	end_idx = core_mmu_va2idx(ti, CFG_TEE_RAM_START +
+				      CFG_TEE_RAM_VA_SIZE);
+	/* Most addresses are mapped lineary, try that first if possible. */
+	idx = core_mmu_va2idx(ti, pa);
+	if (idx >= core_mmu_va2idx(ti, CFG_TEE_RAM_START) &&
+	    idx < end_idx) {
+		core_mmu_get_entry(ti, idx, &p, &a);
+		if ((a & TEE_MATTR_VALID_BLOCK) && p == pa)
+			return (void *)core_mmu_idx2va(ti, idx);
+	}
+
+	for (idx = core_mmu_va2idx(ti, CFG_TEE_RAM_START);
+	     idx < end_idx; idx++) {
+		core_mmu_get_entry(ti, idx, &p, &a);
+		if ((a & TEE_MATTR_VALID_BLOCK) && p == pa)
+			return (void *)core_mmu_idx2va(ti, idx);
+	}
+
+	return NULL;
+}
+#else
+static void *phys_to_virt_tee_ram(paddr_t pa)
+{
+	return map_pa2va(find_map_by_type_and_pa(MEM_AREA_TEE_RAM, pa), pa);
+}
+#endif
+
+void *phys_to_virt(paddr_t pa, enum teecore_memtypes m)
+{
+	void *va;
+
+	switch (m) {
+	case MEM_AREA_TA_VASPACE:
+		va = phys_to_virt_ta_vaspace(pa);
+		break;
+	case MEM_AREA_TEE_RAM:
+		va = phys_to_virt_tee_ram(pa);
+		break;
+	default:
+		va = map_pa2va(find_map_by_type_and_pa(m, pa), pa);
+	}
+	check_va_matches_pa(pa, va);
+	return va;
+}
+
+bool cpu_mmu_enabled(void)
+{
+	uint32_t sctlr;
+
+#ifdef ARM32
+	sctlr =  read_sctlr();
+#else
+	sctlr =  read_sctlr_el1();
+#endif
+
+	return sctlr & SCTLR_M ? true : false;
+}
diff --git a/core/arch/arm/mm/core_mmu_lpae.c b/core/arch/arm/mm/core_mmu_lpae.c
new file mode 100644
index 0000000..eb96c70
--- /dev/null
+++ b/core/arch/arm/mm/core_mmu_lpae.c
@@ -0,0 +1,890 @@
+/*
+ * Copyright (c) 2015-2016, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/*
+ * Copyright (c) 2014, ARM Limited and Contributors. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * 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.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE 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 <arm.h>
+#include <assert.h>
+#include <compiler.h>
+#include <inttypes.h>
+#include <kernel/thread.h>
+#include <kernel/panic.h>
+#include <kernel/misc.h>
+#include <mm/core_memprot.h>
+#include <mm/pgt_cache.h>
+#include <mm/core_memprot.h>
+#include <string.h>
+#include <trace.h>
+#include <types_ext.h>
+#include <util.h>
+
+#include "core_mmu_private.h"
+
+#ifndef DEBUG_XLAT_TABLE
+#define DEBUG_XLAT_TABLE 0
+#endif
+
+#if DEBUG_XLAT_TABLE
+#define debug_print(...) DMSG_RAW(__VA_ARGS__)
+#else
+#define debug_print(...) ((void)0)
+#endif
+
+
+/*
+ * Miscellaneous MMU related constants
+ */
+
+#define INVALID_DESC		0x0
+#define BLOCK_DESC		0x1
+#define L3_BLOCK_DESC		0x3
+#define TABLE_DESC		0x3
+#define DESC_ENTRY_TYPE_MASK	0x3
+
+#define HIDDEN_DESC		0x4
+#define HIDDEN_DIRTY_DESC	0x8
+
+#define XN			(1ull << 2)
+#define PXN			(1ull << 1)
+#define CONT_HINT		(1ull << 0)
+
+#define UPPER_ATTRS(x)		(((x) & 0x7) << 52)
+#define NON_GLOBAL		(1ull << 9)
+#define ACCESS_FLAG		(1ull << 8)
+#define NSH			(0x0 << 6)
+#define OSH			(0x2 << 6)
+#define ISH			(0x3 << 6)
+
+#define AP_RO			(0x1 << 5)
+#define AP_RW			(0x0 << 5)
+#define AP_UNPRIV		(0x1 << 4)
+
+#define NS				(0x1 << 3)
+#define LOWER_ATTRS_SHIFT		2
+#define LOWER_ATTRS(x)			(((x) & 0xfff) << LOWER_ATTRS_SHIFT)
+
+#define ATTR_DEVICE_INDEX		0x0
+#define ATTR_IWBWA_OWBWA_NTR_INDEX	0x1
+#define ATTR_INDEX_MASK			0x7
+
+#define ATTR_DEVICE			(0x4)
+#define ATTR_IWBWA_OWBWA_NTR		(0xff)
+
+#define MAIR_ATTR_SET(attr, index)	(((uint64_t)attr) << ((index) << 3))
+
+#define OUTPUT_ADDRESS_MASK	(0x0000FFFFFFFFF000ULL)
+
+/* (internal) physical address size bits in EL3/EL1 */
+#define TCR_PS_BITS_4GB		(0x0)
+#define TCR_PS_BITS_64GB	(0x1)
+#define TCR_PS_BITS_1TB		(0x2)
+#define TCR_PS_BITS_4TB		(0x3)
+#define TCR_PS_BITS_16TB	(0x4)
+#define TCR_PS_BITS_256TB	(0x5)
+
+#define ADDR_MASK_48_TO_63	0xFFFF000000000000ULL
+#define ADDR_MASK_44_TO_47	0x0000F00000000000ULL
+#define ADDR_MASK_42_TO_43	0x00000C0000000000ULL
+#define ADDR_MASK_40_TO_41	0x0000030000000000ULL
+#define ADDR_MASK_36_TO_39	0x000000F000000000ULL
+#define ADDR_MASK_32_TO_35	0x0000000F00000000ULL
+
+#define UNSET_DESC		((uint64_t)-1)
+
+#define FOUR_KB_SHIFT		12
+#define PAGE_SIZE_SHIFT		FOUR_KB_SHIFT
+#define PAGE_SIZE		(1 << PAGE_SIZE_SHIFT)
+#define PAGE_SIZE_MASK		(PAGE_SIZE - 1)
+#define IS_PAGE_ALIGNED(addr)	(((addr) & PAGE_SIZE_MASK) == 0)
+
+#define XLAT_ENTRY_SIZE_SHIFT	3 /* Each MMU table entry is 8 bytes (1 << 3) */
+#define XLAT_ENTRY_SIZE		(1 << XLAT_ENTRY_SIZE_SHIFT)
+
+#define XLAT_TABLE_SIZE_SHIFT	PAGE_SIZE_SHIFT
+#define XLAT_TABLE_SIZE		(1 << XLAT_TABLE_SIZE_SHIFT)
+
+/* Values for number of entries in each MMU translation table */
+#define XLAT_TABLE_ENTRIES_SHIFT (XLAT_TABLE_SIZE_SHIFT - XLAT_ENTRY_SIZE_SHIFT)
+#define XLAT_TABLE_ENTRIES	(1 << XLAT_TABLE_ENTRIES_SHIFT)
+#define XLAT_TABLE_ENTRIES_MASK	(XLAT_TABLE_ENTRIES - 1)
+
+/* Values to convert a memory address to an index into a translation table */
+#define L3_XLAT_ADDRESS_SHIFT	PAGE_SIZE_SHIFT
+#define L2_XLAT_ADDRESS_SHIFT	(L3_XLAT_ADDRESS_SHIFT + \
+				 XLAT_TABLE_ENTRIES_SHIFT)
+#define L1_XLAT_ADDRESS_SHIFT	(L2_XLAT_ADDRESS_SHIFT + \
+				 XLAT_TABLE_ENTRIES_SHIFT)
+
+#define MAX_MMAP_REGIONS	16
+#define NUM_L1_ENTRIES		\
+		(CFG_LPAE_ADDR_SPACE_SIZE >> L1_XLAT_ADDRESS_SHIFT)
+
+#ifndef MAX_XLAT_TABLES
+#define MAX_XLAT_TABLES		5
+#endif
+
+/* MMU L1 table, one for each core */
+static uint64_t l1_xlation_table[CFG_TEE_CORE_NB_CORE][NUM_L1_ENTRIES]
+	__aligned(NUM_L1_ENTRIES * XLAT_ENTRY_SIZE) __section(".nozi.mmu.l1");
+
+static uint64_t xlat_tables[MAX_XLAT_TABLES][XLAT_TABLE_ENTRIES]
+	__aligned(XLAT_TABLE_SIZE) __section(".nozi.mmu.l2");
+
+/* MMU L2 table for TAs, one for each thread */
+static uint64_t xlat_tables_ul1[CFG_NUM_THREADS][XLAT_TABLE_ENTRIES]
+	__aligned(XLAT_TABLE_SIZE) __section(".nozi.mmu.l2");
+
+
+static unsigned next_xlat __early_bss;
+static uint64_t tcr_ps_bits __early_bss;
+static int user_va_idx = -1;
+
+static uint32_t desc_to_mattr(unsigned level, uint64_t desc)
+{
+	uint32_t a;
+
+	if (!(desc & 1)) {
+		if (desc & HIDDEN_DESC)
+			return TEE_MATTR_HIDDEN_BLOCK;
+		if (desc & HIDDEN_DIRTY_DESC)
+			return TEE_MATTR_HIDDEN_DIRTY_BLOCK;
+		return 0;
+	}
+
+	if (level == 3) {
+		if ((desc & DESC_ENTRY_TYPE_MASK) != L3_BLOCK_DESC)
+			return 0;
+	} else {
+		if ((desc & DESC_ENTRY_TYPE_MASK) == TABLE_DESC)
+			return TEE_MATTR_TABLE;
+	}
+
+	a = TEE_MATTR_VALID_BLOCK;
+
+	if (desc & LOWER_ATTRS(ACCESS_FLAG))
+		a |= TEE_MATTR_PRX | TEE_MATTR_URX;
+
+	if (!(desc & LOWER_ATTRS(AP_RO)))
+		a |= TEE_MATTR_PW | TEE_MATTR_UW;
+
+	if (!(desc & LOWER_ATTRS(AP_UNPRIV)))
+		a &= ~TEE_MATTR_URWX;
+
+	if (desc & UPPER_ATTRS(XN))
+		a &= ~(TEE_MATTR_PX | TEE_MATTR_UX);
+
+	if (desc & UPPER_ATTRS(PXN))
+		a &= ~TEE_MATTR_PX;
+
+	COMPILE_TIME_ASSERT(ATTR_DEVICE_INDEX == TEE_MATTR_CACHE_NONCACHE);
+	COMPILE_TIME_ASSERT(ATTR_IWBWA_OWBWA_NTR_INDEX ==
+			    TEE_MATTR_CACHE_CACHED);
+
+	a |= ((desc & LOWER_ATTRS(ATTR_INDEX_MASK)) >> LOWER_ATTRS_SHIFT) <<
+	     TEE_MATTR_CACHE_SHIFT;
+
+	if (!(desc & LOWER_ATTRS(NON_GLOBAL)))
+		a |= TEE_MATTR_GLOBAL;
+
+	if (!(desc & LOWER_ATTRS(NS)))
+		a |= TEE_MATTR_SECURE;
+
+	return a;
+}
+
+static uint64_t mattr_to_desc(unsigned level, uint32_t attr)
+{
+	uint64_t desc;
+	uint32_t a = attr;
+
+	if (a & TEE_MATTR_HIDDEN_BLOCK)
+		return INVALID_DESC | HIDDEN_DESC;
+
+	if (a & TEE_MATTR_HIDDEN_DIRTY_BLOCK)
+		return INVALID_DESC | HIDDEN_DIRTY_DESC;
+
+	if (a & TEE_MATTR_TABLE)
+		return TABLE_DESC;
+
+	if (!(a & TEE_MATTR_VALID_BLOCK))
+		return 0;
+
+	if (a & (TEE_MATTR_PX | TEE_MATTR_PW))
+		a |= TEE_MATTR_PR;
+	if (a & (TEE_MATTR_UX | TEE_MATTR_UW))
+		a |= TEE_MATTR_UR;
+	if (a & TEE_MATTR_UR)
+		a |= TEE_MATTR_PR;
+	if (a & TEE_MATTR_UW)
+		a |= TEE_MATTR_PW;
+
+	if (level == 3)
+		desc = L3_BLOCK_DESC;
+	else
+		desc = BLOCK_DESC;
+
+	if (!(a & (TEE_MATTR_PX | TEE_MATTR_UX)))
+		desc |= UPPER_ATTRS(XN);
+	if (!(a & TEE_MATTR_PX))
+		desc |= UPPER_ATTRS(PXN);
+
+	if (a & TEE_MATTR_UR)
+		desc |= LOWER_ATTRS(AP_UNPRIV);
+
+	if (!(a & TEE_MATTR_PW))
+		desc |= LOWER_ATTRS(AP_RO);
+
+	/* Keep in sync with core_mmu.c:core_mmu_mattr_is_ok */
+	switch ((a >> TEE_MATTR_CACHE_SHIFT) & TEE_MATTR_CACHE_MASK) {
+	case TEE_MATTR_CACHE_NONCACHE:
+		desc |= LOWER_ATTRS(ATTR_DEVICE_INDEX | OSH);
+		break;
+	case TEE_MATTR_CACHE_CACHED:
+		desc |= LOWER_ATTRS(ATTR_IWBWA_OWBWA_NTR_INDEX | ISH);
+		break;
+	default:
+		/*
+		 * "Can't happen" the attribute is supposed to be checked
+		 * with core_mmu_mattr_is_ok() before.
+		 */
+		panic();
+	}
+
+	if (a & (TEE_MATTR_UR | TEE_MATTR_PR))
+		desc |= LOWER_ATTRS(ACCESS_FLAG);
+
+	if (!(a & TEE_MATTR_GLOBAL))
+		desc |= LOWER_ATTRS(NON_GLOBAL);
+
+	desc |= a & TEE_MATTR_SECURE ? 0 : LOWER_ATTRS(NS);
+
+	return desc;
+}
+
+static uint64_t mmap_desc(uint32_t attr, uint64_t addr_pa,
+					unsigned level)
+{
+	return mattr_to_desc(level, attr) | addr_pa;
+}
+
+static int mmap_region_attr(struct tee_mmap_region *mm, uint64_t base_va,
+					uint64_t size)
+{
+	uint32_t attr = mm->attr;
+
+	for (;;) {
+		mm++;
+
+		if (!mm->size)
+			return attr; /* Reached end of list */
+
+		if (mm->va >= base_va + size)
+			return attr; /* Next region is after area so end */
+
+		if (mm->va + mm->size <= base_va)
+			continue; /* Next region has already been overtaken */
+
+		if (mm->attr == attr)
+			continue; /* Region doesn't override attribs so skip */
+
+		if (mm->va > base_va ||
+			mm->va + mm->size < base_va + size)
+			return -1; /* Region doesn't fully cover our area */
+	}
+}
+
+static struct tee_mmap_region *init_xlation_table(struct tee_mmap_region *mm,
+			uint64_t base_va, uint64_t *table, unsigned level)
+{
+	unsigned int level_size_shift = L1_XLAT_ADDRESS_SHIFT - (level - 1) *
+						XLAT_TABLE_ENTRIES_SHIFT;
+	unsigned int level_size = BIT32(level_size_shift);
+	uint64_t level_index_mask = SHIFT_U64(XLAT_TABLE_ENTRIES_MASK,
+					      level_size_shift);
+
+	assert(level <= 3);
+
+	debug_print("New xlat table (level %u):", level);
+
+	do  {
+		uint64_t desc = UNSET_DESC;
+
+		if (mm->va + mm->size <= base_va) {
+			/* Area now after the region so skip it */
+			mm++;
+			continue;
+		}
+
+
+		if (mm->va >= base_va + level_size) {
+			/* Next region is after area so nothing to map yet */
+			desc = INVALID_DESC;
+			debug_print("%*s%010" PRIx64 " %8x",
+					level * 2, "", base_va, level_size);
+		} else if (mm->va <= base_va &&
+			   mm->va + mm->size >= base_va + level_size &&
+			   !(mm->pa & (level_size - 1))) {
+			/* Next region covers all of area */
+			int attr = mmap_region_attr(mm, base_va, level_size);
+
+			if (attr >= 0) {
+				desc = mmap_desc(attr,
+						 base_va - mm->va + mm->pa,
+						 level);
+				debug_print("%*s%010" PRIx64 " %8x %s-%s-%s-%s",
+					level * 2, "", base_va, level_size,
+					attr & (TEE_MATTR_CACHE_CACHED <<
+						TEE_MATTR_CACHE_SHIFT) ?
+						"MEM" : "DEV",
+					attr & TEE_MATTR_PW ? "RW" : "RO",
+					attr & TEE_MATTR_PX ? "X" : "XN",
+					attr & TEE_MATTR_SECURE ? "S" : "NS");
+			} else {
+				debug_print("%*s%010" PRIx64 " %8x",
+					level * 2, "", base_va, level_size);
+			}
+		}
+		/* else Next region only partially covers area, so need */
+
+		if (desc == UNSET_DESC) {
+			/* Area not covered by a region so need finer table */
+			uint64_t *new_table = xlat_tables[next_xlat++];
+			/* Clear table before use */
+			if (next_xlat > MAX_XLAT_TABLES)
+				panic("running out of xlat tables");
+			memset(new_table, 0, XLAT_TABLE_SIZE);
+
+			desc = TABLE_DESC | virt_to_phys(new_table);
+
+			/* Recurse to fill in new table */
+			mm = init_xlation_table(mm, base_va, new_table,
+					   level + 1);
+		}
+
+		*table++ = desc;
+		base_va += level_size;
+	} while (mm->size && (base_va & level_index_mask));
+
+	return mm;
+}
+
+static unsigned int calc_physical_addr_size_bits(uint64_t max_addr)
+{
+	/* Physical address can't exceed 48 bits */
+	assert(!(max_addr & ADDR_MASK_48_TO_63));
+
+	/* 48 bits address */
+	if (max_addr & ADDR_MASK_44_TO_47)
+		return TCR_PS_BITS_256TB;
+
+	/* 44 bits address */
+	if (max_addr & ADDR_MASK_42_TO_43)
+		return TCR_PS_BITS_16TB;
+
+	/* 42 bits address */
+	if (max_addr & ADDR_MASK_40_TO_41)
+		return TCR_PS_BITS_4TB;
+
+	/* 40 bits address */
+	if (max_addr & ADDR_MASK_36_TO_39)
+		return TCR_PS_BITS_1TB;
+
+	/* 36 bits address */
+	if (max_addr & ADDR_MASK_32_TO_35)
+		return TCR_PS_BITS_64GB;
+
+	return TCR_PS_BITS_4GB;
+}
+
+void core_init_mmu_tables(struct tee_mmap_region *mm)
+{
+	paddr_t max_pa = 0;
+	uint64_t max_va = 0;
+	size_t n;
+
+	for (n = 0; mm[n].size; n++) {
+		paddr_t pa_end;
+		vaddr_t va_end;
+
+		debug_print(" %010" PRIxVA " %010" PRIxPA " %10zx %x",
+			    mm[n].va, mm[n].pa, mm[n].size, mm[n].attr);
+
+		if (!IS_PAGE_ALIGNED(mm[n].pa) || !IS_PAGE_ALIGNED(mm[n].size))
+			panic("unaligned region");
+
+		pa_end = mm[n].pa + mm[n].size - 1;
+		va_end = mm[n].va + mm[n].size - 1;
+		if (pa_end > max_pa)
+			max_pa = pa_end;
+		if (va_end > max_va)
+			max_va = va_end;
+	}
+
+	/* Clear table before use */
+	memset(l1_xlation_table[0], 0, NUM_L1_ENTRIES * XLAT_ENTRY_SIZE);
+	init_xlation_table(mm, 0, l1_xlation_table[0], 1);
+	for (n = 1; n < CFG_TEE_CORE_NB_CORE; n++)
+		memcpy(l1_xlation_table[n], l1_xlation_table[0],
+			XLAT_ENTRY_SIZE * NUM_L1_ENTRIES);
+
+	for (n = 1; n < NUM_L1_ENTRIES; n++) {
+		if (!l1_xlation_table[0][n]) {
+			user_va_idx = n;
+			break;
+		}
+	}
+	assert(user_va_idx != -1);
+
+	tcr_ps_bits = calc_physical_addr_size_bits(max_pa);
+	COMPILE_TIME_ASSERT(CFG_LPAE_ADDR_SPACE_SIZE > 0);
+	assert(max_va < CFG_LPAE_ADDR_SPACE_SIZE);
+}
+
+bool core_mmu_place_tee_ram_at_top(paddr_t paddr)
+{
+	size_t l1size = (1 << L1_XLAT_ADDRESS_SHIFT);
+	paddr_t l1mask = l1size - 1;
+
+	return (paddr & l1mask) > (l1size / 2);
+}
+
+#ifdef ARM32
+void core_init_mmu_regs(void)
+{
+	uint32_t ttbcr = TTBCR_EAE;
+	uint32_t mair;
+	paddr_t ttbr0;
+
+	ttbr0 = virt_to_phys(l1_xlation_table[get_core_pos()]);
+
+	mair  = MAIR_ATTR_SET(ATTR_DEVICE, ATTR_DEVICE_INDEX);
+	mair |= MAIR_ATTR_SET(ATTR_IWBWA_OWBWA_NTR, ATTR_IWBWA_OWBWA_NTR_INDEX);
+	write_mair0(mair);
+
+	ttbcr |= TTBCR_XRGNX_WBWA << TTBCR_IRGN0_SHIFT;
+	ttbcr |= TTBCR_XRGNX_WBWA << TTBCR_ORGN0_SHIFT;
+	ttbcr |= TTBCR_SHX_ISH << TTBCR_SH0_SHIFT;
+
+	/* Disable the use of TTBR1 */
+	ttbcr |= TTBCR_EPD1;
+
+	/* TTBCR.A1 = 0 => ASID is stored in TTBR0 */
+
+	write_ttbcr(ttbcr);
+	write_ttbr0_64bit(ttbr0);
+	write_ttbr1_64bit(0);
+}
+#endif /*ARM32*/
+
+#ifdef ARM64
+void core_init_mmu_regs(void)
+{
+	uint64_t mair;
+	uint64_t tcr;
+	paddr_t ttbr0;
+
+	ttbr0 = virt_to_phys(l1_xlation_table[get_core_pos()]);
+
+	mair  = MAIR_ATTR_SET(ATTR_DEVICE, ATTR_DEVICE_INDEX);
+	mair |= MAIR_ATTR_SET(ATTR_IWBWA_OWBWA_NTR, ATTR_IWBWA_OWBWA_NTR_INDEX);
+	write_mair_el1(mair);
+
+	tcr = TCR_RES1;
+	tcr |= TCR_XRGNX_WBWA << TCR_IRGN0_SHIFT;
+	tcr |= TCR_XRGNX_WBWA << TCR_ORGN0_SHIFT;
+	tcr |= TCR_SHX_ISH << TCR_SH0_SHIFT;
+	tcr |= tcr_ps_bits << TCR_EL1_IPS_SHIFT;
+	tcr |= 64 - __builtin_ctzl(CFG_LPAE_ADDR_SPACE_SIZE);
+
+	/* Disable the use of TTBR1 */
+	tcr |= TCR_EPD1;
+
+	/*
+	 * TCR.A1 = 0 => ASID is stored in TTBR0
+	 * TCR.AS = 0 => Same ASID size as in Aarch32/ARMv7
+	 */
+
+	write_tcr_el1(tcr);
+	write_ttbr0_el1(ttbr0);
+	write_ttbr1_el1(0);
+}
+#endif /*ARM64*/
+
+void core_mmu_set_info_table(struct core_mmu_table_info *tbl_info,
+		unsigned level, vaddr_t va_base, void *table)
+{
+	tbl_info->level = level;
+	tbl_info->table = table;
+	tbl_info->va_base = va_base;
+	tbl_info->shift = L1_XLAT_ADDRESS_SHIFT -
+			  (level - 1) * XLAT_TABLE_ENTRIES_SHIFT;
+	assert(level <= 3);
+	if (level == 1)
+		tbl_info->num_entries = NUM_L1_ENTRIES;
+	else
+		tbl_info->num_entries = XLAT_TABLE_ENTRIES;
+}
+
+void core_mmu_get_user_pgdir(struct core_mmu_table_info *pgd_info)
+{
+	vaddr_t va_range_base;
+	void *tbl = xlat_tables_ul1[thread_get_id()];
+
+	core_mmu_get_user_va_range(&va_range_base, NULL);
+	core_mmu_set_info_table(pgd_info, 2, va_range_base, tbl);
+}
+
+void core_mmu_create_user_map(struct user_ta_ctx *utc,
+			      struct core_mmu_user_map *map)
+{
+	struct core_mmu_table_info dir_info;
+
+	COMPILE_TIME_ASSERT(sizeof(uint64_t) * XLAT_TABLE_ENTRIES == PGT_SIZE);
+
+	core_mmu_get_user_pgdir(&dir_info);
+	memset(dir_info.table, 0, PGT_SIZE);
+	core_mmu_populate_user_map(&dir_info, utc);
+	map->user_map = virt_to_phys(dir_info.table) | TABLE_DESC;
+	map->asid = utc->context & TTBR_ASID_MASK;
+}
+
+bool core_mmu_find_table(vaddr_t va, unsigned max_level,
+		struct core_mmu_table_info *tbl_info)
+{
+	uint64_t *tbl = l1_xlation_table[get_core_pos()];
+	uintptr_t ntbl;
+	unsigned level = 1;
+	vaddr_t va_base = 0;
+	unsigned num_entries = NUM_L1_ENTRIES;
+
+	while (true) {
+		unsigned level_size_shift =
+			L1_XLAT_ADDRESS_SHIFT - (level - 1) *
+						XLAT_TABLE_ENTRIES_SHIFT;
+		unsigned n = (va - va_base) >> level_size_shift;
+
+		if (n >= num_entries)
+			return false;
+
+		if (level == max_level || level == 3 ||
+			(tbl[n] & TABLE_DESC) != TABLE_DESC) {
+			/*
+			 * We've either reached max_level, level 3, a block
+			 * mapping entry or an "invalid" mapping entry.
+			 */
+			tbl_info->table = tbl;
+			tbl_info->va_base = va_base;
+			tbl_info->level = level;
+			tbl_info->shift = level_size_shift;
+			tbl_info->num_entries = num_entries;
+			return true;
+		}
+
+		/* Copy bits 39:12 from tbl[n] to ntbl */
+		ntbl = (tbl[n] & ((1ULL << 40) - 1)) & ~((1 << 12) - 1);
+
+		tbl = phys_to_virt(ntbl, MEM_AREA_TEE_RAM);
+		if (!tbl)
+			return false;
+
+		va_base += n << level_size_shift;
+		level++;
+		num_entries = XLAT_TABLE_ENTRIES;
+	}
+}
+
+bool core_mmu_divide_block(struct core_mmu_table_info *tbl_info,
+			   unsigned int idx)
+{
+	uint64_t *new_table;
+	uint64_t *entry;
+	uint64_t new_table_desc;
+	size_t new_entry_size;
+	paddr_t paddr;
+	uint32_t attr;
+	int i;
+
+	if (tbl_info->level >= 3)
+		return false;
+
+	if (next_xlat >= MAX_XLAT_TABLES)
+		return false;
+
+	if (tbl_info->level == 1 && idx >= NUM_L1_ENTRIES)
+		return false;
+
+	if (tbl_info->level > 1 && idx >= XLAT_TABLE_ENTRIES)
+		return false;
+
+	entry = (uint64_t *)tbl_info->table + idx;
+	assert((*entry & DESC_ENTRY_TYPE_MASK) == BLOCK_DESC);
+
+	new_table = xlat_tables[next_xlat++];
+	new_table_desc = TABLE_DESC | (uint64_t)(uintptr_t)new_table;
+
+	/* store attributes of original block */
+	attr = desc_to_mattr(tbl_info->level, *entry);
+	paddr = *entry & OUTPUT_ADDRESS_MASK;
+	new_entry_size = 1 << (tbl_info->shift - XLAT_TABLE_ENTRIES_SHIFT);
+
+	/* Fill new xlat table with entries pointing to the same memory */
+	for (i = 0; i < XLAT_TABLE_ENTRIES; i++) {
+		*new_table = paddr | mattr_to_desc(tbl_info->level + 1, attr);
+		paddr += new_entry_size;
+		new_table++;
+	}
+
+	/* Update descriptor at current level */
+	*entry = new_table_desc;
+	return true;
+}
+
+void core_mmu_set_entry_primitive(void *table, size_t level, size_t idx,
+				  paddr_t pa, uint32_t attr)
+{
+	uint64_t *tbl = table;
+	uint64_t desc = mattr_to_desc(level, attr);
+
+	tbl[idx] = desc | pa;
+}
+
+void core_mmu_get_entry_primitive(const void *table, size_t level,
+				  size_t idx, paddr_t *pa, uint32_t *attr)
+{
+	const uint64_t *tbl = table;
+
+	if (pa)
+		*pa = (tbl[idx] & ((1ull << 40) - 1)) & ~((1 << 12) - 1);
+
+	if (attr)
+		*attr = desc_to_mattr(level, tbl[idx]);
+}
+
+bool core_mmu_user_va_range_is_defined(void)
+{
+	return user_va_idx != -1;
+}
+
+void core_mmu_get_user_va_range(vaddr_t *base, size_t *size)
+{
+	assert(user_va_idx != -1);
+
+	if (base)
+		*base = (vaddr_t)user_va_idx << L1_XLAT_ADDRESS_SHIFT;
+	if (size)
+		*size = 1 << L1_XLAT_ADDRESS_SHIFT;
+}
+
+bool core_mmu_user_mapping_is_active(void)
+{
+	assert(user_va_idx != -1);
+	return !!l1_xlation_table[get_core_pos()][user_va_idx];
+}
+
+#ifdef ARM32
+void core_mmu_get_user_map(struct core_mmu_user_map *map)
+{
+	assert(user_va_idx != -1);
+
+	map->user_map = l1_xlation_table[get_core_pos()][user_va_idx];
+	if (map->user_map) {
+		map->asid = (read_ttbr0_64bit() >> TTBR_ASID_SHIFT) &
+			    TTBR_ASID_MASK;
+	} else {
+		map->asid = 0;
+	}
+}
+
+void core_mmu_set_user_map(struct core_mmu_user_map *map)
+{
+	uint64_t ttbr;
+	uint32_t exceptions = thread_mask_exceptions(THREAD_EXCP_ALL);
+
+	assert(user_va_idx != -1);
+
+	ttbr = read_ttbr0_64bit();
+	/* Clear ASID */
+	ttbr &= ~((uint64_t)TTBR_ASID_MASK << TTBR_ASID_SHIFT);
+	write_ttbr0_64bit(ttbr);
+	isb();
+
+	/* Set the new map */
+	if (map && map->user_map) {
+		l1_xlation_table[get_core_pos()][user_va_idx] = map->user_map;
+		dsb();	/* Make sure the write above is visible */
+		ttbr |= ((uint64_t)map->asid << TTBR_ASID_SHIFT);
+		write_ttbr0_64bit(ttbr);
+		isb();
+	} else {
+		l1_xlation_table[get_core_pos()][user_va_idx] = 0;
+		dsb();	/* Make sure the write above is visible */
+	}
+
+	core_tlb_maintenance(TLBINV_UNIFIEDTLB, 0);
+
+	thread_unmask_exceptions(exceptions);
+}
+
+enum core_mmu_fault core_mmu_get_fault_type(uint32_t fault_descr)
+{
+	assert(fault_descr & FSR_LPAE);
+
+	switch (fault_descr & FSR_STATUS_MASK) {
+	case 0x21: /* b100001 Alignment fault */
+		return CORE_MMU_FAULT_ALIGNMENT;
+	case 0x11: /* b010001 Asynchronous extern abort (DFSR only) */
+		return CORE_MMU_FAULT_ASYNC_EXTERNAL;
+	case 0x12: /* b100010 Debug event */
+		return CORE_MMU_FAULT_DEBUG_EVENT;
+	default:
+		break;
+	}
+
+	switch ((fault_descr & FSR_STATUS_MASK) >> 2) {
+	case 0x1: /* b0001LL Translation fault */
+		return CORE_MMU_FAULT_TRANSLATION;
+	case 0x2: /* b0010LL Access flag fault */
+	case 0x3: /* b0011LL Permission fault */
+		if (fault_descr & FSR_WNR)
+			return CORE_MMU_FAULT_WRITE_PERMISSION;
+		else
+			return CORE_MMU_FAULT_READ_PERMISSION;
+	default:
+		return CORE_MMU_FAULT_OTHER;
+	}
+}
+#endif /*ARM32*/
+
+#ifdef ARM64
+void core_mmu_get_user_map(struct core_mmu_user_map *map)
+{
+	assert(user_va_idx != -1);
+
+	map->user_map = l1_xlation_table[get_core_pos()][user_va_idx];
+	if (map->user_map) {
+		map->asid = (read_ttbr0_el1() >> TTBR_ASID_SHIFT) &
+			    TTBR_ASID_MASK;
+	} else {
+		map->asid = 0;
+	}
+}
+
+void core_mmu_set_user_map(struct core_mmu_user_map *map)
+{
+	uint64_t ttbr;
+	uint32_t daif = read_daif();
+
+	write_daif(daif | DAIF_AIF);
+
+	ttbr = read_ttbr0_el1();
+	/* Clear ASID */
+	ttbr &= ~((uint64_t)TTBR_ASID_MASK << TTBR_ASID_SHIFT);
+	write_ttbr0_el1(ttbr);
+	isb();
+
+	/* Set the new map */
+	if (map && map->user_map) {
+		l1_xlation_table[get_core_pos()][user_va_idx] = map->user_map;
+		dsb();	/* Make sure the write above is visible */
+		ttbr |= ((uint64_t)map->asid << TTBR_ASID_SHIFT);
+		write_ttbr0_el1(ttbr);
+		isb();
+	} else {
+		l1_xlation_table[get_core_pos()][user_va_idx] = 0;
+		dsb();	/* Make sure the write above is visible */
+	}
+
+	core_tlb_maintenance(TLBINV_UNIFIEDTLB, 0);
+
+	write_daif(daif);
+}
+
+enum core_mmu_fault core_mmu_get_fault_type(uint32_t fault_descr)
+{
+	switch ((fault_descr >> ESR_EC_SHIFT) & ESR_EC_MASK) {
+	case ESR_EC_SP_ALIGN:
+	case ESR_EC_PC_ALIGN:
+		return CORE_MMU_FAULT_ALIGNMENT;
+	case ESR_EC_IABT_EL0:
+	case ESR_EC_DABT_EL0:
+	case ESR_EC_IABT_EL1:
+	case ESR_EC_DABT_EL1:
+		switch (fault_descr & ESR_FSC_MASK) {
+		case ESR_FSC_TRANS_L0:
+		case ESR_FSC_TRANS_L1:
+		case ESR_FSC_TRANS_L2:
+		case ESR_FSC_TRANS_L3:
+			return CORE_MMU_FAULT_TRANSLATION;
+		case ESR_FSC_ACCF_L1:
+		case ESR_FSC_ACCF_L2:
+		case ESR_FSC_ACCF_L3:
+		case ESR_FSC_PERMF_L1:
+		case ESR_FSC_PERMF_L2:
+		case ESR_FSC_PERMF_L3:
+			if (fault_descr & ESR_ABT_WNR)
+				return CORE_MMU_FAULT_WRITE_PERMISSION;
+			else
+				return CORE_MMU_FAULT_READ_PERMISSION;
+		case ESR_FSC_ALIGN:
+			return CORE_MMU_FAULT_ALIGNMENT;
+		default:
+			return CORE_MMU_FAULT_OTHER;
+		}
+	default:
+		return CORE_MMU_FAULT_OTHER;
+	}
+}
+#endif /*ARM64*/
diff --git a/core/arch/arm/mm/core_mmu_private.h b/core/arch/arm/mm/core_mmu_private.h
new file mode 100644
index 0000000..5bcb9ea
--- /dev/null
+++ b/core/arch/arm/mm/core_mmu_private.h
@@ -0,0 +1,43 @@
+/*
+ * Copyright (c) 2015-2016, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+#ifndef CORE_MMU_PRIVATE_H
+#define CORE_MMU_PRIVATE_H
+
+#include <mm/core_mmu.h>
+#include <mm/tee_mmu_types.h>
+
+
+void core_init_mmu_tables(struct tee_mmap_region *mm);
+
+void core_mmu_set_info_table(struct core_mmu_table_info *tbl_info,
+			     unsigned level, vaddr_t va_base, void *table);
+void core_mmu_populate_user_map(struct core_mmu_table_info *dir_info,
+				struct user_ta_ctx *utc);
+
+
+#endif /*CORE_MMU_PRIVATE_H*/
+
diff --git a/core/arch/arm/mm/core_mmu_v7.c b/core/arch/arm/mm/core_mmu_v7.c
new file mode 100644
index 0000000..54f6caa
--- /dev/null
+++ b/core/arch/arm/mm/core_mmu_v7.c
@@ -0,0 +1,790 @@
+/*
+ * Copyright (c) 2016, Linaro Limited
+ * 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 <arm.h>
+#include <assert.h>
+#include <kernel/panic.h>
+#include <kernel/thread.h>
+#include <mm/core_memprot.h>
+#include <mm/core_mmu.h>
+#include <mm/pgt_cache.h>
+#include <platform_config.h>
+#include <stdlib.h>
+#include <string.h>
+#include <trace.h>
+#include <util.h>
+#include "core_mmu_private.h"
+
+#ifdef CFG_WITH_LPAE
+#error This file is not to be used with LPAE
+#endif
+
+/*
+ * MMU related values
+ */
+
+/* Sharable */
+#define TEE_MMU_TTB_S           (1 << 1)
+
+/* Not Outer Sharable */
+#define TEE_MMU_TTB_NOS         (1 << 5)
+
+/* Normal memory, Inner Non-cacheable */
+#define TEE_MMU_TTB_IRGN_NC     0
+
+/* Normal memory, Inner Write-Back Write-Allocate Cacheable */
+#define TEE_MMU_TTB_IRGN_WBWA   (1 << 6)
+
+/* Normal memory, Inner Write-Through Cacheable */
+#define TEE_MMU_TTB_IRGN_WT     1
+
+/* Normal memory, Inner Write-Back no Write-Allocate Cacheable */
+#define TEE_MMU_TTB_IRGN_WB     (1 | (1 << 6))
+
+/* Normal memory, Outer Write-Back Write-Allocate Cacheable */
+#define TEE_MMU_TTB_RNG_WBWA    (1 << 3)
+
+#define TEE_MMU_DEFAULT_ATTRS \
+		(TEE_MMU_TTB_S | TEE_MMU_TTB_NOS | \
+		 TEE_MMU_TTB_IRGN_WBWA | TEE_MMU_TTB_RNG_WBWA)
+
+
+#define INVALID_DESC		0x0
+#define HIDDEN_DESC		0x4
+#define HIDDEN_DIRTY_DESC	0x8
+
+#define SECTION_SHIFT		20
+#define SECTION_MASK		0x000fffff
+#define SECTION_SIZE		0x00100000
+
+/* armv7 memory mapping attributes: section mapping */
+#define SECTION_SECURE			(0 << 19)
+#define SECTION_NOTSECURE		(1 << 19)
+#define SECTION_SHARED			(1 << 16)
+#define SECTION_NOTGLOBAL		(1 << 17)
+#define SECTION_ACCESS_FLAG		(1 << 10)
+#define SECTION_UNPRIV			(1 << 11)
+#define SECTION_RO			(1 << 15)
+#define SECTION_TEXCB(texcb)		((((texcb) >> 2) << 12) | \
+					 ((((texcb) >> 1) & 0x1) << 3) | \
+					 (((texcb) & 0x1) << 2))
+#define SECTION_DEVICE			SECTION_TEXCB(ATTR_DEVICE_INDEX)
+#define SECTION_NORMAL			SECTION_TEXCB(ATTR_DEVICE_INDEX)
+#define SECTION_NORMAL_CACHED		SECTION_TEXCB(ATTR_IWBWA_OWBWA_INDEX)
+
+#define SECTION_XN			(1 << 4)
+#define SECTION_PXN			(1 << 0)
+#define SECTION_SECTION			(2 << 0)
+
+#define SECTION_PT_NOTSECURE		(1 << 3)
+#define SECTION_PT_PT			(1 << 0)
+
+#define SMALL_PAGE_SMALL_PAGE		(1 << 1)
+#define SMALL_PAGE_SHARED		(1 << 10)
+#define SMALL_PAGE_NOTGLOBAL		(1 << 11)
+#define SMALL_PAGE_TEXCB(texcb)		((((texcb) >> 2) << 6) | \
+					 ((((texcb) >> 1) & 0x1) << 3) | \
+					 (((texcb) & 0x1) << 2))
+#define SMALL_PAGE_DEVICE		SMALL_PAGE_TEXCB(ATTR_DEVICE_INDEX)
+#define SMALL_PAGE_NORMAL		SMALL_PAGE_TEXCB(ATTR_DEVICE_INDEX)
+#define SMALL_PAGE_NORMAL_CACHED	SMALL_PAGE_TEXCB(ATTR_IWBWA_OWBWA_INDEX)
+#define SMALL_PAGE_ACCESS_FLAG		(1 << 4)
+#define SMALL_PAGE_UNPRIV		(1 << 5)
+#define SMALL_PAGE_RO			(1 << 9)
+#define SMALL_PAGE_XN			(1 << 0)
+
+
+/* The TEX, C and B bits concatenated */
+#define ATTR_DEVICE_INDEX		0x0
+#define ATTR_IWBWA_OWBWA_INDEX		0x1
+
+#define PRRR_IDX(idx, tr, nos)		(((tr) << (2 * (idx))) | \
+					 ((uint32_t)(nos) << ((idx) + 24)))
+#define NMRR_IDX(idx, ir, or)		(((ir) << (2 * (idx))) | \
+					 ((uint32_t)(or) << (2 * (idx) + 16)))
+#define PRRR_DS0			(1 << 16)
+#define PRRR_DS1			(1 << 17)
+#define PRRR_NS0			(1 << 18)
+#define PRRR_NS1			(1 << 19)
+
+#define ATTR_DEVICE_PRRR		PRRR_IDX(ATTR_DEVICE_INDEX, 1, 0)
+#define ATTR_DEVICE_NMRR		NMRR_IDX(ATTR_DEVICE_INDEX, 0, 0)
+
+#define ATTR_IWBWA_OWBWA_PRRR		PRRR_IDX(ATTR_IWBWA_OWBWA_INDEX, 2, 1)
+#define ATTR_IWBWA_OWBWA_NMRR		NMRR_IDX(ATTR_IWBWA_OWBWA_INDEX, 1, 1)
+
+#define NUM_L1_ENTRIES		4096
+#define NUM_L2_ENTRIES		256
+
+#define L1_TBL_SIZE		(NUM_L1_ENTRIES * 4)
+#define L2_TBL_SIZE		(NUM_L2_ENTRIES * 4)
+#define L1_ALIGNMENT		L1_TBL_SIZE
+#define L2_ALIGNMENT		L2_TBL_SIZE
+
+/* Defined to the smallest possible secondary L1 MMU table */
+#define TTBCR_N_VALUE		7
+
+/* Number of sections in ttbr0 when user mapping activated */
+#define NUM_UL1_ENTRIES         (1 << (12 - TTBCR_N_VALUE))
+#define UL1_ALIGNMENT		(NUM_UL1_ENTRIES * 4)
+/* TTB attributes */
+
+/* TTB0 of TTBR0 (depends on TTBCR_N_VALUE) */
+#define TTB_UL1_MASK		(~(UL1_ALIGNMENT - 1))
+/* TTB1 of TTBR1 */
+#define TTB_L1_MASK		(~(L1_ALIGNMENT - 1))
+
+#ifndef MAX_XLAT_TABLES
+#define MAX_XLAT_TABLES		4
+#endif
+
+enum desc_type {
+	DESC_TYPE_PAGE_TABLE,
+	DESC_TYPE_SECTION,
+	DESC_TYPE_SUPER_SECTION,
+	DESC_TYPE_LARGE_PAGE,
+	DESC_TYPE_SMALL_PAGE,
+	DESC_TYPE_INVALID,
+};
+
+/* Main MMU L1 table for teecore */
+static uint32_t main_mmu_l1_ttb[NUM_L1_ENTRIES]
+		__aligned(L1_ALIGNMENT) __section(".nozi.mmu.l1");
+
+/* L2 MMU tables */
+static uint32_t main_mmu_l2_ttb[MAX_XLAT_TABLES][NUM_L2_ENTRIES]
+		__aligned(L2_ALIGNMENT) __section(".nozi.mmu.l2");
+
+/* MMU L1 table for TAs, one for each thread */
+static uint32_t main_mmu_ul1_ttb[CFG_NUM_THREADS][NUM_UL1_ENTRIES]
+		__aligned(UL1_ALIGNMENT) __section(".nozi.mmu.ul1");
+
+static vaddr_t core_mmu_get_main_ttb_va(void)
+{
+	return (vaddr_t)main_mmu_l1_ttb;
+}
+
+static paddr_t core_mmu_get_main_ttb_pa(void)
+{
+	paddr_t pa = virt_to_phys((void *)core_mmu_get_main_ttb_va());
+
+	if (pa & ~TTB_L1_MASK)
+		panic("invalid core l1 table");
+	return pa;
+}
+
+static vaddr_t core_mmu_get_ul1_ttb_va(void)
+{
+	return (vaddr_t)main_mmu_ul1_ttb[thread_get_id()];
+}
+
+static paddr_t core_mmu_get_ul1_ttb_pa(void)
+{
+	paddr_t pa = virt_to_phys((void *)core_mmu_get_ul1_ttb_va());
+
+	if (pa & ~TTB_UL1_MASK)
+		panic("invalid user l1 table");
+	return pa;
+}
+
+static void *core_mmu_alloc_l2(size_t size)
+{
+	/* Can't have this in .bss since it's not initialized yet */
+	static uint32_t tables_used __early_bss;
+	uint32_t to_alloc = ROUNDUP(size, NUM_L2_ENTRIES * SMALL_PAGE_SIZE) /
+		(NUM_L2_ENTRIES * SMALL_PAGE_SIZE);
+
+	if (tables_used + to_alloc > MAX_XLAT_TABLES)
+		return NULL;
+
+	tables_used += to_alloc;
+	return main_mmu_l2_ttb[tables_used - to_alloc];
+}
+
+static enum desc_type get_desc_type(unsigned level, uint32_t desc)
+{
+	assert(level >= 1 && level <= 2);
+
+	if (level == 1) {
+		if ((desc & 0x3) == 0x1)
+			return DESC_TYPE_PAGE_TABLE;
+
+		if ((desc & 0x2) == 0x2) {
+			if (desc & (1 << 18))
+				return DESC_TYPE_SUPER_SECTION;
+			return DESC_TYPE_SECTION;
+		}
+	} else {
+		if ((desc & 0x3) == 0x1)
+			return DESC_TYPE_LARGE_PAGE;
+
+		if ((desc & 0x2) == 0x2)
+			return DESC_TYPE_SMALL_PAGE;
+	}
+
+	return DESC_TYPE_INVALID;
+}
+
+static uint32_t texcb_to_mattr(uint32_t texcb)
+{
+	COMPILE_TIME_ASSERT(ATTR_DEVICE_INDEX == TEE_MATTR_CACHE_NONCACHE);
+	COMPILE_TIME_ASSERT(ATTR_IWBWA_OWBWA_INDEX == TEE_MATTR_CACHE_CACHED);
+
+	return texcb << TEE_MATTR_CACHE_SHIFT;
+}
+
+static uint32_t mattr_to_texcb(uint32_t attr)
+{
+	/* Keep in sync with core_mmu.c:core_mmu_mattr_is_ok */
+	return (attr >> TEE_MATTR_CACHE_SHIFT) & TEE_MATTR_CACHE_MASK;
+}
+
+
+static uint32_t desc_to_mattr(unsigned level, uint32_t desc)
+{
+	uint32_t a;
+
+	switch (get_desc_type(level, desc)) {
+	case DESC_TYPE_PAGE_TABLE:
+		a = TEE_MATTR_TABLE;
+		if (!(desc & SECTION_PT_NOTSECURE))
+			a |= TEE_MATTR_SECURE;
+		break;
+	case DESC_TYPE_SECTION:
+		a = TEE_MATTR_VALID_BLOCK;
+		if (desc & SECTION_ACCESS_FLAG)
+			a |= TEE_MATTR_PRX | TEE_MATTR_URX;
+
+		if (!(desc & SECTION_RO))
+			a |= TEE_MATTR_PW | TEE_MATTR_UW;
+
+		if (desc & SECTION_XN)
+			a &= ~(TEE_MATTR_PX | TEE_MATTR_UX);
+
+		if (desc & SECTION_PXN)
+			a &= ~TEE_MATTR_PX;
+
+		a |= texcb_to_mattr(((desc >> 12) & 0x7) | ((desc >> 2) & 0x3));
+
+		if (!(desc & SECTION_NOTGLOBAL))
+			a |= TEE_MATTR_GLOBAL;
+
+		if (!(desc & SECTION_NOTSECURE))
+			a |= TEE_MATTR_SECURE;
+
+		break;
+	case DESC_TYPE_SMALL_PAGE:
+		a = TEE_MATTR_VALID_BLOCK;
+		if (desc & SMALL_PAGE_ACCESS_FLAG)
+			a |= TEE_MATTR_PRX | TEE_MATTR_URX;
+
+		if (!(desc & SMALL_PAGE_RO))
+			a |= TEE_MATTR_PW | TEE_MATTR_UW;
+
+		if (desc & SMALL_PAGE_XN)
+			a &= ~(TEE_MATTR_PX | TEE_MATTR_UX);
+
+		a |= texcb_to_mattr(((desc >> 6) & 0x7) | ((desc >> 2) & 0x3));
+
+		if (!(desc & SMALL_PAGE_NOTGLOBAL))
+			a |= TEE_MATTR_GLOBAL;
+		break;
+	case DESC_TYPE_INVALID:
+		if (desc & HIDDEN_DESC)
+			return TEE_MATTR_HIDDEN_BLOCK;
+		if (desc & HIDDEN_DIRTY_DESC)
+			return TEE_MATTR_HIDDEN_DIRTY_BLOCK;
+		return 0;
+	default:
+		return 0;
+	}
+
+	return a;
+}
+
+static uint32_t mattr_to_desc(unsigned level, uint32_t attr)
+{
+	uint32_t desc;
+	uint32_t a = attr;
+	unsigned texcb;
+
+	if (a & TEE_MATTR_HIDDEN_BLOCK)
+		return INVALID_DESC | HIDDEN_DESC;
+
+	if (a & TEE_MATTR_HIDDEN_DIRTY_BLOCK)
+		return INVALID_DESC | HIDDEN_DIRTY_DESC;
+
+	if (level == 1 && (a & TEE_MATTR_TABLE)) {
+		desc = SECTION_PT_PT;
+		if (!(a & TEE_MATTR_SECURE))
+			desc |= SECTION_PT_NOTSECURE;
+		return desc;
+	}
+
+	if (!(a & TEE_MATTR_VALID_BLOCK))
+		return 0;
+
+	if (a & (TEE_MATTR_PX | TEE_MATTR_PW))
+		a |= TEE_MATTR_PR;
+	if (a & (TEE_MATTR_UX | TEE_MATTR_UW))
+		a |= TEE_MATTR_UR;
+	if (a & TEE_MATTR_UR)
+		a |= TEE_MATTR_PR;
+	if (a & TEE_MATTR_UW)
+		a |= TEE_MATTR_PW;
+
+
+	texcb = mattr_to_texcb(a);
+
+	if (level == 1) {	/* Section */
+		desc = SECTION_SECTION | SECTION_SHARED;
+
+		if (!(a & (TEE_MATTR_PX | TEE_MATTR_UX)))
+			desc |= SECTION_XN;
+
+#ifdef CFG_HWSUPP_MEM_PERM_PXN
+		if (!(a & TEE_MATTR_PX))
+			desc |= SECTION_PXN;
+#endif
+
+		if (a & TEE_MATTR_UR)
+			desc |= SECTION_UNPRIV;
+
+		if (!(a & TEE_MATTR_PW))
+			desc |= SECTION_RO;
+
+		if (a & (TEE_MATTR_UR | TEE_MATTR_PR))
+			desc |= SECTION_ACCESS_FLAG;
+
+		if (!(a & TEE_MATTR_GLOBAL))
+			desc |= SECTION_NOTGLOBAL;
+
+		if (!(a & TEE_MATTR_SECURE))
+			desc |= SECTION_NOTSECURE;
+
+		desc |= SECTION_TEXCB(texcb);
+	} else {
+		desc = SMALL_PAGE_SMALL_PAGE | SMALL_PAGE_SHARED;
+
+		if (!(a & (TEE_MATTR_PX | TEE_MATTR_UX)))
+			desc |= SMALL_PAGE_XN;
+
+		if (a & TEE_MATTR_UR)
+			desc |= SMALL_PAGE_UNPRIV;
+
+		if (!(a & TEE_MATTR_PW))
+			desc |= SMALL_PAGE_RO;
+
+		if (a & (TEE_MATTR_UR | TEE_MATTR_PR))
+			desc |= SMALL_PAGE_ACCESS_FLAG;
+
+		if (!(a & TEE_MATTR_GLOBAL))
+			desc |= SMALL_PAGE_NOTGLOBAL;
+
+		desc |= SMALL_PAGE_TEXCB(texcb);
+	}
+
+	return desc;
+}
+
+void core_mmu_set_info_table(struct core_mmu_table_info *tbl_info,
+		unsigned level, vaddr_t va_base, void *table)
+{
+	tbl_info->level = level;
+	tbl_info->table = table;
+	tbl_info->va_base = va_base;
+	assert(level <= 2);
+	if (level == 1) {
+		tbl_info->shift = SECTION_SHIFT;
+		tbl_info->num_entries = NUM_L1_ENTRIES;
+	} else {
+		tbl_info->shift = SMALL_PAGE_SHIFT;
+		tbl_info->num_entries = NUM_L2_ENTRIES;
+	}
+}
+
+void core_mmu_get_user_pgdir(struct core_mmu_table_info *pgd_info)
+{
+	void *tbl = (void *)core_mmu_get_ul1_ttb_va();
+
+	core_mmu_set_info_table(pgd_info, 1, 0, tbl);
+	pgd_info->num_entries = NUM_UL1_ENTRIES;
+}
+
+void core_mmu_create_user_map(struct user_ta_ctx *utc,
+			      struct core_mmu_user_map *map)
+{
+	struct core_mmu_table_info dir_info;
+
+	COMPILE_TIME_ASSERT(L2_TBL_SIZE == PGT_SIZE);
+
+	core_mmu_get_user_pgdir(&dir_info);
+	memset(dir_info.table, 0, dir_info.num_entries * sizeof(uint32_t));
+	core_mmu_populate_user_map(&dir_info, utc);
+	map->ttbr0 = core_mmu_get_ul1_ttb_pa() | TEE_MMU_DEFAULT_ATTRS;
+	map->ctxid = utc->context & 0xff;
+}
+
+bool core_mmu_find_table(vaddr_t va, unsigned max_level,
+		struct core_mmu_table_info *tbl_info)
+{
+	uint32_t *tbl = (uint32_t *)core_mmu_get_main_ttb_va();
+	unsigned n = va >> SECTION_SHIFT;
+
+	if (max_level == 1 || (tbl[n] & 0x3) != 0x1) {
+		core_mmu_set_info_table(tbl_info, 1, 0, tbl);
+	} else {
+		paddr_t ntbl = tbl[n] & ~((1 << 10) - 1);
+		void *l2tbl = phys_to_virt(ntbl, MEM_AREA_TEE_RAM);
+
+		if (!l2tbl)
+			return false;
+
+		core_mmu_set_info_table(tbl_info, 2, n << SECTION_SHIFT, l2tbl);
+	}
+	return true;
+}
+
+bool core_mmu_divide_block(struct core_mmu_table_info *tbl_info,
+			   unsigned int idx)
+{
+	uint32_t *new_table;
+	uint32_t *entry;
+	uint32_t new_table_desc;
+	paddr_t paddr;
+	uint32_t attr;
+	int i;
+
+	if (tbl_info->level != 1)
+		return false;
+
+	if (idx >= NUM_L1_ENTRIES)
+		return false;
+
+	new_table = core_mmu_alloc_l2(NUM_L2_ENTRIES * SMALL_PAGE_SIZE);
+	if (!new_table)
+		return false;
+
+	entry = (uint32_t *)tbl_info->table + idx;
+	assert(get_desc_type(1, *entry) == DESC_TYPE_SECTION);
+
+	new_table_desc = SECTION_PT_PT | (uint32_t)new_table;
+	if (*entry & SECTION_NOTSECURE)
+		new_table_desc |= SECTION_PT_NOTSECURE;
+
+	/* store attributes of original block */
+	attr = desc_to_mattr(1, *entry);
+	paddr = *entry & ~SECTION_MASK;
+
+	/* Fill new xlat table with entries pointing to the same memory */
+	for (i = 0; i < NUM_L2_ENTRIES; i++) {
+		*new_table = paddr | mattr_to_desc(tbl_info->level + 1, attr);
+		paddr += SMALL_PAGE_SIZE;
+		new_table++;
+	}
+
+	/* Update descriptor at current level */
+	*entry = new_table_desc;
+	return true;
+}
+
+void core_mmu_set_entry_primitive(void *table, size_t level, size_t idx,
+				  paddr_t pa, uint32_t attr)
+{
+	uint32_t *tbl = table;
+	uint32_t desc = mattr_to_desc(level, attr);
+
+	tbl[idx] = desc | pa;
+}
+
+static paddr_t desc_to_pa(unsigned level, uint32_t desc)
+{
+	unsigned shift_mask;
+
+	switch (get_desc_type(level, desc)) {
+	case DESC_TYPE_PAGE_TABLE:
+		shift_mask = 10;
+		break;
+	case DESC_TYPE_SECTION:
+		shift_mask = 20;
+		break;
+	case DESC_TYPE_SUPER_SECTION:
+		shift_mask = 24; /* We're ignoring bits 32 and above. */
+		break;
+	case DESC_TYPE_LARGE_PAGE:
+		shift_mask = 16;
+		break;
+	case DESC_TYPE_SMALL_PAGE:
+		shift_mask = 12;
+		break;
+	default:
+		/* Invalid section, HIDDEN_DESC, HIDDEN_DIRTY_DESC */
+		shift_mask = 4;
+	}
+
+	return desc & ~((1 << shift_mask) - 1);
+}
+
+void core_mmu_get_entry_primitive(const void *table, size_t level,
+				  size_t idx, paddr_t *pa, uint32_t *attr)
+{
+	const uint32_t *tbl = table;
+
+	if (pa)
+		*pa = desc_to_pa(level, tbl[idx]);
+
+	if (attr)
+		*attr = desc_to_mattr(level, tbl[idx]);
+}
+
+void core_mmu_get_user_va_range(vaddr_t *base, size_t *size)
+{
+	if (base) {
+		/* Leaving the first entry unmapped to make NULL unmapped */
+		*base = 1 << SECTION_SHIFT;
+	}
+
+	if (size)
+		*size = (NUM_UL1_ENTRIES - 1) << SECTION_SHIFT;
+}
+
+void core_mmu_get_user_map(struct core_mmu_user_map *map)
+{
+	map->ttbr0 = read_ttbr0();
+	map->ctxid = read_contextidr();
+}
+
+void core_mmu_set_user_map(struct core_mmu_user_map *map)
+{
+	uint32_t exceptions = thread_mask_exceptions(THREAD_EXCP_ALL);
+
+	/*
+	 * Update the reserved Context ID and TTBR0
+	 */
+
+	dsb();  /* ARM erratum 754322 */
+	write_contextidr(0);
+	isb();
+
+	if (map) {
+		write_ttbr0(map->ttbr0);
+		isb();
+		write_contextidr(map->ctxid);
+	} else {
+		write_ttbr0(read_ttbr1());
+	}
+	isb();
+	core_tlb_maintenance(TLBINV_UNIFIEDTLB, 0);
+
+	/* Restore interrupts */
+	thread_unmask_exceptions(exceptions);
+}
+
+bool core_mmu_user_mapping_is_active(void)
+{
+	return read_ttbr0() != read_ttbr1();
+}
+
+static paddr_t map_page_memarea(struct tee_mmap_region *mm)
+{
+	uint32_t *l2 = core_mmu_alloc_l2(mm->size);
+	size_t pg_idx;
+	uint32_t attr;
+
+	if (!l2)
+		panic("no l2 table");
+
+	attr = mattr_to_desc(2, mm->attr);
+
+	/* Zero fill initial entries */
+	pg_idx = 0;
+	while ((pg_idx * SMALL_PAGE_SIZE) < (mm->va & SECTION_MASK)) {
+		l2[pg_idx] = 0;
+		pg_idx++;
+	}
+
+	/* Fill in the entries */
+	while ((pg_idx * SMALL_PAGE_SIZE) <
+		(mm->size + (mm->va & SECTION_MASK))) {
+		l2[pg_idx] = ((mm->pa & ~SECTION_MASK) +
+				pg_idx * SMALL_PAGE_SIZE) | attr;
+		pg_idx++;
+	}
+
+	/* Zero fill the rest */
+	while (pg_idx < ROUNDUP(mm->size, SECTION_SIZE) / SMALL_PAGE_SIZE) {
+		l2[pg_idx] = 0;
+		pg_idx++;
+	}
+
+	return virt_to_phys(l2);
+}
+
+/*
+* map_memarea - load mapping in target L1 table
+* A finer mapping must be supported. Currently section mapping only!
+*/
+static void map_memarea(struct tee_mmap_region *mm, uint32_t *ttb)
+{
+	size_t m, n;
+	uint32_t attr;
+	paddr_t pa;
+	uint32_t region_size;
+
+	assert(mm && ttb);
+
+	/*
+	 * If mm->va is smaller than 32M, then mm->va will conflict with
+	 * user TA address space. This mapping will be overridden/hidden
+	 * later when a user TA is loaded since these low addresses are
+	 * used as TA virtual address space.
+	 *
+	 * Some SoCs have devices at low addresses, so we need to map at
+	 * least those devices at a virtual address which isn't the same
+	 * as the physical.
+	 *
+	 * TODO: support mapping devices at a virtual address which isn't
+	 * the same as the physical address.
+	 */
+	if (mm->va < (NUM_UL1_ENTRIES * SECTION_SIZE))
+		panic("va conflicts with user ta address");
+
+	if ((mm->va | mm->pa | mm->size) & SECTION_MASK) {
+		region_size = SMALL_PAGE_SIZE;
+
+		/*
+		 * Need finer grained mapping, if small pages aren't
+		 * good enough, panic.
+		 */
+		if ((mm->va | mm->pa | mm->size) & SMALL_PAGE_MASK)
+			panic("memarea can't be mapped");
+
+		attr = mattr_to_desc(1, mm->attr | TEE_MATTR_TABLE);
+		pa = map_page_memarea(mm);
+	} else {
+		region_size = SECTION_SIZE;
+
+		attr = mattr_to_desc(1, mm->attr);
+		pa = mm->pa;
+	}
+
+	m = (mm->va >> SECTION_SHIFT);
+	n = ROUNDUP(mm->size, SECTION_SIZE) >> SECTION_SHIFT;
+	while (n--) {
+		ttb[m] = pa | attr;
+		m++;
+		if (region_size == SECTION_SIZE)
+			pa += SECTION_SIZE;
+		else
+			pa += L2_TBL_SIZE;
+	}
+}
+
+void core_init_mmu_tables(struct tee_mmap_region *mm)
+{
+	void *ttb1 = (void *)core_mmu_get_main_ttb_va();
+	size_t n;
+
+	/* reset L1 table */
+	memset(ttb1, 0, L1_TBL_SIZE);
+
+	for (n = 0; mm[n].size; n++)
+		map_memarea(mm + n, ttb1);
+}
+
+bool core_mmu_place_tee_ram_at_top(paddr_t paddr)
+{
+	return paddr > 0x80000000;
+}
+
+void core_init_mmu_regs(void)
+{
+	uint32_t prrr;
+	uint32_t nmrr;
+	paddr_t ttb_pa = core_mmu_get_main_ttb_pa();
+
+	/* Enable Access flag (simplified access permissions) and TEX remap */
+	write_sctlr(read_sctlr() | SCTLR_AFE | SCTLR_TRE);
+
+	prrr = ATTR_DEVICE_PRRR | ATTR_IWBWA_OWBWA_PRRR;
+	nmrr = ATTR_DEVICE_NMRR | ATTR_IWBWA_OWBWA_NMRR;
+
+	prrr |= PRRR_NS1 | PRRR_DS1;
+
+	write_prrr(prrr);
+	write_nmrr(nmrr);
+
+
+	/*
+	 * Program Domain access control register with two domains:
+	 * domain 0: teecore
+	 * domain 1: TA
+	 */
+	write_dacr(DACR_DOMAIN(0, DACR_DOMAIN_PERM_CLIENT) |
+		   DACR_DOMAIN(1, DACR_DOMAIN_PERM_CLIENT));
+
+	/*
+	 * Enable lookups using TTBR0 and TTBR1 with the split of addresses
+	 * defined by TEE_MMU_TTBCR_N_VALUE.
+	 */
+	write_ttbcr(TTBCR_N_VALUE);
+
+	write_ttbr0(ttb_pa | TEE_MMU_DEFAULT_ATTRS);
+	write_ttbr1(ttb_pa | TEE_MMU_DEFAULT_ATTRS);
+}
+
+enum core_mmu_fault core_mmu_get_fault_type(uint32_t fsr)
+{
+	assert(!(fsr & FSR_LPAE));
+
+	switch (fsr & FSR_FS_MASK) {
+	case 0x1: /* DFSR[10,3:0] 0b00001 Alignment fault (DFSR only) */
+		return CORE_MMU_FAULT_ALIGNMENT;
+	case 0x2: /* DFSR[10,3:0] 0b00010 Debug event */
+		return CORE_MMU_FAULT_DEBUG_EVENT;
+	case 0x4: /* DFSR[10,3:0] b00100 Fault on instr cache maintenance */
+	case 0x5: /* DFSR[10,3:0] b00101 Translation fault first level */
+	case 0x7: /* DFSR[10,3:0] b00111 Translation fault second level */
+		return CORE_MMU_FAULT_TRANSLATION;
+	case 0xd: /* DFSR[10,3:0] b01101 Permission fault first level */
+	case 0xf: /* DFSR[10,3:0] b01111 Permission fault second level */
+		if (fsr & FSR_WNR)
+			return CORE_MMU_FAULT_WRITE_PERMISSION;
+		else
+			return CORE_MMU_FAULT_READ_PERMISSION;
+	case 0x3: /* DFSR[10,3:0] b00011 access bit fault on section */
+	case 0x6: /* DFSR[10,3:0] b00110 access bit fault on page */
+		return CORE_MMU_FAULT_ACCESS_BIT;
+	case (1 << 10) | 0x6:
+		/* DFSR[10,3:0] 0b10110 Async external abort (DFSR only) */
+		return CORE_MMU_FAULT_ASYNC_EXTERNAL;
+
+	default:
+		return CORE_MMU_FAULT_OTHER;
+	}
+}
diff --git a/core/arch/arm/mm/mobj.c b/core/arch/arm/mm/mobj.c
new file mode 100644
index 0000000..5458638
--- /dev/null
+++ b/core/arch/arm/mm/mobj.c
@@ -0,0 +1,439 @@
+/*
+ * Copyright (c) 2016-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 <keep.h>
+#include <kernel/mutex.h>
+#include <kernel/panic.h>
+#include <kernel/tee_misc.h>
+#include <mm/core_mmu.h>
+#include <mm/mobj.h>
+#include <mm/tee_mmu.h>
+#include <mm/tee_pager.h>
+#include <optee_msg.h>
+#include <sm/optee_smc.h>
+#include <stdlib.h>
+#include <tee_api_types.h>
+#include <types_ext.h>
+#include <util.h>
+
+struct mobj *mobj_sec_ddr;
+
+/*
+ * mobj_phys implementation
+ */
+
+struct mobj_phys {
+	struct mobj mobj;
+	enum buf_is_attr battr;
+	uint32_t cattr; /* Defined by TEE_MATTR_CACHE_* in tee_mmu_types.h */
+	vaddr_t va;
+	paddr_t pa;
+};
+
+static struct mobj_phys *to_mobj_phys(struct mobj *mobj);
+
+static void *mobj_phys_get_va(struct mobj *mobj, size_t offset)
+{
+	struct mobj_phys *moph = to_mobj_phys(mobj);
+
+	if (!moph->va)
+		return NULL;
+
+	return (void *)(moph->va + offset);
+}
+
+static TEE_Result mobj_phys_get_pa(struct mobj *mobj, size_t offs,
+				   size_t granule, paddr_t *pa)
+{
+	struct mobj_phys *moph = to_mobj_phys(mobj);
+	paddr_t p;
+
+	if (!pa)
+		return TEE_ERROR_GENERIC;
+
+	p = moph->pa + offs;
+
+	if (granule) {
+		if (granule != SMALL_PAGE_SIZE &&
+		    granule != CORE_MMU_PGDIR_SIZE)
+			return TEE_ERROR_GENERIC;
+		p &= ~(granule - 1);
+	}
+
+	*pa = p;
+	return TEE_SUCCESS;
+}
+/* ifndef due to an asserting AArch64 linker */
+#ifndef ARM64
+KEEP_PAGER(mobj_phys_get_pa);
+#endif
+
+static TEE_Result mobj_phys_get_cattr(struct mobj *mobj, uint32_t *cattr)
+{
+	struct mobj_phys *moph = to_mobj_phys(mobj);
+
+	if (!cattr)
+		return TEE_ERROR_GENERIC;
+
+	*cattr = moph->cattr;
+	return TEE_SUCCESS;
+}
+
+static bool mobj_phys_matches(struct mobj *mobj, enum buf_is_attr attr)
+{
+	struct mobj_phys *moph = to_mobj_phys(mobj);
+	enum buf_is_attr a;
+
+	a = moph->battr;
+
+	switch (attr) {
+	case CORE_MEM_SEC:
+		return a == CORE_MEM_SEC || a == CORE_MEM_TEE_RAM ||
+		       a == CORE_MEM_TA_RAM;
+	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:
+		return attr == a;
+	default:
+		return false;
+	}
+}
+
+static void mobj_phys_free(struct mobj *mobj)
+{
+	struct mobj_phys *moph = to_mobj_phys(mobj);
+
+	free(moph);
+}
+
+static const struct mobj_ops mobj_phys_ops __rodata_unpaged = {
+	.get_va = mobj_phys_get_va,
+	.get_pa = mobj_phys_get_pa,
+	.get_cattr = mobj_phys_get_cattr,
+	.matches = mobj_phys_matches,
+	.free = mobj_phys_free,
+};
+
+static struct mobj_phys *to_mobj_phys(struct mobj *mobj)
+{
+	assert(mobj->ops == &mobj_phys_ops);
+	return container_of(mobj, struct mobj_phys, mobj);
+}
+
+struct mobj *mobj_phys_alloc(paddr_t pa, size_t size, uint32_t cattr,
+			     enum buf_is_attr battr)
+{
+	struct mobj_phys *moph;
+	void *va;
+
+	if ((pa & CORE_MMU_USER_PARAM_MASK) ||
+	    (size & CORE_MMU_USER_PARAM_MASK)) {
+		DMSG("Expect %#x alignment", CORE_MMU_USER_PARAM_SIZE);
+		return NULL;
+	}
+
+	va = phys_to_virt(pa, battr);
+	if (!va)
+		return NULL;
+
+	moph = calloc(1, sizeof(*moph));
+	if (!moph)
+		return NULL;
+
+	moph->battr = battr;
+	moph->cattr = cattr;
+	moph->mobj.size = size;
+	moph->mobj.ops = &mobj_phys_ops;
+	moph->pa = pa;
+	moph->va = (vaddr_t)va;
+
+	return &moph->mobj;
+}
+
+/*
+ * mobj_virt implementation
+ */
+
+static void mobj_virt_assert_type(struct mobj *mobj);
+
+static void *mobj_virt_get_va(struct mobj *mobj, size_t offset)
+{
+	mobj_virt_assert_type(mobj);
+
+	return (void *)(vaddr_t)offset;
+}
+
+static const struct mobj_ops mobj_virt_ops __rodata_unpaged = {
+	.get_va = mobj_virt_get_va,
+};
+
+static void mobj_virt_assert_type(struct mobj *mobj __maybe_unused)
+{
+	assert(mobj->ops == &mobj_virt_ops);
+}
+
+struct mobj mobj_virt = { .ops = &mobj_virt_ops, .size = SIZE_MAX };
+
+/*
+ * mobj_mm implementation
+ */
+
+struct mobj_mm {
+	tee_mm_entry_t *mm;
+	struct mobj *parent_mobj;
+	struct mobj mobj;
+};
+
+static struct mobj_mm *to_mobj_mm(struct mobj *mobj);
+
+static size_t mobj_mm_offs(struct mobj *mobj, size_t offs)
+{
+	tee_mm_entry_t *mm = to_mobj_mm(mobj)->mm;
+
+	return (mm->offset << mm->pool->shift) + offs;
+}
+
+static void *mobj_mm_get_va(struct mobj *mobj, size_t offs)
+{
+	return mobj_get_va(to_mobj_mm(mobj)->parent_mobj,
+			   mobj_mm_offs(mobj, offs));
+}
+
+
+static TEE_Result mobj_mm_get_pa(struct mobj *mobj, size_t offs,
+				    size_t granule, paddr_t *pa)
+{
+	return mobj_get_pa(to_mobj_mm(mobj)->parent_mobj,
+			   mobj_mm_offs(mobj, offs), granule, pa);
+}
+/* ifndef due to an asserting AArch64 linker */
+#ifndef ARM64
+KEEP_PAGER(mobj_mm_get_pa);
+#endif
+
+static TEE_Result mobj_mm_get_cattr(struct mobj *mobj, uint32_t *cattr)
+{
+	return mobj_get_cattr(to_mobj_mm(mobj)->parent_mobj, cattr);
+}
+
+static bool mobj_mm_matches(struct mobj *mobj, enum buf_is_attr attr)
+{
+	return mobj_matches(to_mobj_mm(mobj)->parent_mobj, attr);
+}
+
+static void mobj_mm_free(struct mobj *mobj)
+{
+	struct mobj_mm *m = to_mobj_mm(mobj);
+
+	tee_mm_free(m->mm);
+	free(m);
+}
+
+static const struct mobj_ops mobj_mm_ops __rodata_unpaged = {
+	.get_va = mobj_mm_get_va,
+	.get_pa = mobj_mm_get_pa,
+	.get_cattr = mobj_mm_get_cattr,
+	.matches = mobj_mm_matches,
+	.free = mobj_mm_free,
+};
+
+static struct mobj_mm *to_mobj_mm(struct mobj *mobj)
+{
+	assert(mobj->ops == &mobj_mm_ops);
+	return container_of(mobj, struct mobj_mm, mobj);
+}
+
+struct mobj *mobj_mm_alloc(struct mobj *mobj_parent, size_t size,
+			      tee_mm_pool_t *pool)
+{
+	struct mobj_mm *m = calloc(1, sizeof(*m));
+
+	if (!m)
+		return NULL;
+
+	m->mm = tee_mm_alloc(pool, size);
+	if (!m->mm) {
+		free(m);
+		return NULL;
+	}
+
+	m->parent_mobj = mobj_parent;
+	m->mobj.size = size;
+	m->mobj.ops = &mobj_mm_ops;
+
+	return &m->mobj;
+}
+
+#ifdef CFG_PAGED_USER_TA
+/*
+ * mobj_paged implementation
+ */
+
+static void mobj_paged_free(struct mobj *mobj);
+
+static const struct mobj_ops mobj_paged_ops __rodata_unpaged = {
+	.free = mobj_paged_free,
+};
+
+static void mobj_paged_free(struct mobj *mobj)
+{
+	assert(mobj->ops == &mobj_paged_ops);
+	free(mobj);
+}
+
+struct mobj *mobj_paged_alloc(size_t size)
+{
+	struct mobj *mobj = calloc(1, sizeof(*mobj));
+
+	if (mobj) {
+		mobj->size = size;
+		mobj->ops = &mobj_paged_ops;
+	}
+	return mobj;
+}
+
+/*
+ * mobj_seccpy_shm implementation
+ */
+
+struct mobj_seccpy_shm {
+	struct user_ta_ctx *utc;
+	vaddr_t va;
+	size_t pgdir_offset;
+	struct mobj mobj;
+};
+
+static bool __maybe_unused mobj_is_seccpy_shm(struct mobj *mobj);
+
+static struct mobj_seccpy_shm *to_mobj_seccpy_shm(struct mobj *mobj)
+{
+	assert(mobj_is_seccpy_shm(mobj));
+	return container_of(mobj, struct mobj_seccpy_shm, mobj);
+}
+
+static void *mobj_seccpy_shm_get_va(struct mobj *mobj, size_t offs)
+{
+	struct mobj_seccpy_shm *m = to_mobj_seccpy_shm(mobj);
+
+	if (&m->utc->ctx != thread_get_tsd()->ctx)
+		return NULL;
+
+	if (offs >= mobj->size)
+		return NULL;
+	return (void *)(m->va + offs);
+}
+
+static bool mobj_seccpy_shm_matches(struct mobj *mobj __maybe_unused,
+				 enum buf_is_attr attr)
+{
+	assert(mobj_is_seccpy_shm(mobj));
+
+	return attr == CORE_MEM_SEC || attr == CORE_MEM_TEE_RAM;
+}
+
+static void mobj_seccpy_shm_free(struct mobj *mobj)
+{
+	struct mobj_seccpy_shm *m = to_mobj_seccpy_shm(mobj);
+
+	tee_pager_rem_uta_region(m->utc, m->va, mobj->size);
+	tee_mmu_rem_rwmem(m->utc, mobj, m->va);
+	free(m);
+}
+
+static void mobj_seccpy_shm_update_mapping(struct mobj *mobj,
+					struct user_ta_ctx *utc, vaddr_t va)
+{
+	struct thread_specific_data *tsd = thread_get_tsd();
+	struct mobj_seccpy_shm *m = to_mobj_seccpy_shm(mobj);
+	size_t s;
+
+	if (utc == m->utc && va == m->va)
+		return;
+
+	s = ROUNDUP(mobj->size, SMALL_PAGE_SIZE);
+	pgt_transfer(&tsd->pgt_cache, &m->utc->ctx, m->va, &utc->ctx, va, s);
+
+	m->va = va;
+	m->utc = utc;
+}
+
+static const struct mobj_ops mobj_seccpy_shm_ops __rodata_unpaged = {
+	.get_va = mobj_seccpy_shm_get_va,
+	.matches = mobj_seccpy_shm_matches,
+	.free = mobj_seccpy_shm_free,
+	.update_mapping = mobj_seccpy_shm_update_mapping,
+};
+
+static bool mobj_is_seccpy_shm(struct mobj *mobj)
+{
+	return mobj && mobj->ops == &mobj_seccpy_shm_ops;
+}
+
+struct mobj *mobj_seccpy_shm_alloc(size_t size)
+{
+	struct thread_specific_data *tsd = thread_get_tsd();
+	struct mobj_seccpy_shm *m;
+	struct user_ta_ctx *utc;
+	vaddr_t va = 0;
+
+	if (!is_user_ta_ctx(tsd->ctx))
+		return NULL;
+	utc = to_user_ta_ctx(tsd->ctx);
+
+	m = calloc(1, sizeof(*m));
+	if (!m)
+		return NULL;
+
+	m->mobj.size = size;
+	m->mobj.ops = &mobj_seccpy_shm_ops;
+
+	if (tee_mmu_add_rwmem(utc, &m->mobj, -1, &va) != TEE_SUCCESS)
+		goto bad;
+
+	if (!tee_pager_add_uta_area(utc, va, size))
+		goto bad;
+
+	m->va = va;
+	m->pgdir_offset = va & CORE_MMU_PGDIR_MASK;
+	m->utc = to_user_ta_ctx(tsd->ctx);
+	return &m->mobj;
+bad:
+	if (va)
+		tee_mmu_rem_rwmem(utc, &m->mobj, va);
+	free(m);
+	return NULL;
+}
+
+bool mobj_is_paged(struct mobj *mobj)
+{
+	return mobj->ops == &mobj_paged_ops ||
+	       mobj->ops == &mobj_seccpy_shm_ops;
+}
+#endif /*CFG_PAGED_USER_TA*/
diff --git a/core/arch/arm/mm/pager_aes_gcm.c b/core/arch/arm/mm/pager_aes_gcm.c
new file mode 100644
index 0000000..e0ff286
--- /dev/null
+++ b/core/arch/arm/mm/pager_aes_gcm.c
@@ -0,0 +1,348 @@
+/*
+ * Galois/Counter Mode (GCM) and GMAC with AES
+ *
+ * Copyright (c) 2016, Linaro Limited
+ * Copyright (c) 2012, Jouni Malinen <j@w1.fi>
+ *
+ * This software may be distributed under the terms of the BSD license.
+ * See README for more details.
+ *
+ * The license part of what was the "README" above:
+ * License
+ * -------
+ *
+ * This software may be distributed, used, and modified under the terms of
+ * BSD license:
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are
+ * met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * 3. Neither the name(s) of the above-listed copyright holder(s) nor the
+ *    names of its contributors may be used to endorse or promote products
+ *    derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE 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
+ * OWNER 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 <compiler.h>
+#include "pager_private.h"
+#include <tomcrypt.h>
+#include <trace.h>
+#include <utee_defines.h>
+#include <util.h>
+
+/*
+ * Source copied from git://w1.fi/srv/git/hostap.git files
+ * src/utils/common.h and src/crypto/aes-gcm.c
+ *
+ * The source has been modified for the pager use case.
+ */
+
+#define BLOCK_ALIGNMENT	sizeof(uint64_t)
+
+static uint32_t get_be32(const void *a)
+{
+	return TEE_U32_FROM_BIG_ENDIAN(*(const uint32_t *)a);
+}
+
+static void put_be32(void *a, uint32_t val)
+{
+	*(uint32_t *)a = TEE_U32_TO_BIG_ENDIAN(val);
+}
+
+static void put_be64(void *a, uint64_t val)
+{
+	*(uint64_t *)a = TEE_U64_TO_BIG_ENDIAN(val);
+}
+
+static void aes_encrypt(symmetric_key *skey, const uint8_t *plain,
+			uint8_t *crypt)
+{
+	aes_ecb_encrypt(plain, crypt, skey);
+}
+
+static void inc32(uint8_t *block)
+{
+	uint32_t val;
+
+	val = get_be32(block + TEE_AES_BLOCK_SIZE - 4);
+	val++;
+	put_be32(block + TEE_AES_BLOCK_SIZE - 4, val);
+}
+
+static void xor_block(void *dst, const void *src)
+{
+	uint64_t *d = dst;
+	const uint64_t *s = src;
+
+	*d++ ^= *s++;
+	*d++ ^= *s++;
+}
+
+static void shift_right_block(uint8_t *v)
+{
+	uint32_t next_val;
+	uint32_t val;
+
+	val = get_be32(v + 12);
+	next_val = get_be32(v + 8);
+	val >>= 1;
+	val |= next_val << 31;
+	put_be32(v + 12, val);
+
+	val = next_val;
+	next_val = get_be32(v + 4);
+	val >>= 1;
+	val |= next_val << 31;
+	put_be32(v + 8, val);
+
+	val = next_val;
+	next_val = get_be32(v);
+	val >>= 1;
+	val |= next_val << 31;
+	put_be32(v + 4, val);
+
+	val = next_val;
+	val >>= 1;
+	put_be32(v, val);
+}
+
+/* Multiplication in GF(2^128) */
+static void gf_mult(const uint8_t *x, const uint8_t *y, uint8_t *z)
+{
+	uint8_t v[TEE_AES_BLOCK_SIZE] __aligned(BLOCK_ALIGNMENT);
+	unsigned i;
+	unsigned j;
+
+	memset(z, 0, TEE_AES_BLOCK_SIZE); /* Z_0 = 0^128 */
+	memcpy(v, y, TEE_AES_BLOCK_SIZE); /* V_0 = Y */
+
+	for (i = 0; i < TEE_AES_BLOCK_SIZE; i++) {
+		for (j = 0; j < 8; j++) {
+			if (x[i] & BIT(7 - j)) {
+				/* Z_(i + 1) = Z_i XOR V_i */
+				xor_block(z, v);
+			} else {
+				/* Z_(i + 1) = Z_i */
+			}
+
+			if (v[15] & 0x01) {
+				/* V_(i + 1) = (V_i >> 1) XOR R */
+				shift_right_block(v);
+				/* R = 11100001 || 0^120 */
+				v[0] ^= 0xe1;
+			} else {
+				/* V_(i + 1) = V_i >> 1 */
+				shift_right_block(v);
+			}
+		}
+	}
+}
+
+static void ghash_start(uint8_t *y)
+{
+	/* Y_0 = 0^128 */
+	memset(y, 0, TEE_AES_BLOCK_SIZE);
+}
+
+
+static void ghash(const uint8_t *h, const uint8_t *in, size_t len, uint8_t *out)
+{
+	size_t n;
+	uint8_t tmp[TEE_AES_BLOCK_SIZE] __aligned(BLOCK_ALIGNMENT);
+
+	/* We're only dealing with complete blocks */
+	assert(!(len % TEE_AES_BLOCK_SIZE));
+
+	for (n = 0; n < len; n += TEE_AES_BLOCK_SIZE) {
+		/* Y_i = (Y^(i-1) XOR X_i) dot H */
+		xor_block(out, in + n);
+
+		/* dot operation:
+		 * multiplication operation for binary Galois (finite) field of
+		 * 2^128 elements */
+		gf_mult(out, h, tmp);
+		memcpy(out, tmp, TEE_AES_BLOCK_SIZE);
+	}
+	/* Return Y_m */
+}
+
+static bool aes_gcm_init_hash_subkey(symmetric_key *skey, const uint8_t *key,
+				     size_t key_len, uint8_t *H)
+{
+	if (aes_setup(key, key_len, 0, skey) != CRYPT_OK)
+		return false;
+
+	/* Generate hash subkey H = AES_K(0^128) */
+	memset(H, 0, TEE_AES_BLOCK_SIZE);
+	aes_encrypt(skey, H, H);
+	return true;
+}
+
+
+static void aes_gcm_prepare_j0(const struct pager_aes_gcm_iv *iv, uint8_t *J0)
+{
+	/* Prepare block J_0 = IV || 0^31 || 1 [len(IV) = 96] */
+	memcpy(J0, iv, sizeof(*iv));
+	memset(J0 + sizeof(*iv), 0, TEE_AES_BLOCK_SIZE - sizeof(*iv));
+	J0[TEE_AES_BLOCK_SIZE - 1] = 0x01;
+}
+
+static void aes_gcm_core(symmetric_key *skey, bool enc, const uint8_t *J0,
+			 const uint8_t *H, const uint8_t *in, size_t len,
+			 uint8_t *out, uint8_t *tmp, uint8_t *S)
+{
+	uint8_t J0inc[TEE_AES_BLOCK_SIZE] __aligned(BLOCK_ALIGNMENT);
+	size_t n;
+
+	/* We're only dealing with complete blocks */
+	assert(len && !(len % TEE_AES_BLOCK_SIZE));
+
+	/*
+	 * Below in the loop we're doing the encryption and hashing
+	 * on each block interleaved since the encrypted data is stored
+	 * in less secure memory.
+	 */
+
+	/*
+	 * u = 128 * ceil[len(C)/128] - len(C)
+	 * v = 128 * ceil[len(A)/128] - len(A)
+	 * S = GHASH_H(A || 0^v || C || 0^u || [len(A)]64 || [len(C)]64)
+	 * (i.e., zero padded to block size A || C and lengths of each in bits)
+	 */
+	ghash_start(S);
+
+
+	memcpy(J0inc, J0, TEE_AES_BLOCK_SIZE);
+	inc32(J0inc);
+
+	/* Full blocks */
+	for (n = 0; n < len; n += TEE_AES_BLOCK_SIZE) {
+		aes_encrypt(skey, J0inc, tmp);
+		xor_block(tmp, in + n);
+		memcpy(out + n, tmp, TEE_AES_BLOCK_SIZE);
+		inc32(J0inc);
+
+		/* Hash */
+		if (enc)
+			xor_block(S, tmp);
+		else
+			xor_block(S, in + n);
+		gf_mult(S, H, tmp);
+		memcpy(S, tmp, TEE_AES_BLOCK_SIZE);
+	}
+
+	put_be64(tmp, 0); /* no aad */
+	put_be64(tmp + 8, len * 8);
+	ghash(H, tmp, TEE_AES_BLOCK_SIZE, S);
+}
+
+/**
+ * aes_gcm_ae - GCM-AE_K(IV, P, A)
+ */
+static bool aes_gcm_ae(const uint8_t *key, size_t key_len,
+		       const struct pager_aes_gcm_iv *iv,
+		       const uint8_t *plain, size_t plain_len,
+		       uint8_t *crypt, uint8_t *tag)
+{
+	symmetric_key skey;
+	uint8_t H[TEE_AES_BLOCK_SIZE] __aligned(BLOCK_ALIGNMENT);
+	uint8_t J0[TEE_AES_BLOCK_SIZE] __aligned(BLOCK_ALIGNMENT);
+	uint8_t S[TEE_AES_BLOCK_SIZE] __aligned(BLOCK_ALIGNMENT);
+	uint8_t tmp[TEE_AES_BLOCK_SIZE] __aligned(BLOCK_ALIGNMENT);
+
+	if (!aes_gcm_init_hash_subkey(&skey, key, key_len, H))
+		return false;
+
+	aes_gcm_prepare_j0(iv, J0);
+
+	/* C = GCTR_K(inc_32(J_0), P) */
+	aes_gcm_core(&skey, true, J0, H, plain, plain_len, crypt, tmp, S);
+
+	/* T = MSB_t(GCTR_K(J_0, S)) */
+	aes_encrypt(&skey, J0, tag);
+	xor_block(tag, S);
+
+	/* Return (C, T) */
+
+	aes_done(&skey);
+
+	return true;
+}
+
+/**
+ * aes_gcm_ad - GCM-AD_K(IV, C, A, T)
+ */
+static bool aes_gcm_ad(const uint8_t *key, size_t key_len,
+		       const struct pager_aes_gcm_iv *iv,
+		       const uint8_t *crypt, size_t crypt_len,
+		       const uint8_t *tag, uint8_t *plain)
+{
+	symmetric_key skey;
+	uint8_t H[TEE_AES_BLOCK_SIZE] __aligned(BLOCK_ALIGNMENT);
+	uint8_t J0[TEE_AES_BLOCK_SIZE] __aligned(BLOCK_ALIGNMENT);
+	uint8_t S[TEE_AES_BLOCK_SIZE] __aligned(BLOCK_ALIGNMENT);
+	uint8_t tmp[TEE_AES_BLOCK_SIZE] __aligned(BLOCK_ALIGNMENT);
+
+	if (!aes_gcm_init_hash_subkey(&skey, key, key_len, H))
+		return false;
+
+	aes_gcm_prepare_j0(iv, J0);
+
+	/* P = GCTR_K(inc_32(J_0), C) */
+	aes_gcm_core(&skey, false, J0, H, crypt, crypt_len, plain, tmp, S);
+
+	/* T' = MSB_t(GCTR_K(J_0, S)) */
+	aes_encrypt(&skey, J0, tmp);
+	xor_block(tmp, S);
+
+	aes_done(&skey);
+
+	return !buf_compare_ct(tag, tmp, TEE_AES_BLOCK_SIZE);
+}
+
+static bool check_block_alignment(const void *p)
+{
+	return !((vaddr_t)p % BLOCK_ALIGNMENT);
+}
+
+bool pager_aes_gcm_decrypt(const void *key, size_t keylen,
+			   const struct pager_aes_gcm_iv *iv,
+			   const uint8_t tag[PAGER_AES_GCM_TAG_LEN],
+			   const void *src, void *dst, size_t datalen)
+{
+	if (!datalen || (datalen % TEE_AES_BLOCK_SIZE) ||
+	    !check_block_alignment(src) || !check_block_alignment(dst))
+		return false;
+	return aes_gcm_ad(key, keylen, iv, src, datalen, tag, dst);
+}
+
+bool pager_aes_gcm_encrypt(const void *key, size_t keylen,
+			   const struct pager_aes_gcm_iv *iv,
+			   uint8_t tag[PAGER_AES_GCM_TAG_LEN],
+			   const void *src, void *dst, size_t datalen)
+{
+	if (!datalen || (datalen % TEE_AES_BLOCK_SIZE) ||
+	    !check_block_alignment(src) || !check_block_alignment(dst))
+		return false;
+	return aes_gcm_ae(key, keylen, iv, src, datalen, dst, tag);
+}
diff --git a/core/arch/arm/mm/pager_private.h b/core/arch/arm/mm/pager_private.h
new file mode 100644
index 0000000..e7acf95
--- /dev/null
+++ b/core/arch/arm/mm/pager_private.h
@@ -0,0 +1,45 @@
+/*
+ * Copyright (c) 2016, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <types_ext.h>
+
+struct pager_aes_gcm_iv {
+	uint32_t iv[3];
+};
+
+#define PAGER_AES_GCM_TAG_LEN	16
+
+bool pager_aes_gcm_decrypt(const void *key, size_t keylen,
+			   const struct pager_aes_gcm_iv *iv,
+			   const uint8_t tag[PAGER_AES_GCM_TAG_LEN],
+			   const void *src, void *dst, size_t datalen);
+
+bool pager_aes_gcm_encrypt(const void *key, size_t keylen,
+			   const struct pager_aes_gcm_iv *iv,
+			   uint8_t tag[PAGER_AES_GCM_TAG_LEN],
+			   const void *src, void *dst, size_t datalen);
+
diff --git a/core/arch/arm/mm/pgt_cache.c b/core/arch/arm/mm/pgt_cache.c
new file mode 100644
index 0000000..76c9e6e
--- /dev/null
+++ b/core/arch/arm/mm/pgt_cache.c
@@ -0,0 +1,567 @@
+/*
+ * Copyright (c) 2016, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <assert.h>
+#include <kernel/mutex.h>
+#include <kernel/tee_misc.h>
+#include <mm/core_mmu.h>
+#include <mm/pgt_cache.h>
+#include <mm/tee_pager.h>
+#include <stdlib.h>
+#include <trace.h>
+#include <util.h>
+
+/*
+ * With pager enabled we allocate page table from the pager.
+ *
+ * For LPAE each page table is a complete page which is allocated and freed
+ * using the interface provided by the pager.
+ *
+ * For compat v7 page tables there's room for four page table in one page
+ * so we need to keep track of how much of an allocated page is used. When
+ * a page is completely unused it's returned to the pager.
+ *
+ * With pager disabled we have a static allocation of page tables instead.
+ *
+ * In all cases we limit the number of active page tables to
+ * PGT_CACHE_SIZE.  This pool of page tables are shared between all
+ * threads. In case a thread can't allocate the needed number of pager
+ * tables it will release all its current tables and wait for some more to
+ * be freed. A threads allocated tables are freed each time a TA is
+ * unmapped so each thread should be able to allocate the needed tables in
+ * turn if needed.
+ */
+
+#if defined(CFG_WITH_PAGER) && !defined(CFG_WITH_LPAE)
+struct pgt_parent {
+	size_t num_used;
+	struct pgt_cache pgt_cache;
+};
+
+static struct pgt_parent pgt_parents[PGT_CACHE_SIZE / PGT_NUM_PGT_PER_PAGE];
+#else
+
+static struct pgt_cache pgt_free_list = SLIST_HEAD_INITIALIZER(pgt_free_list);
+#endif
+
+#ifdef CFG_PAGED_USER_TA
+/*
+ * When a user TA context is temporarily unmapped the used struct pgt's of
+ * the context (page tables holding valid physical pages) are saved in this
+ * cache in the hope that some of the valid physical pages may still be
+ * valid when the context is mapped again.
+ */
+static struct pgt_cache pgt_cache_list = SLIST_HEAD_INITIALIZER(pgt_cache_list);
+#endif
+
+static struct pgt pgt_entries[PGT_CACHE_SIZE];
+
+static struct mutex pgt_mu = MUTEX_INITIALIZER;
+static struct condvar pgt_cv = CONDVAR_INITIALIZER;
+
+#if defined(CFG_WITH_PAGER) && defined(CFG_WITH_LPAE)
+void pgt_init(void)
+{
+	size_t n;
+
+	for (n = 0; n < PGT_CACHE_SIZE; n++) {
+		struct pgt *p = pgt_entries + n;
+
+		p->tbl = tee_pager_alloc(PGT_SIZE, TEE_MATTR_LOCKED);
+		SLIST_INSERT_HEAD(&pgt_free_list, p, link);
+	}
+}
+#elif defined(CFG_WITH_PAGER) && !defined(CFG_WITH_LPAE)
+void pgt_init(void)
+{
+	size_t n;
+	size_t m;
+
+	COMPILE_TIME_ASSERT(PGT_CACHE_SIZE % PGT_NUM_PGT_PER_PAGE == 0);
+	COMPILE_TIME_ASSERT(PGT_SIZE * PGT_NUM_PGT_PER_PAGE == SMALL_PAGE_SIZE);
+
+	for (n = 0; n < ARRAY_SIZE(pgt_parents); n++) {
+		uint8_t *tbl = tee_pager_alloc(SMALL_PAGE_SIZE,
+					       TEE_MATTR_LOCKED);
+
+		SLIST_INIT(&pgt_parents[n].pgt_cache);
+		for (m = 0; m < PGT_NUM_PGT_PER_PAGE; m++) {
+			struct pgt *p = pgt_entries +
+					n * PGT_NUM_PGT_PER_PAGE + m;
+
+			p->tbl = tbl + m * PGT_SIZE;
+			p->parent = &pgt_parents[n];
+			SLIST_INSERT_HEAD(&pgt_parents[n].pgt_cache, p, link);
+		}
+	}
+}
+#else
+void pgt_init(void)
+{
+	/*
+	 * We're putting this in .nozi.* instead of .bss because .nozi.* already
+	 * has a large alignment, while .bss has a small alignment. The current
+	 * link script is optimized for small alignment in .bss
+	 */
+	static uint8_t pgt_tables[PGT_CACHE_SIZE][PGT_SIZE]
+			__aligned(PGT_SIZE) __section(".nozi.pgt_cache");
+	size_t n;
+
+	for (n = 0; n < ARRAY_SIZE(pgt_tables); n++) {
+		struct pgt *p = pgt_entries + n;
+
+		p->tbl = pgt_tables[n];
+		SLIST_INSERT_HEAD(&pgt_free_list, p, link);
+	}
+}
+#endif
+
+#if defined(CFG_WITH_LPAE) || !defined(CFG_WITH_PAGER)
+static struct pgt *pop_from_free_list(void)
+{
+	struct pgt *p = SLIST_FIRST(&pgt_free_list);
+
+	if (p) {
+		SLIST_REMOVE_HEAD(&pgt_free_list, link);
+		memset(p->tbl, 0, PGT_SIZE);
+	}
+	return p;
+}
+
+static void push_to_free_list(struct pgt *p)
+{
+	SLIST_INSERT_HEAD(&pgt_free_list, p, link);
+#if defined(CFG_WITH_PAGER)
+	tee_pager_release_phys(p->tbl, PGT_SIZE);
+#endif
+}
+#else
+static struct pgt *pop_from_free_list(void)
+{
+	size_t n;
+
+	for (n = 0; n < ARRAY_SIZE(pgt_parents); n++) {
+		struct pgt *p = SLIST_FIRST(&pgt_parents[n].pgt_cache);
+
+		if (p) {
+			SLIST_REMOVE_HEAD(&pgt_parents[n].pgt_cache, link);
+			pgt_parents[n].num_used++;
+			memset(p->tbl, 0, PGT_SIZE);
+			return p;
+		}
+	}
+	return NULL;
+}
+
+static void push_to_free_list(struct pgt *p)
+{
+	SLIST_INSERT_HEAD(&p->parent->pgt_cache, p, link);
+	assert(p->parent->num_used > 0);
+	p->parent->num_used--;
+	if (!p->parent->num_used) {
+		vaddr_t va = (vaddr_t)p->tbl & ~SMALL_PAGE_MASK;
+
+		tee_pager_release_phys((void *)va, SMALL_PAGE_SIZE);
+	}
+}
+#endif
+
+#ifdef CFG_PAGED_USER_TA
+static void push_to_cache_list(struct pgt *pgt)
+{
+	SLIST_INSERT_HEAD(&pgt_cache_list, pgt, link);
+}
+
+static bool match_pgt(struct pgt *pgt, vaddr_t vabase, void *ctx)
+{
+	return pgt->ctx == ctx && pgt->vabase == vabase;
+}
+
+static struct pgt *pop_from_cache_list(vaddr_t vabase, void *ctx)
+{
+	struct pgt *pgt;
+	struct pgt *p;
+
+	pgt = SLIST_FIRST(&pgt_cache_list);
+	if (!pgt)
+		return NULL;
+	if (match_pgt(pgt, vabase, ctx)) {
+		SLIST_REMOVE_HEAD(&pgt_cache_list, link);
+		return pgt;
+	}
+
+	while (true) {
+		p = SLIST_NEXT(pgt, link);
+		if (!p)
+			break;
+		if (match_pgt(p, vabase, ctx)) {
+			SLIST_REMOVE_AFTER(pgt, link);
+			break;
+		}
+		pgt = p;
+	}
+	return p;
+}
+
+static struct pgt *pop_least_used_from_cache_list(void)
+{
+	struct pgt *pgt;
+	struct pgt *p_prev = NULL;
+	size_t least_used;
+
+	pgt = SLIST_FIRST(&pgt_cache_list);
+	if (!pgt)
+		return NULL;
+	if (!pgt->num_used_entries)
+		goto out;
+	least_used = pgt->num_used_entries;
+
+	while (true) {
+		if (!SLIST_NEXT(pgt, link))
+			break;
+		if (SLIST_NEXT(pgt, link)->num_used_entries <= least_used) {
+			p_prev = pgt;
+			least_used = SLIST_NEXT(pgt, link)->num_used_entries;
+		}
+		pgt = SLIST_NEXT(pgt, link);
+	}
+
+out:
+	if (p_prev) {
+		pgt = SLIST_NEXT(p_prev, link);
+		SLIST_REMOVE_AFTER(p_prev, link);
+	} else {
+		pgt = SLIST_FIRST(&pgt_cache_list);
+		SLIST_REMOVE_HEAD(&pgt_cache_list, link);
+	}
+	return pgt;
+}
+
+static void pgt_free_unlocked(struct pgt_cache *pgt_cache, bool save_ctx)
+{
+	while (!SLIST_EMPTY(pgt_cache)) {
+		struct pgt *p = SLIST_FIRST(pgt_cache);
+
+		SLIST_REMOVE_HEAD(pgt_cache, link);
+		if (save_ctx && p->num_used_entries) {
+			push_to_cache_list(p);
+		} else {
+			tee_pager_pgt_save_and_release_entries(p);
+			assert(!p->num_used_entries);
+			p->ctx = NULL;
+			p->vabase = 0;
+
+			push_to_free_list(p);
+		}
+	}
+}
+
+static struct pgt *pop_from_some_list(vaddr_t vabase, void *ctx)
+{
+	struct pgt *p = pop_from_cache_list(vabase, ctx);
+
+	if (p)
+		return p;
+	p = pop_from_free_list();
+	if (!p) {
+		p = pop_least_used_from_cache_list();
+		if (!p)
+			return NULL;
+		tee_pager_pgt_save_and_release_entries(p);
+	}
+	assert(!p->num_used_entries);
+	p->ctx = ctx;
+	p->vabase = vabase;
+	return p;
+}
+
+void pgt_flush_ctx(struct tee_ta_ctx *ctx)
+{
+	struct pgt *p;
+	struct pgt *pp = NULL;
+
+	mutex_lock(&pgt_mu);
+
+	while (true) {
+		p = SLIST_FIRST(&pgt_cache_list);
+		if (!p)
+			goto out;
+		if (p->ctx != ctx)
+			break;
+		SLIST_REMOVE_HEAD(&pgt_cache_list, link);
+		tee_pager_pgt_save_and_release_entries(p);
+		assert(!p->num_used_entries);
+		p->ctx = NULL;
+		p->vabase = 0;
+		push_to_free_list(p);
+	}
+
+	pp = p;
+	while (true) {
+		p = SLIST_NEXT(pp, link);
+		if (!p)
+			break;
+		if (p->ctx == ctx) {
+			SLIST_REMOVE_AFTER(pp, link);
+			tee_pager_pgt_save_and_release_entries(p);
+			assert(!p->num_used_entries);
+			p->ctx = NULL;
+			p->vabase = 0;
+			push_to_free_list(p);
+		} else {
+			pp = p;
+		}
+	}
+
+out:
+	mutex_unlock(&pgt_mu);
+}
+
+static void flush_pgt_entry(struct pgt *p)
+{
+	tee_pager_pgt_save_and_release_entries(p);
+	assert(!p->num_used_entries);
+	p->ctx = NULL;
+	p->vabase = 0;
+}
+
+static bool pgt_entry_matches(struct pgt *p, void *ctx, vaddr_t begin,
+			      vaddr_t last)
+{
+	if (!p)
+		return false;
+	if (p->ctx != ctx)
+		return false;
+	if (last <= begin)
+		return false;
+	if (!core_is_buffer_inside(p->vabase, SMALL_PAGE_SIZE, begin,
+				   last - begin))
+		return false;
+
+	return true;
+}
+
+static void flush_ctx_range_from_list(struct pgt_cache *pgt_cache, void *ctx,
+				      vaddr_t begin, vaddr_t last)
+{
+	struct pgt *p;
+	struct pgt *next_p;
+
+	/*
+	 * Do the special case where the first element in the list is
+	 * removed first.
+	 */
+	p = SLIST_FIRST(pgt_cache);
+	while (pgt_entry_matches(p, ctx, begin, last)) {
+		flush_pgt_entry(p);
+		SLIST_REMOVE_HEAD(pgt_cache, link);
+		push_to_free_list(p);
+		p = SLIST_FIRST(pgt_cache);
+	}
+
+	/*
+	 * p either points to the first element in the list or it's NULL,
+	 * if NULL the list is empty and we're done.
+	 */
+	if (!p)
+		return;
+
+	/*
+	 * Do the common case where the next element in the list is
+	 * removed.
+	 */
+	while (true) {
+		next_p = SLIST_NEXT(p, link);
+		if (!next_p)
+			break;
+		if (pgt_entry_matches(next_p, ctx, begin, last)) {
+			flush_pgt_entry(next_p);
+			SLIST_REMOVE_AFTER(p, link);
+			push_to_free_list(next_p);
+			continue;
+		}
+
+		p = SLIST_NEXT(p, link);
+	}
+}
+
+void pgt_flush_ctx_range(struct pgt_cache *pgt_cache, void *ctx,
+			 vaddr_t begin, vaddr_t last)
+{
+	mutex_lock(&pgt_mu);
+
+	flush_ctx_range_from_list(pgt_cache, ctx, begin, last);
+	flush_ctx_range_from_list(&pgt_cache_list, ctx, begin, last);
+
+	condvar_broadcast(&pgt_cv);
+	mutex_unlock(&pgt_mu);
+}
+
+static void transfer_tables(struct pgt_cache *pgt_cache, void *old_ctx,
+			    vaddr_t old_va, void *new_ctx, vaddr_t new_va,
+			    size_t size)
+{
+	const size_t pgtsize = CORE_MMU_PGDIR_SIZE;
+	const vaddr_t new_base = ROUNDDOWN(new_va, pgtsize);
+	const vaddr_t old_base = ROUNDDOWN(old_va, pgtsize);
+	const size_t num_new_pgt = (size - 1 + new_va - new_base) / pgtsize + 1;
+	const size_t num_old_pgt = (size - 1 + old_va - old_base) / pgtsize + 1;
+	struct pgt *new_pgt[num_new_pgt];
+	struct pgt *old_pgt[num_old_pgt];
+	struct pgt *pgt;
+	size_t n;
+
+	/*
+	 * Fill in new_pgt based on pgt_cache. Note that the pages should
+	 * already have been allocated.
+	 */
+	SLIST_FOREACH(pgt, pgt_cache, link) {
+		if (pgt->vabase < new_base)
+			continue;
+		n = (pgt->vabase - new_base) / pgtsize;
+		if (n < num_new_pgt)
+			new_pgt[n] = pgt;
+	}
+	for (n = 0; n < num_new_pgt; n++) {
+		assert(new_pgt[n]);
+		assert(new_pgt[n]->ctx == new_ctx);
+	}
+
+	mutex_lock(&pgt_mu);
+
+	/* Extract the array of pgts that need their content transferred */
+	for (n = 0; n < num_old_pgt; n++) {
+		/*
+		 * If the pgt isn't in the cache list there's nothing to
+		 * transfer, so NULL here is OK.
+		 */
+		old_pgt[n] = pop_from_cache_list(old_base + n * pgtsize,
+						 old_ctx);
+	}
+
+	tee_pager_transfer_uta_region(to_user_ta_ctx(old_ctx), old_va,
+				      to_user_ta_ctx(new_ctx), new_va, new_pgt,
+				      size);
+
+	for (n = 0; n < num_old_pgt; n++) {
+		if (!old_pgt[n])
+			continue;
+
+		if (old_pgt[n]->num_used_entries)
+			push_to_cache_list(old_pgt[n]);
+		else
+			push_to_free_list(old_pgt[n]);
+	}
+
+	mutex_unlock(&pgt_mu);
+}
+
+void pgt_transfer(struct pgt_cache *pgt_cache, void *old_ctx, vaddr_t old_va,
+		  void *new_ctx, vaddr_t new_va, size_t size)
+{
+	if (size)
+		transfer_tables(pgt_cache, old_ctx, old_va, new_ctx,
+				new_va, size);
+}
+
+#else /*!CFG_PAGED_USER_TA*/
+
+static void pgt_free_unlocked(struct pgt_cache *pgt_cache,
+			      bool save_ctx __unused)
+{
+	while (!SLIST_EMPTY(pgt_cache)) {
+		struct pgt *p = SLIST_FIRST(pgt_cache);
+
+		SLIST_REMOVE_HEAD(pgt_cache, link);
+		push_to_free_list(p);
+	}
+}
+
+static struct pgt *pop_from_some_list(vaddr_t vabase __unused,
+				      void *ctx __unused)
+{
+	return pop_from_free_list();
+}
+#endif /*!CFG_PAGED_USER_TA*/
+
+static bool pgt_alloc_unlocked(struct pgt_cache *pgt_cache, void *ctx,
+			       vaddr_t begin, vaddr_t last)
+{
+	const vaddr_t base = ROUNDDOWN(begin, CORE_MMU_PGDIR_SIZE);
+	const size_t num_tbls = ((last - base) >> CORE_MMU_PGDIR_SHIFT) + 1;
+	size_t n = 0;
+	struct pgt *p;
+	struct pgt *pp = NULL;
+
+	while (n < num_tbls) {
+		p = pop_from_some_list(base + n * CORE_MMU_PGDIR_SIZE, ctx);
+		if (!p) {
+			pgt_free_unlocked(pgt_cache, ctx);
+			return false;
+		}
+
+		if (pp)
+			SLIST_INSERT_AFTER(pp, p, link);
+		else
+			SLIST_INSERT_HEAD(pgt_cache, p, link);
+		pp = p;
+		n++;
+	}
+
+	return true;
+}
+
+void pgt_alloc(struct pgt_cache *pgt_cache, void *ctx,
+	       vaddr_t begin, vaddr_t last)
+{
+	if (last <= begin)
+		return;
+
+	mutex_lock(&pgt_mu);
+
+	pgt_free_unlocked(pgt_cache, ctx);
+	while (!pgt_alloc_unlocked(pgt_cache, ctx, begin, last)) {
+		DMSG("Waiting for page tables");
+		condvar_broadcast(&pgt_cv);
+		condvar_wait(&pgt_cv, &pgt_mu);
+	}
+
+	mutex_unlock(&pgt_mu);
+}
+
+void pgt_free(struct pgt_cache *pgt_cache, bool save_ctx)
+{
+	if (SLIST_EMPTY(pgt_cache))
+		return;
+
+	mutex_lock(&pgt_mu);
+
+	pgt_free_unlocked(pgt_cache, save_ctx);
+
+	condvar_broadcast(&pgt_cv);
+	mutex_unlock(&pgt_mu);
+}
diff --git a/core/arch/arm/mm/sub.mk b/core/arch/arm/mm/sub.mk
new file mode 100644
index 0000000..71f70f3
--- /dev/null
+++ b/core/arch/arm/mm/sub.mk
@@ -0,0 +1,12 @@
+srcs-y += core_mmu.c
+srcs-$(CFG_WITH_PAGER) += tee_pager.c
+srcs-$(CFG_WITH_PAGER) += pager_aes_gcm.c
+srcs-y += tee_mmu.c
+ifeq ($(CFG_WITH_LPAE),y)
+srcs-y += core_mmu_lpae.c
+else
+srcs-y += core_mmu_v7.c
+endif
+srcs-y += tee_mm.c
+srcs-$(CFG_SMALL_PAGE_USER_TA) += pgt_cache.c
+srcs-y += mobj.c
diff --git a/core/arch/arm/mm/tee_mm.c b/core/arch/arm/mm/tee_mm.c
new file mode 100644
index 0000000..22a39df
--- /dev/null
+++ b/core/arch/arm/mm/tee_mm.c
@@ -0,0 +1,354 @@
+/*
+ * Copyright (c) 2014, STMicroelectronics International N.V.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <kernel/panic.h>
+#include <kernel/tee_common.h>
+#include <util.h>
+#include <trace.h>
+
+#include <mm/tee_mm.h>
+#include <mm/tee_mm.h>
+#include <mm/tee_pager.h>
+
+bool tee_mm_init(tee_mm_pool_t *pool, paddr_t lo, paddr_t hi, uint8_t shift,
+		 uint32_t flags)
+{
+	if (pool == NULL)
+		return false;
+
+	lo = ROUNDUP(lo, 1 << shift);
+	hi = ROUNDDOWN(hi, 1 << shift);
+
+	assert(((uint64_t)(hi - lo) >> shift) < (uint64_t)UINT32_MAX);
+
+	pool->lo = lo;
+	pool->hi = hi;
+	pool->shift = shift;
+	pool->flags = flags;
+	pool->entry = calloc(1, sizeof(tee_mm_entry_t));
+
+	if (pool->entry == NULL)
+		return false;
+
+	if (pool->flags & TEE_MM_POOL_HI_ALLOC)
+		pool->entry->offset = ((hi - lo - 1) >> shift) + 1;
+	pool->entry->pool = pool;
+
+	return true;
+}
+
+void tee_mm_final(tee_mm_pool_t *pool)
+{
+	if (pool == NULL || pool->entry == NULL)
+		return;
+
+	while (pool->entry->next != NULL)
+		tee_mm_free(pool->entry->next);
+	free(pool->entry);
+	pool->entry = NULL;
+}
+
+static tee_mm_entry_t *tee_mm_add(tee_mm_entry_t *p)
+{
+	/* add to list */
+	if (p->next == NULL) {
+		p->next = malloc(sizeof(tee_mm_entry_t));
+		if (p->next == NULL)
+			return NULL;
+		p->next->next = NULL;
+	} else {
+		tee_mm_entry_t *nn = malloc(sizeof(tee_mm_entry_t));
+
+		if (nn == NULL)
+			return NULL;
+		nn->next = p->next;
+		p->next = nn;
+	}
+	return p->next;
+}
+
+#ifdef CFG_WITH_STATS
+static size_t tee_mm_stats_allocated(tee_mm_pool_t *pool)
+{
+	tee_mm_entry_t *entry;
+	uint32_t sz = 0;
+
+	if (!pool)
+		return 0;
+
+	entry = pool->entry;
+	while (entry) {
+		sz += entry->size;
+		entry = entry->next;
+	}
+
+	return sz << pool->shift;
+}
+
+void tee_mm_get_pool_stats(tee_mm_pool_t *pool, struct malloc_stats *stats,
+			   bool reset)
+{
+	memset(stats, 0, sizeof(*stats));
+
+	stats->size = pool->hi - pool->lo;
+	stats->max_allocated = pool->max_allocated;
+	stats->allocated = tee_mm_stats_allocated(pool);
+
+	if (reset)
+		pool->max_allocated = 0;
+}
+
+static void update_max_allocated(tee_mm_pool_t *pool)
+{
+	size_t sz = tee_mm_stats_allocated(pool);
+
+	if (sz > pool->max_allocated)
+		pool->max_allocated = sz;
+}
+#else /* CFG_WITH_STATS */
+static inline void update_max_allocated(tee_mm_pool_t *pool __unused)
+{
+}
+#endif /* CFG_WITH_STATS */
+
+tee_mm_entry_t *tee_mm_alloc(tee_mm_pool_t *pool, size_t size)
+{
+	size_t psize;
+	tee_mm_entry_t *entry;
+	tee_mm_entry_t *nn;
+	size_t remaining;
+
+	/* Check that pool is initialized */
+	if (!pool || !pool->entry)
+		return NULL;
+
+	entry = pool->entry;
+	if (size == 0)
+		psize = 0;
+	else
+		psize = ((size - 1) >> pool->shift) + 1;
+	/* Protect with mutex (multi thread) */
+
+	/* find free slot */
+	if (pool->flags & TEE_MM_POOL_HI_ALLOC) {
+		while (entry->next != NULL && psize >
+		       (entry->offset - entry->next->offset -
+			entry->next->size))
+			entry = entry->next;
+	} else {
+		while (entry->next != NULL && psize >
+		       (entry->next->offset - entry->size - entry->offset))
+			entry = entry->next;
+	}
+
+	/* check if we have enough memory */
+	if (entry->next == NULL) {
+		if (pool->flags & TEE_MM_POOL_HI_ALLOC) {
+			/*
+			 * entry->offset is a "block count" offset from
+			 * pool->lo. The byte offset is
+			 * (entry->offset << pool->shift).
+			 * In the HI_ALLOC allocation scheme the memory is
+			 * allocated from the end of the segment, thus to
+			 * validate there is sufficient memory validate that
+			 * (entry->offset << pool->shift) > size.
+			 */
+			if ((entry->offset << pool->shift) < size)
+				/* out of memory */
+				return NULL;
+		} else {
+			if (pool->hi <= pool->lo)
+				panic("invalid pool");
+
+			remaining = (pool->hi - pool->lo);
+			remaining -= ((entry->offset + entry->size) <<
+				      pool->shift);
+
+			if (remaining < size)
+				/* out of memory */
+				return NULL;
+		}
+	}
+
+	nn = tee_mm_add(entry);
+	if (nn == NULL)
+		return NULL;
+
+	if (pool->flags & TEE_MM_POOL_HI_ALLOC)
+		nn->offset = entry->offset - psize;
+	else
+		nn->offset = entry->offset + entry->size;
+	nn->size = psize;
+	nn->pool = pool;
+
+	update_max_allocated(pool);
+
+	/* Protect with mutex end (multi thread) */
+
+	return nn;
+}
+
+static inline bool fit_in_gap(tee_mm_pool_t *pool, tee_mm_entry_t *e,
+			      paddr_t offslo, paddr_t offshi)
+{
+	if (pool->flags & TEE_MM_POOL_HI_ALLOC) {
+		if (offshi > e->offset ||
+		    (e->next != NULL &&
+		     (offslo < e->next->offset + e->next->size)) ||
+		    (offshi << pool->shift) - 1 > (pool->hi - pool->lo))
+			/* memory not available */
+			return false;
+	} else {
+		if (offslo < (e->offset + e->size) ||
+		    (e->next != NULL && (offshi > e->next->offset)) ||
+		    (offshi << pool->shift) > (pool->hi - pool->lo))
+			/* memory not available */
+			return false;
+	}
+
+	return true;
+}
+
+tee_mm_entry_t *tee_mm_alloc2(tee_mm_pool_t *pool, paddr_t base, size_t size)
+{
+	tee_mm_entry_t *entry;
+	paddr_t offslo;
+	paddr_t offshi;
+	tee_mm_entry_t *mm;
+
+	/* Check that pool is initialized */
+	if (!pool || !pool->entry)
+		return NULL;
+
+	/* Wrapping and sanity check */
+	if ((base + size) < base || base < pool->lo)
+		return NULL;
+
+	entry = pool->entry;
+	offslo = (base - pool->lo) >> pool->shift;
+	offshi = ((base - pool->lo + size - 1) >> pool->shift) + 1;
+
+	/* find slot */
+	if (pool->flags & TEE_MM_POOL_HI_ALLOC) {
+		while (entry->next != NULL &&
+		       offshi < entry->next->offset + entry->next->size)
+			entry = entry->next;
+	} else {
+		while (entry->next != NULL && offslo > entry->next->offset)
+			entry = entry->next;
+	}
+
+	/* Check that memory is available */
+	if (!fit_in_gap(pool, entry, offslo, offshi))
+		return NULL;
+
+	mm = tee_mm_add(entry);
+	if (mm == NULL)
+		return NULL;
+
+	mm->offset = offslo;
+	mm->size = offshi - offslo;
+	mm->pool = pool;
+
+	update_max_allocated(pool);
+
+	return mm;
+}
+
+void tee_mm_free(tee_mm_entry_t *p)
+{
+	tee_mm_entry_t *entry;
+
+	if (!p || !p->pool)
+		return;
+
+	entry = p->pool->entry;
+
+	/* Protect with mutex (multi thread) */
+
+	/* remove entry from list */
+	while (entry->next != NULL && entry->next != p)
+		entry = entry->next;
+
+	if (!entry->next)
+		panic("invalid mm_entry");
+
+	entry->next = entry->next->next;
+
+	free(p);
+
+	/* Protect with mutex end (multi thread) */
+}
+
+size_t tee_mm_get_bytes(const tee_mm_entry_t *mm)
+{
+	if (!mm || !mm->pool)
+		return 0;
+	else
+		return mm->size << mm->pool->shift;
+}
+
+bool tee_mm_addr_is_within_range(tee_mm_pool_t *pool, paddr_t addr)
+{
+	return (pool && ((addr >= pool->lo) && (addr <= pool->hi)));
+}
+
+bool tee_mm_is_empty(tee_mm_pool_t *pool)
+{
+	return pool == NULL || pool->entry == NULL || pool->entry->next == NULL;
+}
+
+/* Physical Secure DDR pool */
+tee_mm_pool_t tee_mm_sec_ddr __early_bss;
+
+/* Virtual eSRAM pool */
+tee_mm_pool_t tee_mm_vcore __early_bss;
+
+tee_mm_entry_t *tee_mm_find(const tee_mm_pool_t *pool, paddr_t addr)
+{
+	tee_mm_entry_t *entry = pool->entry;
+	uint16_t offset = (addr - pool->lo) >> pool->shift;
+
+	if (addr > pool->hi || addr < pool->lo)
+		return NULL;
+
+	while (entry->next != NULL) {
+		entry = entry->next;
+
+		if ((offset >= entry->offset) &&
+		    (offset < (entry->offset + entry->size))) {
+			return entry;
+		}
+	}
+
+	return NULL;
+}
+
+uintptr_t tee_mm_get_smem(const tee_mm_entry_t *mm)
+{
+	return (mm->offset << mm->pool->shift) + mm->pool->lo;
+}
diff --git a/core/arch/arm/mm/tee_mmu.c b/core/arch/arm/mm/tee_mmu.c
new file mode 100644
index 0000000..f5c6dde
--- /dev/null
+++ b/core/arch/arm/mm/tee_mmu.c
@@ -0,0 +1,896 @@
+/*
+ * Copyright (c) 2016, Linaro Limited
+ * 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 <arm.h>
+#include <assert.h>
+#include <kernel/panic.h>
+#include <kernel/tee_common.h>
+#include <kernel/tee_misc.h>
+#include <kernel/tz_ssvce.h>
+#include <mm/tee_mmu.h>
+#include <mm/tee_mmu_types.h>
+#include <mm/pgt_cache.h>
+#include <mm/tee_mm.h>
+#include <mm/core_memprot.h>
+#include <mm/core_mmu.h>
+#include <mm/mobj.h>
+#include <mm/tee_pager.h>
+#include <sm/optee_smc.h>
+#include <stdlib.h>
+#include <tee_api_defines_extensions.h>
+#include <tee_api_types.h>
+#include <trace.h>
+#include <types_ext.h>
+#include <user_ta_header.h>
+#include <util.h>
+
+#ifdef CFG_PL310
+#include <kernel/tee_l2cc_mutex.h>
+#endif
+
+#define TEE_MMU_UDATA_ATTR		(TEE_MATTR_VALID_BLOCK | \
+					 TEE_MATTR_PRW | TEE_MATTR_URW | \
+					 TEE_MATTR_SECURE)
+#define TEE_MMU_UCODE_ATTR		(TEE_MATTR_VALID_BLOCK | \
+					 TEE_MATTR_PRW | TEE_MATTR_URWX | \
+					 TEE_MATTR_SECURE)
+
+#define TEE_MMU_UCACHE_DEFAULT_ATTR	(TEE_MATTR_CACHE_CACHED << \
+					 TEE_MATTR_CACHE_SHIFT)
+
+/* Support for 31 concurrent sessions */
+static uint32_t g_asid = 0xffffffff;
+
+static TEE_Result tee_mmu_umap_add_param(struct tee_mmu_info *mmu,
+					 struct param_mem *mem)
+{
+	TEE_Result res;
+	struct tee_ta_region *last_entry = NULL;
+	size_t n;
+	uint32_t attr = TEE_MMU_UDATA_ATTR;
+	size_t nsz;
+	size_t noffs;
+
+	if (!mobj_is_paged(mem->mobj)) {
+		uint32_t cattr;
+
+		res = mobj_get_cattr(mem->mobj, &cattr);
+		if (res != TEE_SUCCESS)
+			return res;
+		attr |= cattr << TEE_MATTR_CACHE_SHIFT;
+	}
+
+	if (!mobj_is_secure(mem->mobj))
+		attr &= ~TEE_MATTR_SECURE;
+
+	/* Check that we can map memory using this attribute */
+	if (!core_mmu_mattr_is_ok(attr))
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	/* Find empty entry */
+	for (n = TEE_MMU_UMAP_PARAM_IDX; n < TEE_MMU_UMAP_MAX_ENTRIES; n++)
+		if (!mmu->regions[n].size)
+			break;
+
+	if (n == TEE_MMU_UMAP_MAX_ENTRIES) {
+		/* No entries left "can't happen" */
+		return TEE_ERROR_EXCESS_DATA;
+	}
+
+	mmu->regions[n].mobj = mem->mobj;
+	mmu->regions[n].offset = ROUNDDOWN(mem->offs, CORE_MMU_USER_PARAM_SIZE);
+	mmu->regions[n].size = ROUNDUP(mem->offs - mmu->regions[n].offset +
+				       mem->size,
+				       CORE_MMU_USER_PARAM_SIZE);
+	mmu->regions[n].attr = attr;
+
+	/* Try to coalesce some entries */
+	while (true) {
+		/* Find last param */
+		n = TEE_MMU_UMAP_MAX_ENTRIES - 1;
+
+		while (!mmu->regions[n].size) {
+			n--;
+			if (n < TEE_MMU_UMAP_PARAM_IDX) {
+				/* No param entries found, "can't happen" */
+				return TEE_ERROR_BAD_STATE;
+			}
+		}
+
+		if (last_entry == mmu->regions + n)
+			return TEE_SUCCESS; /* Can't coalesc more */
+		last_entry = mmu->regions + n;
+
+		n--;
+		while (n >= TEE_MMU_UMAP_PARAM_IDX) {
+			struct tee_ta_region *entry = mmu->regions + n;
+
+			n--;
+			if (last_entry->mobj != entry->mobj)
+				continue;
+
+			if ((last_entry->offset + last_entry->size) ==
+			    entry->offset ||
+			    (entry->offset + entry->size) ==
+			    last_entry->offset ||
+			    core_is_buffer_intersect(last_entry->offset,
+						     last_entry->size,
+						     entry->offset,
+						     entry->size)) {
+				noffs = MIN(last_entry->offset, entry->offset);
+				nsz = MAX(last_entry->offset + last_entry->size,
+					  entry->offset + entry->size) - noffs;
+				entry->offset = noffs;
+				entry->size = nsz;
+				last_entry->mobj = NULL;
+				last_entry->size = 0;
+				last_entry->attr = 0;
+				break;
+			}
+		}
+	}
+}
+
+static TEE_Result tee_mmu_umap_set_vas(struct tee_mmu_info *mmu)
+{
+	const size_t granule = CORE_MMU_USER_PARAM_SIZE;
+	vaddr_t va_range_base;
+	vaddr_t va;
+	size_t va_range_size;
+	size_t n;
+
+	/* Find last table entry used to map code and data */
+	n = TEE_MMU_UMAP_PARAM_IDX - 1;
+	while (n && !mmu->regions[n].size)
+		n--;
+	va = mmu->regions[n].va + mmu->regions[n].size;
+	assert(va);
+
+	core_mmu_get_user_va_range(&va_range_base, &va_range_size);
+	assert(va_range_base == mmu->ta_private_vmem_start);
+
+	/*
+	 * Assign parameters in secure memory.
+	 */
+	va = ROUNDUP(va, granule);
+	for (n = TEE_MMU_UMAP_PARAM_IDX; n < TEE_MMU_UMAP_MAX_ENTRIES; n++) {
+		if (!mmu->regions[n].size ||
+		    !(mmu->regions[n].attr & TEE_MATTR_SECURE))
+			continue;
+		mmu->regions[n].va = va;
+		va += mmu->regions[n].size;
+		/* Put some empty space between each area */
+		va += granule;
+		if ((va - va_range_base) >= va_range_size)
+			return TEE_ERROR_EXCESS_DATA;
+	}
+
+	/*
+	 * Assign parameters in nonsecure shared memory.
+	 * Note that we're making sure that they will reside in a new page
+	 * directory as they are to be mapped nonsecure.
+	 */
+	va = ROUNDUP(va, CORE_MMU_PGDIR_SIZE);
+	for (n = TEE_MMU_UMAP_PARAM_IDX; n < TEE_MMU_UMAP_MAX_ENTRIES; n++) {
+		if (!mmu->regions[n].size ||
+		    (mmu->regions[n].attr & TEE_MATTR_SECURE))
+			continue;
+		mmu->regions[n].va = va;
+		va += mmu->regions[n].size;
+		/* Put some empty space between each area */
+		va += granule;
+		if ((va - va_range_base) >= va_range_size)
+			return TEE_ERROR_EXCESS_DATA;
+	}
+
+	return TEE_SUCCESS;
+}
+
+TEE_Result tee_mmu_init(struct user_ta_ctx *utc)
+{
+	uint32_t asid = 1;
+
+	if (!utc->context) {
+		utc->context = 1;
+
+		/* Find available ASID */
+		while (!(asid & g_asid) && (asid != 0)) {
+			utc->context++;
+			asid = asid << 1;
+		}
+
+		if (asid == 0) {
+			DMSG("Failed to allocate ASID");
+			return TEE_ERROR_GENERIC;
+		}
+		g_asid &= ~asid;
+	}
+
+	utc->mmu = calloc(1, sizeof(struct tee_mmu_info));
+	if (!utc->mmu)
+		return TEE_ERROR_OUT_OF_MEMORY;
+	core_mmu_get_user_va_range(&utc->mmu->ta_private_vmem_start, NULL);
+	return TEE_SUCCESS;
+}
+
+#ifdef CFG_SMALL_PAGE_USER_TA
+static TEE_Result alloc_pgt(struct user_ta_ctx *utc __maybe_unused,
+			    vaddr_t base, vaddr_t end)
+{
+	struct thread_specific_data *tsd __maybe_unused;
+	vaddr_t b = ROUNDDOWN(base, CORE_MMU_PGDIR_SIZE);
+	vaddr_t e = ROUNDUP(end, CORE_MMU_PGDIR_SIZE);
+	size_t ntbl = (e - b) >> CORE_MMU_PGDIR_SHIFT;
+
+	if (!pgt_check_avail(ntbl)) {
+		EMSG("%zu page tables not available", ntbl);
+		return TEE_ERROR_OUT_OF_MEMORY;
+	}
+
+#ifdef CFG_PAGED_USER_TA
+	tsd = thread_get_tsd();
+	if (&utc->ctx == tsd->ctx) {
+		/*
+		 * The supplied utc is the current active utc, allocate the
+		 * page tables too as the pager needs to use them soon.
+		 */
+		pgt_alloc(&tsd->pgt_cache, &utc->ctx, b, e - 1);
+	}
+#endif
+
+	return TEE_SUCCESS;
+}
+
+static void free_pgt(struct user_ta_ctx *utc, vaddr_t base, size_t size)
+{
+	struct thread_specific_data *tsd = thread_get_tsd();
+	struct pgt_cache *pgt_cache = NULL;
+
+	if (&utc->ctx == tsd->ctx)
+		pgt_cache = &tsd->pgt_cache;
+
+	pgt_flush_ctx_range(pgt_cache, &utc->ctx, base, base + size);
+}
+
+#else
+static TEE_Result alloc_pgt(struct user_ta_ctx *utc __unused,
+			    vaddr_t base __unused, vaddr_t end __unused)
+{
+	return TEE_SUCCESS;
+}
+
+static void free_pgt(struct user_ta_ctx *utc __unused, vaddr_t base __unused,
+		     size_t size __unused)
+{
+}
+#endif
+
+void tee_mmu_map_stack(struct user_ta_ctx *utc, struct mobj *mobj)
+{
+	const size_t granule = CORE_MMU_USER_CODE_SIZE;
+	struct tee_ta_region *region = utc->mmu->regions +
+				       TEE_MMU_UMAP_STACK_IDX;
+
+	region->mobj = mobj;
+	region->offset = 0;
+	region->va = utc->mmu->ta_private_vmem_start;
+	region->size = ROUNDUP(utc->mobj_stack->size, granule);
+	region->attr = TEE_MATTR_VALID_BLOCK | TEE_MATTR_SECURE |
+		       TEE_MATTR_URW | TEE_MATTR_PRW |
+		       (TEE_MATTR_CACHE_CACHED << TEE_MATTR_CACHE_SHIFT);
+}
+
+TEE_Result tee_mmu_map_add_segment(struct user_ta_ctx *utc, struct mobj *mobj,
+				   size_t offs, size_t size, uint32_t prot)
+{
+	const uint32_t attr = TEE_MATTR_VALID_BLOCK | TEE_MATTR_SECURE |
+			      (TEE_MATTR_CACHE_CACHED << TEE_MATTR_CACHE_SHIFT);
+	const size_t granule = CORE_MMU_USER_CODE_SIZE;
+	struct tee_ta_region *tbl = utc->mmu->regions;
+	vaddr_t va;
+	vaddr_t end_va;
+	size_t n = TEE_MMU_UMAP_CODE_IDX;
+	size_t o;
+
+	if (!tbl[n].size) {
+		/* We're continuing the va space from previous entry. */
+		assert(tbl[n - 1].size);
+
+		/* This is the first segment */
+		va = tbl[n - 1].va + tbl[n - 1].size;
+		end_va = ROUNDUP((offs & (granule - 1)) + size, granule) + va;
+		o = ROUNDDOWN(offs, granule);
+		goto set_entry;
+	}
+
+	/*
+	 * mobj of code segments must not change once the first is
+	 * assigned.
+	 */
+	if (mobj != tbl[n].mobj)
+		return TEE_ERROR_SECURITY;
+
+	/*
+	 * Let's find an entry we overlap with or if we need to add a new
+	 * entry.
+	 */
+	o = offs - tbl[n].offset;
+	va = ROUNDDOWN(o, granule) + tbl[n].va;
+	end_va = ROUNDUP(o + size, granule) + tbl[n].va;
+	o = ROUNDDOWN(offs, granule);
+	while (true) {
+		if (va >= (tbl[n].va + tbl[n].size)) {
+			n++;
+			if (n >= TEE_MMU_UMAP_PARAM_IDX)
+				return TEE_ERROR_SECURITY;
+			if (!tbl[n].size)
+				goto set_entry;
+			continue;
+		}
+
+		/*
+		 * There's at least partial overlap with this entry
+		 *
+		 * Since we're overlapping there should be at least one
+		 * free entry after this.
+		 */
+		if (((n + 1) >= TEE_MMU_UMAP_PARAM_IDX) || tbl[n + 1].size)
+			return TEE_ERROR_SECURITY;
+
+		/* offset must match or the segments aren't added in order */
+		if (o != (va - tbl[n].va + tbl[n].offset))
+			return TEE_ERROR_SECURITY;
+		/* We should only overlap in the last granule of the entry. */
+		if ((va + granule) < (tbl[n].va + tbl[n].size))
+			return TEE_ERROR_SECURITY;
+
+		/* Merge protection attribute for this entry */
+		tbl[n].attr |= prot;
+
+		va += granule;
+		/* If the segment was completely overlapped, we're done. */
+		if (va == end_va)
+			return TEE_SUCCESS;
+		o += granule;
+		n++;
+		goto set_entry;
+	}
+
+set_entry:
+	tbl[n].mobj = mobj;
+	tbl[n].va = va;
+	tbl[n].offset = o;
+	tbl[n].size = end_va - va;
+	tbl[n].attr = prot | attr;
+
+	utc->mmu->ta_private_vmem_end = tbl[n].va + tbl[n].size;
+	/*
+	 * Check that we have enough translation tables available to map
+	 * this TA.
+	 */
+	return alloc_pgt(utc, utc->mmu->ta_private_vmem_start,
+			 utc->mmu->ta_private_vmem_end);
+}
+
+void tee_mmu_map_clear(struct user_ta_ctx *utc)
+{
+	utc->mmu->ta_private_vmem_end = 0;
+	memset(utc->mmu->regions, 0, sizeof(utc->mmu->regions));
+}
+
+static void clear_param_map(struct user_ta_ctx *utc)
+{
+	const size_t n = TEE_MMU_UMAP_PARAM_IDX;
+	const size_t array_size = ARRAY_SIZE(utc->mmu->regions);
+
+	memset(utc->mmu->regions + n, 0,
+	       (array_size - n) * sizeof(utc->mmu->regions[0]));
+}
+
+static TEE_Result param_mem_to_user_va(struct user_ta_ctx *utc,
+				       struct param_mem *mem, void **user_va)
+{
+	size_t n;
+
+	for (n = TEE_MMU_UMAP_PARAM_IDX; n < TEE_MMU_UMAP_MAX_ENTRIES; n++) {
+		struct tee_ta_region *region = utc->mmu->regions + n;
+		vaddr_t va;
+
+		if (mem->mobj != region->mobj)
+			continue;
+		if (mem->offs < region->offset)
+			continue;
+		if (mem->offs >= (region->offset + region->size))
+			continue;
+		va = region->va + mem->offs - region->offset;
+		*user_va = (void *)va;
+		return TEE_SUCCESS;
+	}
+	return TEE_ERROR_GENERIC;
+}
+
+TEE_Result tee_mmu_map_param(struct user_ta_ctx *utc,
+		struct tee_ta_param *param, void *param_va[TEE_NUM_PARAMS])
+{
+	TEE_Result res = TEE_SUCCESS;
+	size_t n;
+
+	/* Clear all the param entries as they can hold old information */
+	clear_param_map(utc);
+
+	/* Map secure memory params first then nonsecure memory params */
+	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_nonsec(mem->mobj))
+			continue;
+
+		res = tee_mmu_umap_add_param(utc->mmu, mem);
+		if (res != TEE_SUCCESS)
+			return res;
+	}
+	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_nonsec(mem->mobj))
+			continue;
+
+		res = tee_mmu_umap_add_param(utc->mmu, mem);
+		if (res != TEE_SUCCESS)
+			return res;
+	}
+
+	res = tee_mmu_umap_set_vas(utc->mmu);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	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 == 0)
+			continue;
+
+		res = param_mem_to_user_va(utc, mem, param_va + n);
+		if (res != TEE_SUCCESS)
+			return res;
+	}
+
+	utc->mmu->ta_private_vmem_start = utc->mmu->regions[0].va;
+
+	n = ARRAY_SIZE(utc->mmu->regions);
+	do {
+		n--;
+	} while (n && !utc->mmu->regions[n].size);
+
+	return alloc_pgt(utc, utc->mmu->ta_private_vmem_start,
+			 utc->mmu->regions[n].va + utc->mmu->regions[n].size);
+}
+
+TEE_Result tee_mmu_add_rwmem(struct user_ta_ctx *utc, struct mobj *mobj,
+			     int pgdir_offset, vaddr_t *va)
+{
+	struct tee_ta_region *reg = NULL;
+	struct tee_ta_region *last_reg;
+	vaddr_t v;
+	vaddr_t end_v;
+	size_t n;
+
+	assert(pgdir_offset < CORE_MMU_PGDIR_SIZE);
+
+	/*
+	 * Avoid the corner case when no regions are assigned, currently
+	 * stack and code areas are always assigned before we end up here.
+	 */
+	if (!utc->mmu->regions[0].size)
+		return TEE_ERROR_GENERIC;
+
+	for (n = 1; n < ARRAY_SIZE(utc->mmu->regions); n++) {
+		if (!reg && utc->mmu->regions[n].size)
+			continue;
+		last_reg = utc->mmu->regions + n;
+
+		if (!reg) {
+			reg = last_reg;
+			v = ROUNDUP((reg - 1)->va + (reg - 1)->size,
+				    SMALL_PAGE_SIZE);
+#ifndef CFG_WITH_LPAE
+			/*
+			 * Non-LPAE mappings can't mix secure and
+			 * non-secure in a single pgdir.
+			 */
+			if (mobj_is_secure((reg - 1)->mobj) !=
+			    mobj_is_secure(mobj))
+				v = ROUNDUP(v, CORE_MMU_PGDIR_SIZE);
+#endif
+
+			/*
+			 * If mobj needs to span several page directories
+			 * the offset into the first pgdir need to match
+			 * the supplied offset or some area used by the
+			 * pager may not fit into a single pgdir.
+			 */
+			if (pgdir_offset >= 0 &&
+			    mobj->size > CORE_MMU_PGDIR_SIZE) {
+				if ((v & CORE_MMU_PGDIR_MASK) <
+				    (size_t)pgdir_offset)
+					v = ROUNDDOWN(v, CORE_MMU_PGDIR_SIZE);
+				else
+					v = ROUNDUP(v, CORE_MMU_PGDIR_SIZE);
+				v += pgdir_offset;
+			}
+			end_v = ROUNDUP(v + mobj->size, SMALL_PAGE_SIZE);
+			continue;
+		}
+
+		if (!last_reg->size)
+			continue;
+		/*
+		 * There's one registered region after our selected spot,
+		 * check if we can still fit or if we need a later spot.
+		 */
+		if (end_v > last_reg->va) {
+			reg = NULL;
+			continue;
+		}
+#ifndef CFG_WITH_LPAE
+		if (mobj_is_secure(mobj) != mobj_is_secure(last_reg->mobj) &&
+		    end_v > ROUNDDOWN(last_reg->va, CORE_MMU_PGDIR_SIZE))
+			reg = NULL;
+#endif
+	}
+
+	if (reg) {
+		TEE_Result res;
+
+		end_v = MAX(end_v, last_reg->va + last_reg->size);
+		res = alloc_pgt(utc, utc->mmu->ta_private_vmem_start, end_v);
+		if (res != TEE_SUCCESS)
+			return res;
+
+		*va = v;
+		reg->va = v;
+		reg->mobj = mobj;
+		reg->offset = 0;
+		reg->size = ROUNDUP(mobj->size, SMALL_PAGE_SIZE);
+		if (mobj_is_secure(mobj))
+			reg->attr = TEE_MATTR_SECURE;
+		else
+			reg->attr = 0;
+		return TEE_SUCCESS;
+	}
+
+	return TEE_ERROR_OUT_OF_MEMORY;
+}
+
+void tee_mmu_rem_rwmem(struct user_ta_ctx *utc, struct mobj *mobj, vaddr_t va)
+{
+	size_t n;
+
+	for (n = 0; n < ARRAY_SIZE(utc->mmu->regions); n++) {
+		struct tee_ta_region *reg = utc->mmu->regions + n;
+
+		if (reg->mobj == mobj && reg->va == va) {
+			free_pgt(utc, reg->va, reg->size);
+			memset(reg, 0, sizeof(*reg));
+			return;
+		}
+	}
+}
+
+/*
+ * tee_mmu_final - finalise and free ctx mmu
+ */
+void tee_mmu_final(struct user_ta_ctx *utc)
+{
+	uint32_t asid = 1 << ((utc->context - 1) & 0xff);
+
+	/* return ASID */
+	g_asid |= asid;
+
+	/* clear MMU entries to avoid clash when asid is reused */
+	secure_mmu_unifiedtlbinv_byasid(utc->context & 0xff);
+	utc->context = 0;
+
+	free(utc->mmu);
+	utc->mmu = NULL;
+}
+
+/* return true only if buffer fits inside TA private memory */
+bool tee_mmu_is_vbuf_inside_ta_private(const struct user_ta_ctx *utc,
+				  const void *va, size_t size)
+{
+	return core_is_buffer_inside(va, size,
+	  utc->mmu->ta_private_vmem_start,
+	  utc->mmu->ta_private_vmem_end - utc->mmu->ta_private_vmem_start);
+}
+
+/* return true only if buffer intersects TA private memory */
+bool tee_mmu_is_vbuf_intersect_ta_private(const struct user_ta_ctx *utc,
+					  const void *va, size_t size)
+{
+	return core_is_buffer_intersect(va, size,
+	  utc->mmu->ta_private_vmem_start,
+	  utc->mmu->ta_private_vmem_end - utc->mmu->ta_private_vmem_start);
+}
+
+TEE_Result tee_mmu_vbuf_to_mobj_offs(const struct user_ta_ctx *utc,
+				     const void *va, size_t size,
+				     struct mobj **mobj, size_t *offs)
+{
+	size_t n;
+
+	for (n = 0; n < ARRAY_SIZE(utc->mmu->regions); n++) {
+		if (!utc->mmu->regions[n].mobj)
+			continue;
+		if (core_is_buffer_inside(va, size, utc->mmu->regions[n].va,
+					  utc->mmu->regions[n].size)) {
+			*mobj = utc->mmu->regions[n].mobj;
+			*offs = (vaddr_t)va - utc->mmu->regions[n].va +
+				utc->mmu->regions[n].offset;
+			return TEE_SUCCESS;
+		}
+	}
+
+	return TEE_ERROR_BAD_PARAMETERS;
+}
+
+static TEE_Result tee_mmu_user_va2pa_attr(const struct user_ta_ctx *utc,
+			void *ua, paddr_t *pa, uint32_t *attr)
+{
+	size_t n;
+
+	for (n = 0; n < ARRAY_SIZE(utc->mmu->regions); n++) {
+		if (core_is_buffer_inside(ua, 1, utc->mmu->regions[n].va,
+					  utc->mmu->regions[n].size)) {
+			if (pa) {
+				TEE_Result res;
+				paddr_t p;
+
+				res = mobj_get_pa(utc->mmu->regions[n].mobj,
+						  utc->mmu->regions[n].offset,
+						  0, &p);
+				if (res != TEE_SUCCESS)
+					return res;
+
+				*pa = (paddr_t)ua - utc->mmu->regions[n].va + p;
+			}
+			if (attr)
+				*attr = utc->mmu->regions[n].attr;
+			return TEE_SUCCESS;
+		}
+	}
+	return TEE_ERROR_ACCESS_DENIED;
+}
+
+TEE_Result tee_mmu_user_va2pa_helper(const struct user_ta_ctx *utc, void *ua,
+				     paddr_t *pa)
+{
+	return tee_mmu_user_va2pa_attr(utc, ua, pa, NULL);
+}
+
+/* */
+TEE_Result tee_mmu_user_pa2va_helper(const struct user_ta_ctx *utc,
+				      paddr_t pa, void **va)
+{
+	TEE_Result res;
+	paddr_t p;
+	size_t n;
+
+	for (n = 0; n < ARRAY_SIZE(utc->mmu->regions); n++) {
+		if (!utc->mmu->regions[n].mobj)
+			continue;
+
+		res = mobj_get_pa(utc->mmu->regions[n].mobj,
+				  utc->mmu->regions[n].offset, 0, &p);
+		if (res != TEE_SUCCESS)
+			return res;
+
+		if (core_is_buffer_inside(pa, 1, p,
+					  utc->mmu->regions[n].size)) {
+			*va = (void *)(pa - p + utc->mmu->regions[n].va);
+			return TEE_SUCCESS;
+		}
+	}
+
+	return TEE_ERROR_ACCESS_DENIED;
+}
+
+TEE_Result tee_mmu_check_access_rights(const struct user_ta_ctx *utc,
+				       uint32_t flags, uaddr_t uaddr,
+				       size_t len)
+{
+	uaddr_t a;
+	size_t addr_incr = MIN(CORE_MMU_USER_CODE_SIZE,
+			       CORE_MMU_USER_PARAM_SIZE);
+
+	/* Address wrap */
+	if ((uaddr + len) < uaddr)
+		return TEE_ERROR_ACCESS_DENIED;
+
+	if ((flags & TEE_MEMORY_ACCESS_NONSECURE) &&
+	    (flags & TEE_MEMORY_ACCESS_SECURE))
+		return TEE_ERROR_ACCESS_DENIED;
+
+	/*
+	 * Rely on TA private memory test to check if address range is private
+	 * to TA or not.
+	 */
+	if (!(flags & TEE_MEMORY_ACCESS_ANY_OWNER) &&
+	   !tee_mmu_is_vbuf_inside_ta_private(utc, (void *)uaddr, len))
+		return TEE_ERROR_ACCESS_DENIED;
+
+	for (a = uaddr; a < (uaddr + len); a += addr_incr) {
+		uint32_t attr;
+		TEE_Result res;
+
+		res = tee_mmu_user_va2pa_attr(utc, (void *)a, NULL, &attr);
+		if (res != TEE_SUCCESS)
+			return res;
+
+		if ((flags & TEE_MEMORY_ACCESS_NONSECURE) &&
+		    (attr & TEE_MATTR_SECURE))
+			return TEE_ERROR_ACCESS_DENIED;
+
+		if ((flags & TEE_MEMORY_ACCESS_SECURE) &&
+		    !(attr & TEE_MATTR_SECURE))
+			return TEE_ERROR_ACCESS_DENIED;
+
+		if ((flags & TEE_MEMORY_ACCESS_WRITE) && !(attr & TEE_MATTR_UW))
+			return TEE_ERROR_ACCESS_DENIED;
+		if ((flags & TEE_MEMORY_ACCESS_READ) && !(attr & TEE_MATTR_UR))
+			return TEE_ERROR_ACCESS_DENIED;
+	}
+
+	return TEE_SUCCESS;
+}
+
+void tee_mmu_set_ctx(struct tee_ta_ctx *ctx)
+{
+	struct thread_specific_data *tsd = thread_get_tsd();
+
+	core_mmu_set_user_map(NULL);
+#ifdef CFG_SMALL_PAGE_USER_TA
+	/*
+	 * No matter what happens below, the current user TA will not be
+	 * current any longer. Make sure pager is in sync with that.
+	 * This function has to be called before there's a chance that
+	 * pgt_free_unlocked() is called.
+	 *
+	 * Save translation tables in a cache if it's a user TA.
+	 */
+	pgt_free(&tsd->pgt_cache, tsd->ctx && is_user_ta_ctx(tsd->ctx));
+#endif
+
+	if (ctx && is_user_ta_ctx(ctx)) {
+		struct core_mmu_user_map map;
+		struct user_ta_ctx *utc = to_user_ta_ctx(ctx);
+
+		core_mmu_create_user_map(utc, &map);
+		core_mmu_set_user_map(&map);
+		tee_pager_assign_uta_tables(utc);
+	}
+	tsd->ctx = ctx;
+}
+
+struct tee_ta_ctx *tee_mmu_get_ctx(void)
+{
+	return thread_get_tsd()->ctx;
+}
+
+uintptr_t tee_mmu_get_load_addr(const struct tee_ta_ctx *const ctx)
+{
+	const struct user_ta_ctx *utc = to_user_ta_ctx((void *)ctx);
+
+	assert(utc->mmu);
+	return utc->mmu->regions[TEE_MMU_UMAP_CODE_IDX].va;
+}
+
+void teecore_init_ta_ram(void)
+{
+	vaddr_t s;
+	vaddr_t e;
+	paddr_t ps;
+	paddr_t pe;
+
+	/* get virtual addr/size of RAM where TA are loaded/executedNSec
+	 * shared mem allcated from teecore */
+	core_mmu_get_mem_by_type(MEM_AREA_TA_RAM, &s, &e);
+	ps = virt_to_phys((void *)s);
+	pe = virt_to_phys((void *)(e - 1)) + 1;
+
+	if (!ps || (ps & CORE_MMU_USER_CODE_MASK) ||
+	    !pe || (pe & CORE_MMU_USER_CODE_MASK))
+		panic("invalid TA RAM");
+
+	/* extra check: we could rely on  core_mmu_get_mem_by_type() */
+	if (!tee_pbuf_is_sec(ps, pe - ps))
+		panic("TA RAM is not secure");
+
+	if (!tee_mm_is_empty(&tee_mm_sec_ddr))
+		panic("TA RAM pool is not empty");
+
+	/* remove previous config and init TA ddr memory pool */
+	tee_mm_final(&tee_mm_sec_ddr);
+	tee_mm_init(&tee_mm_sec_ddr, ps, pe, CORE_MMU_USER_CODE_SHIFT,
+		    TEE_MM_POOL_NO_FLAGS);
+}
+
+void teecore_init_pub_ram(void)
+{
+	vaddr_t s;
+	vaddr_t e;
+
+	/* get virtual addr/size of NSec shared mem allcated from teecore */
+	core_mmu_get_mem_by_type(MEM_AREA_NSEC_SHM, &s, &e);
+
+	if (s >= e || s & SMALL_PAGE_MASK || e & SMALL_PAGE_MASK)
+		panic("invalid PUB RAM");
+
+	/* extra check: we could rely on  core_mmu_get_mem_by_type() */
+	if (!tee_vbuf_is_non_sec(s, e - s))
+		panic("PUB RAM is not non-secure");
+
+#ifdef CFG_PL310
+	/* Allocate statically the l2cc mutex */
+	tee_l2cc_store_mutex_boot_pa(virt_to_phys((void *)s));
+	s += sizeof(uint32_t);			/* size of a pl310 mutex */
+	s =  ROUNDUP(s, SMALL_PAGE_SIZE);	/* keep required alignment */
+#endif
+
+	default_nsec_shm_paddr = virt_to_phys((void *)s);
+	default_nsec_shm_size = e - s;
+}
+
+uint32_t tee_mmu_user_get_cache_attr(struct user_ta_ctx *utc, void *va)
+{
+	uint32_t attr;
+
+	if (tee_mmu_user_va2pa_attr(utc, va, NULL, &attr) != TEE_SUCCESS)
+		panic("cannot get attr");
+
+	return (attr >> TEE_MATTR_CACHE_SHIFT) & TEE_MATTR_CACHE_MASK;
+}
diff --git a/core/arch/arm/mm/tee_pager.c b/core/arch/arm/mm/tee_pager.c
new file mode 100644
index 0000000..c7238fe
--- /dev/null
+++ b/core/arch/arm/mm/tee_pager.c
@@ -0,0 +1,1473 @@
+/*
+ * Copyright (c) 2016, Linaro Limited
+ * 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 <arm.h>
+#include <assert.h>
+#include <keep.h>
+#include <sys/queue.h>
+#include <kernel/abort.h>
+#include <kernel/panic.h>
+#include <kernel/spinlock.h>
+#include <kernel/tee_misc.h>
+#include <kernel/tee_ta_manager.h>
+#include <kernel/thread.h>
+#include <mm/core_memprot.h>
+#include <mm/tee_mm.h>
+#include <mm/tee_pager.h>
+#include <types_ext.h>
+#include <stdlib.h>
+#include <tee_api_defines.h>
+#include <tee/tee_cryp_provider.h>
+#include <trace.h>
+#include <utee_defines.h>
+#include <util.h>
+
+#include "pager_private.h"
+
+#define PAGER_AE_KEY_BITS	256
+
+struct pager_rw_pstate {
+	uint64_t iv;
+	uint8_t tag[PAGER_AES_GCM_TAG_LEN];
+};
+
+enum area_type {
+	AREA_TYPE_RO,
+	AREA_TYPE_RW,
+	AREA_TYPE_LOCK,
+};
+
+struct tee_pager_area {
+	union {
+		const uint8_t *hashes;
+		struct pager_rw_pstate *rwp;
+	} u;
+	uint8_t *store;
+	enum area_type type;
+	uint32_t flags;
+	vaddr_t base;
+	size_t size;
+	struct pgt *pgt;
+	TAILQ_ENTRY(tee_pager_area) link;
+};
+
+TAILQ_HEAD(tee_pager_area_head, tee_pager_area);
+
+static struct tee_pager_area_head tee_pager_area_head =
+	TAILQ_HEAD_INITIALIZER(tee_pager_area_head);
+
+#define INVALID_PGIDX	UINT_MAX
+
+/*
+ * struct tee_pager_pmem - Represents a physical page used for paging.
+ *
+ * @pgidx	an index of the entry in area->ti.
+ * @va_alias	Virtual address where the physical page always is aliased.
+ *		Used during remapping of the page when the content need to
+ *		be updated before it's available at the new location.
+ * @area	a pointer to the pager area
+ */
+struct tee_pager_pmem {
+	unsigned pgidx;
+	void *va_alias;
+	struct tee_pager_area *area;
+	TAILQ_ENTRY(tee_pager_pmem) link;
+};
+
+/* The list of physical pages. The first page in the list is the oldest */
+TAILQ_HEAD(tee_pager_pmem_head, tee_pager_pmem);
+
+static struct tee_pager_pmem_head tee_pager_pmem_head =
+	TAILQ_HEAD_INITIALIZER(tee_pager_pmem_head);
+
+static struct tee_pager_pmem_head tee_pager_lock_pmem_head =
+	TAILQ_HEAD_INITIALIZER(tee_pager_lock_pmem_head);
+
+static uint8_t pager_ae_key[PAGER_AE_KEY_BITS / 8];
+
+/* number of pages hidden */
+#define TEE_PAGER_NHIDE (tee_pager_npages / 3)
+
+/* Number of registered physical pages, used hiding pages. */
+static size_t tee_pager_npages;
+
+#ifdef CFG_WITH_STATS
+static struct tee_pager_stats pager_stats;
+
+static inline void incr_ro_hits(void)
+{
+	pager_stats.ro_hits++;
+}
+
+static inline void incr_rw_hits(void)
+{
+	pager_stats.rw_hits++;
+}
+
+static inline void incr_hidden_hits(void)
+{
+	pager_stats.hidden_hits++;
+}
+
+static inline void incr_zi_released(void)
+{
+	pager_stats.zi_released++;
+}
+
+static inline void incr_npages_all(void)
+{
+	pager_stats.npages_all++;
+}
+
+static inline void set_npages(void)
+{
+	pager_stats.npages = tee_pager_npages;
+}
+
+void tee_pager_get_stats(struct tee_pager_stats *stats)
+{
+	*stats = pager_stats;
+
+	pager_stats.hidden_hits = 0;
+	pager_stats.ro_hits = 0;
+	pager_stats.rw_hits = 0;
+	pager_stats.zi_released = 0;
+}
+
+#else /* CFG_WITH_STATS */
+static inline void incr_ro_hits(void) { }
+static inline void incr_rw_hits(void) { }
+static inline void incr_hidden_hits(void) { }
+static inline void incr_zi_released(void) { }
+static inline void incr_npages_all(void) { }
+static inline void set_npages(void) { }
+
+void tee_pager_get_stats(struct tee_pager_stats *stats)
+{
+	memset(stats, 0, sizeof(struct tee_pager_stats));
+}
+#endif /* CFG_WITH_STATS */
+
+static struct pgt pager_core_pgt;
+struct core_mmu_table_info tee_pager_tbl_info;
+static struct core_mmu_table_info pager_alias_tbl_info;
+
+static unsigned pager_spinlock = SPINLOCK_UNLOCK;
+
+/* Defines the range of the alias area */
+static tee_mm_entry_t *pager_alias_area;
+/*
+ * Physical pages are added in a stack like fashion to the alias area,
+ * @pager_alias_next_free gives the address of next free entry if
+ * @pager_alias_next_free is != 0
+ */
+static uintptr_t pager_alias_next_free;
+
+static uint32_t pager_lock(void)
+{
+	uint32_t exceptions = thread_mask_exceptions(THREAD_EXCP_ALL);
+
+	cpu_spin_lock(&pager_spinlock);
+	return exceptions;
+}
+
+static void pager_unlock(uint32_t exceptions)
+{
+	cpu_spin_unlock(&pager_spinlock);
+	thread_set_exceptions(exceptions);
+}
+
+static void set_alias_area(tee_mm_entry_t *mm)
+{
+	struct core_mmu_table_info *ti = &pager_alias_tbl_info;
+	size_t tbl_va_size;
+	unsigned idx;
+	unsigned last_idx;
+	vaddr_t smem = tee_mm_get_smem(mm);
+	size_t nbytes = tee_mm_get_bytes(mm);
+
+	DMSG("0x%" PRIxVA " - 0x%" PRIxVA, smem, smem + nbytes);
+
+	if (pager_alias_area)
+		panic("null pager_alias_area");
+
+	if (!ti->num_entries && !core_mmu_find_table(smem, UINT_MAX, ti))
+		panic("Can't find translation table");
+
+	if ((1 << ti->shift) != SMALL_PAGE_SIZE)
+		panic("Unsupported page size in translation table");
+
+	tbl_va_size = (1 << ti->shift) * ti->num_entries;
+	if (!core_is_buffer_inside(smem, nbytes,
+				   ti->va_base, tbl_va_size)) {
+		EMSG("area 0x%" PRIxVA " len 0x%zx doesn't fit it translation table 0x%" PRIxVA " len 0x%zx",
+		     smem, nbytes, ti->va_base, tbl_va_size);
+		panic();
+	}
+
+	if (smem & SMALL_PAGE_MASK || nbytes & SMALL_PAGE_MASK)
+		panic("invalid area alignment");
+
+	pager_alias_area = mm;
+	pager_alias_next_free = smem;
+
+	/* Clear all mapping in the alias area */
+	idx = core_mmu_va2idx(ti, smem);
+	last_idx = core_mmu_va2idx(ti, smem + nbytes);
+	for (; idx < last_idx; idx++)
+		core_mmu_set_entry(ti, idx, 0, 0);
+
+	/* TODO only invalidate entries touched above */
+	core_tlb_maintenance(TLBINV_UNIFIEDTLB, 0);
+}
+
+static void generate_ae_key(void)
+{
+	if (rng_generate(pager_ae_key, sizeof(pager_ae_key)) != TEE_SUCCESS)
+		panic("failed to generate random");
+}
+
+void tee_pager_init(tee_mm_entry_t *mm_alias)
+{
+	set_alias_area(mm_alias);
+	generate_ae_key();
+}
+
+static void *pager_add_alias_page(paddr_t pa)
+{
+	unsigned idx;
+	struct core_mmu_table_info *ti = &pager_alias_tbl_info;
+	uint32_t attr = TEE_MATTR_VALID_BLOCK | TEE_MATTR_GLOBAL |
+			(TEE_MATTR_CACHE_CACHED << TEE_MATTR_CACHE_SHIFT) |
+			TEE_MATTR_SECURE | TEE_MATTR_PRW;
+
+	DMSG("0x%" PRIxPA, pa);
+
+	if (!pager_alias_next_free || !ti->num_entries)
+		panic("invalid alias entry");
+
+	idx = core_mmu_va2idx(ti, pager_alias_next_free);
+	core_mmu_set_entry(ti, idx, pa, attr);
+	pgt_inc_used_entries(&pager_core_pgt);
+	pager_alias_next_free += SMALL_PAGE_SIZE;
+	if (pager_alias_next_free >= (tee_mm_get_smem(pager_alias_area) +
+				      tee_mm_get_bytes(pager_alias_area)))
+		pager_alias_next_free = 0;
+	return (void *)core_mmu_idx2va(ti, idx);
+}
+
+static struct tee_pager_area *alloc_area(struct pgt *pgt,
+					 vaddr_t base, size_t size,
+					 uint32_t flags, const void *store,
+					 const void *hashes)
+{
+	struct tee_pager_area *area = calloc(1, sizeof(*area));
+	enum area_type at;
+	tee_mm_entry_t *mm_store = NULL;
+
+	if (!area)
+		return NULL;
+
+	if (flags & (TEE_MATTR_PW | TEE_MATTR_UW)) {
+		if (flags & TEE_MATTR_LOCKED) {
+			at = AREA_TYPE_LOCK;
+			goto out;
+		}
+		mm_store = tee_mm_alloc(&tee_mm_sec_ddr, size);
+		if (!mm_store)
+			goto bad;
+		area->store = phys_to_virt(tee_mm_get_smem(mm_store),
+					   MEM_AREA_TA_RAM);
+		if (!area->store)
+			goto bad;
+		area->u.rwp = calloc(size / SMALL_PAGE_SIZE,
+				     sizeof(struct pager_rw_pstate));
+		if (!area->u.rwp)
+			goto bad;
+		at = AREA_TYPE_RW;
+	} else {
+		area->store = (void *)store;
+		area->u.hashes = hashes;
+		at = AREA_TYPE_RO;
+	}
+out:
+	area->pgt = pgt;
+	area->base = base;
+	area->size = size;
+	area->flags = flags;
+	area->type = at;
+	return area;
+bad:
+	tee_mm_free(mm_store);
+	free(area->u.rwp);
+	free(area);
+	return NULL;
+}
+
+static void area_insert_tail(struct tee_pager_area *area)
+{
+	uint32_t exceptions = pager_lock();
+
+	TAILQ_INSERT_TAIL(&tee_pager_area_head, area, link);
+
+	pager_unlock(exceptions);
+}
+KEEP_PAGER(area_insert_tail);
+
+static size_t tbl_usage_count(struct pgt *pgt)
+{
+	size_t n;
+	paddr_t pa;
+	size_t usage = 0;
+
+	for (n = 0; n < tee_pager_tbl_info.num_entries; n++) {
+		core_mmu_get_entry_primitive(pgt->tbl, tee_pager_tbl_info.level,
+					     n, &pa, NULL);
+		if (pa)
+			usage++;
+	}
+	return usage;
+}
+
+bool tee_pager_add_core_area(vaddr_t base, size_t size, uint32_t flags,
+			const void *store, const void *hashes)
+{
+	struct tee_pager_area *area;
+	size_t tbl_va_size;
+	struct core_mmu_table_info *ti = &tee_pager_tbl_info;
+
+	DMSG("0x%" PRIxPTR " - 0x%" PRIxPTR " : flags 0x%x, store %p, hashes %p",
+		base, base + size, flags, store, hashes);
+
+	if (base & SMALL_PAGE_MASK || size & SMALL_PAGE_MASK || !size) {
+		EMSG("invalid pager area [%" PRIxVA " +0x%zx]", base, size);
+		panic();
+	}
+
+	if (!(flags & TEE_MATTR_PW) && (!store || !hashes))
+		panic("write pages cannot provide store or hashes");
+
+	if ((flags & TEE_MATTR_PW) && (store || hashes))
+		panic("non-write pages must provide store and hashes");
+
+	if (!pager_core_pgt.tbl) {
+		pager_core_pgt.tbl = ti->table;
+		pgt_set_used_entries(&pager_core_pgt,
+				     tbl_usage_count(&pager_core_pgt));
+	}
+
+	tbl_va_size = (1 << ti->shift) * ti->num_entries;
+	if (!core_is_buffer_inside(base, size, ti->va_base, tbl_va_size)) {
+		DMSG("area 0x%" PRIxPTR " len 0x%zx doesn't fit it translation table 0x%" PRIxVA " len 0x%zx",
+			base, size, ti->va_base, tbl_va_size);
+		return false;
+	}
+
+	area = alloc_area(&pager_core_pgt, base, size, flags, store, hashes);
+	if (!area)
+		return false;
+
+	area_insert_tail(area);
+	return true;
+}
+
+static struct tee_pager_area *find_area(struct tee_pager_area_head *areas,
+					vaddr_t va)
+{
+	struct tee_pager_area *area;
+
+	if (!areas)
+		return NULL;
+
+	TAILQ_FOREACH(area, areas, link) {
+		if (core_is_buffer_inside(va, 1, area->base, area->size))
+			return area;
+	}
+	return NULL;
+}
+
+#ifdef CFG_PAGED_USER_TA
+static struct tee_pager_area *find_uta_area(vaddr_t va)
+{
+	struct tee_ta_ctx *ctx = thread_get_tsd()->ctx;
+
+	if (!ctx || !is_user_ta_ctx(ctx))
+		return NULL;
+	return find_area(to_user_ta_ctx(ctx)->areas, va);
+}
+#else
+static struct tee_pager_area *find_uta_area(vaddr_t va __unused)
+{
+	return NULL;
+}
+#endif /*CFG_PAGED_USER_TA*/
+
+
+static uint32_t get_area_mattr(uint32_t area_flags)
+{
+	uint32_t attr = TEE_MATTR_VALID_BLOCK | TEE_MATTR_SECURE |
+			TEE_MATTR_CACHE_CACHED << TEE_MATTR_CACHE_SHIFT |
+			(area_flags & (TEE_MATTR_PRWX | TEE_MATTR_URWX));
+
+	if (!(area_flags & (TEE_MATTR_UR | TEE_MATTR_UX | TEE_MATTR_UW)))
+		attr |= TEE_MATTR_GLOBAL;
+
+	return attr;
+}
+
+static paddr_t get_pmem_pa(struct tee_pager_pmem *pmem)
+{
+	paddr_t pa;
+	unsigned idx;
+
+	idx = core_mmu_va2idx(&pager_alias_tbl_info, (vaddr_t)pmem->va_alias);
+	core_mmu_get_entry(&pager_alias_tbl_info, idx, &pa, NULL);
+	return pa;
+}
+
+static bool decrypt_page(struct pager_rw_pstate *rwp, const void *src,
+			void *dst)
+{
+	struct pager_aes_gcm_iv iv = {
+		{ (vaddr_t)rwp, rwp->iv >> 32, rwp->iv }
+	};
+
+	return pager_aes_gcm_decrypt(pager_ae_key, sizeof(pager_ae_key),
+				     &iv, rwp->tag, src, dst, SMALL_PAGE_SIZE);
+}
+
+static void encrypt_page(struct pager_rw_pstate *rwp, void *src, void *dst)
+{
+	struct pager_aes_gcm_iv iv;
+
+	assert((rwp->iv + 1) > rwp->iv);
+	rwp->iv++;
+	/*
+	 * IV is constructed as recommended in section "8.2.1 Deterministic
+	 * Construction" of "Recommendation for Block Cipher Modes of
+	 * Operation: Galois/Counter Mode (GCM) and GMAC",
+	 * http://csrc.nist.gov/publications/nistpubs/800-38D/SP-800-38D.pdf
+	 */
+	iv.iv[0] = (vaddr_t)rwp;
+	iv.iv[1] = rwp->iv >> 32;
+	iv.iv[2] = rwp->iv;
+
+	if (!pager_aes_gcm_encrypt(pager_ae_key, sizeof(pager_ae_key),
+				   &iv, rwp->tag,
+				   src, dst, SMALL_PAGE_SIZE))
+		panic("gcm failed");
+}
+
+static void tee_pager_load_page(struct tee_pager_area *area, vaddr_t page_va,
+			void *va_alias)
+{
+	size_t idx = (page_va - area->base) >> SMALL_PAGE_SHIFT;
+	const void *stored_page = area->store + idx * SMALL_PAGE_SIZE;
+
+	switch (area->type) {
+	case AREA_TYPE_RO:
+		{
+			const void *hash = area->u.hashes +
+					   idx * TEE_SHA256_HASH_SIZE;
+
+			memcpy(va_alias, stored_page, SMALL_PAGE_SIZE);
+			incr_ro_hits();
+
+			if (hash_sha256_check(hash, va_alias,
+					      SMALL_PAGE_SIZE) != TEE_SUCCESS) {
+				EMSG("PH 0x%" PRIxVA " failed", page_va);
+				panic();
+			}
+		}
+		break;
+	case AREA_TYPE_RW:
+		FMSG("Restore %p %#" PRIxVA " iv %#" PRIx64,
+			va_alias, page_va, area->u.rwp[idx].iv);
+		if (!area->u.rwp[idx].iv)
+			memset(va_alias, 0, SMALL_PAGE_SIZE);
+		else if (!decrypt_page(&area->u.rwp[idx], stored_page,
+				       va_alias)) {
+			EMSG("PH 0x%" PRIxVA " failed", page_va);
+			panic();
+		}
+		incr_rw_hits();
+		break;
+	case AREA_TYPE_LOCK:
+		FMSG("Zero init %p %#" PRIxVA, va_alias, page_va);
+		memset(va_alias, 0, SMALL_PAGE_SIZE);
+		break;
+	default:
+		panic();
+	}
+}
+
+static void tee_pager_save_page(struct tee_pager_pmem *pmem, uint32_t attr)
+{
+	const uint32_t dirty_bits = TEE_MATTR_PW | TEE_MATTR_UW |
+				    TEE_MATTR_HIDDEN_DIRTY_BLOCK;
+
+	if (pmem->area->type == AREA_TYPE_RW && (attr & dirty_bits)) {
+		size_t offs = pmem->area->base & CORE_MMU_PGDIR_MASK;
+		size_t idx = pmem->pgidx - (offs >> SMALL_PAGE_SHIFT);
+		void *stored_page = pmem->area->store + idx * SMALL_PAGE_SIZE;
+
+		assert(pmem->area->flags & (TEE_MATTR_PW | TEE_MATTR_UW));
+		encrypt_page(&pmem->area->u.rwp[idx], pmem->va_alias,
+			     stored_page);
+		FMSG("Saved %#" PRIxVA " iv %#" PRIx64,
+			pmem->area->base + idx * SMALL_PAGE_SIZE,
+			pmem->area->u.rwp[idx].iv);
+	}
+}
+
+static void area_get_entry(struct tee_pager_area *area, size_t idx,
+			   paddr_t *pa, uint32_t *attr)
+{
+	assert(area->pgt);
+	assert(idx < tee_pager_tbl_info.num_entries);
+	core_mmu_get_entry_primitive(area->pgt->tbl, tee_pager_tbl_info.level,
+				     idx, pa, attr);
+}
+
+static void area_set_entry(struct tee_pager_area *area, size_t idx,
+			   paddr_t pa, uint32_t attr)
+{
+	assert(area->pgt);
+	assert(idx < tee_pager_tbl_info.num_entries);
+	core_mmu_set_entry_primitive(area->pgt->tbl, tee_pager_tbl_info.level,
+				     idx, pa, attr);
+}
+
+static size_t area_va2idx(struct tee_pager_area *area, vaddr_t va)
+{
+	return (va - (area->base & ~CORE_MMU_PGDIR_MASK)) >> SMALL_PAGE_SHIFT;
+}
+
+static vaddr_t __maybe_unused area_idx2va(struct tee_pager_area *area,
+					 size_t idx)
+{
+	return (idx << SMALL_PAGE_SHIFT) + (area->base & ~CORE_MMU_PGDIR_MASK);
+}
+
+#ifdef CFG_PAGED_USER_TA
+static void free_area(struct tee_pager_area *area)
+{
+	tee_mm_free(tee_mm_find(&tee_mm_sec_ddr,
+				virt_to_phys(area->store)));
+	if (area->type == AREA_TYPE_RW)
+		free(area->u.rwp);
+	free(area);
+}
+
+static bool pager_add_uta_area(struct user_ta_ctx *utc, vaddr_t base,
+			       size_t size)
+{
+	struct tee_pager_area *area;
+	uint32_t flags;
+	vaddr_t b = base;
+	size_t s = ROUNDUP(size, SMALL_PAGE_SIZE);
+
+	if (!utc->areas) {
+		utc->areas = malloc(sizeof(*utc->areas));
+		if (!utc->areas)
+			return false;
+		TAILQ_INIT(utc->areas);
+	}
+
+	flags = TEE_MATTR_PRW | TEE_MATTR_URWX;
+
+	while (s) {
+		size_t s2;
+
+		if (find_area(utc->areas, b))
+			return false;
+
+		s2 = MIN(CORE_MMU_PGDIR_SIZE - (b & CORE_MMU_PGDIR_MASK), s);
+
+		/* Table info will be set when the context is activated. */
+		area = alloc_area(NULL, b, s2, flags, NULL, NULL);
+		if (!area)
+			return false;
+		TAILQ_INSERT_TAIL(utc->areas, area, link);
+		b += s2;
+		s -= s2;
+	}
+
+	return true;
+}
+
+bool tee_pager_add_uta_area(struct user_ta_ctx *utc, vaddr_t base, size_t size)
+{
+	struct thread_specific_data *tsd = thread_get_tsd();
+	struct tee_pager_area *area;
+	struct core_mmu_table_info dir_info = { NULL };
+
+	if (&utc->ctx != tsd->ctx) {
+		/*
+		 * Changes are to an utc that isn't active. Just add the
+		 * areas page tables will be dealt with later.
+		 */
+		return pager_add_uta_area(utc, base, size);
+	}
+
+	/*
+	 * Assign page tables before adding areas to be able to tell which
+	 * are newly added and should be removed in case of failure.
+	 */
+	tee_pager_assign_uta_tables(utc);
+	if (!pager_add_uta_area(utc, base, size)) {
+		struct tee_pager_area *next_a;
+
+		/* Remove all added areas */
+		TAILQ_FOREACH_SAFE(area, utc->areas, link, next_a) {
+			if (!area->pgt) {
+				TAILQ_REMOVE(utc->areas, area, link);
+				free_area(area);
+			}
+		}
+		return false;
+	}
+
+	/*
+	 * Assign page tables to the new areas and make sure that the page
+	 * tables are registered in the upper table.
+	 */
+	tee_pager_assign_uta_tables(utc);
+	core_mmu_get_user_pgdir(&dir_info);
+	TAILQ_FOREACH(area, utc->areas, link) {
+		paddr_t pa;
+		size_t idx;
+		uint32_t attr;
+
+		idx = core_mmu_va2idx(&dir_info, area->pgt->vabase);
+		core_mmu_get_entry(&dir_info, idx, &pa, &attr);
+
+		/*
+		 * Check if the page table already is used, if it is, it's
+		 * already registered.
+		 */
+		if (area->pgt->num_used_entries) {
+			assert(attr & TEE_MATTR_TABLE);
+			assert(pa == virt_to_phys(area->pgt->tbl));
+			continue;
+		}
+
+		attr = TEE_MATTR_SECURE | TEE_MATTR_TABLE;
+		pa = virt_to_phys(area->pgt->tbl);
+		assert(pa);
+		/*
+		 * Note that the update of the table entry is guaranteed to
+		 * be atomic.
+		 */
+		core_mmu_set_entry(&dir_info, idx, pa, attr);
+	}
+
+	return true;
+}
+
+static void init_tbl_info_from_pgt(struct core_mmu_table_info *ti,
+				   struct pgt *pgt)
+{
+	assert(pgt);
+	ti->table = pgt->tbl;
+	ti->va_base = pgt->vabase;
+	ti->level = tee_pager_tbl_info.level;
+	ti->shift = tee_pager_tbl_info.shift;
+	ti->num_entries = tee_pager_tbl_info.num_entries;
+}
+
+static void transpose_area(struct tee_pager_area *area, struct pgt *new_pgt,
+			   vaddr_t new_base)
+{
+	uint32_t exceptions = pager_lock();
+
+	/*
+	 * If there's no pgt assigned to the old area there's no pages to
+	 * deal with either, just update with a new pgt and base.
+	 */
+	if (area->pgt) {
+		struct core_mmu_table_info old_ti;
+		struct core_mmu_table_info new_ti;
+		struct tee_pager_pmem *pmem;
+
+		init_tbl_info_from_pgt(&old_ti, area->pgt);
+		init_tbl_info_from_pgt(&new_ti, new_pgt);
+
+
+		TAILQ_FOREACH(pmem, &tee_pager_pmem_head, link) {
+			vaddr_t va;
+			paddr_t pa;
+			uint32_t attr;
+
+			if (pmem->area != area)
+				continue;
+			core_mmu_get_entry(&old_ti, pmem->pgidx, &pa, &attr);
+			core_mmu_set_entry(&old_ti, pmem->pgidx, 0, 0);
+
+			assert(pa == get_pmem_pa(pmem));
+			assert(attr);
+			assert(area->pgt->num_used_entries);
+			area->pgt->num_used_entries--;
+
+			va = core_mmu_idx2va(&old_ti, pmem->pgidx);
+			va = va - area->base + new_base;
+			pmem->pgidx = core_mmu_va2idx(&new_ti, va);
+			core_mmu_set_entry(&new_ti, pmem->pgidx, pa, attr);
+			new_pgt->num_used_entries++;
+		}
+	}
+
+	area->pgt = new_pgt;
+	area->base = new_base;
+	pager_unlock(exceptions);
+}
+KEEP_PAGER(transpose_area);
+
+void tee_pager_transfer_uta_region(struct user_ta_ctx *src_utc,
+				   vaddr_t src_base,
+				   struct user_ta_ctx *dst_utc,
+				   vaddr_t dst_base, struct pgt **dst_pgt,
+				   size_t size)
+{
+	struct tee_pager_area *area;
+	struct tee_pager_area *next_a;
+
+	TAILQ_FOREACH_SAFE(area, src_utc->areas, link, next_a) {
+		vaddr_t new_area_base;
+		size_t new_idx;
+
+		if (!core_is_buffer_inside(area->base, area->size,
+					  src_base, size))
+			continue;
+
+		TAILQ_REMOVE(src_utc->areas, area, link);
+
+		new_area_base = dst_base + (src_base - area->base);
+		new_idx = (new_area_base - dst_pgt[0]->vabase) /
+			  CORE_MMU_PGDIR_SIZE;
+		assert((new_area_base & ~CORE_MMU_PGDIR_MASK) ==
+		       dst_pgt[new_idx]->vabase);
+		transpose_area(area, dst_pgt[new_idx], new_area_base);
+
+		/*
+		 * Assert that this will not cause any conflicts in the new
+		 * utc.  This should already be guaranteed, but a bug here
+		 * could be tricky to find.
+		 */
+		assert(!find_area(dst_utc->areas, area->base));
+		TAILQ_INSERT_TAIL(dst_utc->areas, area, link);
+	}
+}
+
+static void rem_area(struct tee_pager_area_head *area_head,
+		     struct tee_pager_area *area)
+{
+	struct tee_pager_pmem *pmem;
+	uint32_t exceptions;
+
+	exceptions = pager_lock();
+
+	TAILQ_REMOVE(area_head, area, link);
+
+	TAILQ_FOREACH(pmem, &tee_pager_pmem_head, link) {
+		if (pmem->area == area) {
+			area_set_entry(area, pmem->pgidx, 0, 0);
+			pgt_dec_used_entries(area->pgt);
+			pmem->area = NULL;
+			pmem->pgidx = INVALID_PGIDX;
+		}
+	}
+
+	pager_unlock(exceptions);
+	free_area(area);
+}
+KEEP_PAGER(rem_area);
+
+void tee_pager_rem_uta_region(struct user_ta_ctx *utc, vaddr_t base,
+			      size_t size)
+{
+	struct tee_pager_area *area;
+	struct tee_pager_area *next_a;
+	size_t s = ROUNDUP(size, SMALL_PAGE_SIZE);
+
+	TAILQ_FOREACH_SAFE(area, utc->areas, link, next_a) {
+		if (core_is_buffer_inside(area->base, area->size, base, s))
+			rem_area(utc->areas, area);
+	}
+}
+
+void tee_pager_rem_uta_areas(struct user_ta_ctx *utc)
+{
+	struct tee_pager_area *area;
+
+	if (!utc->areas)
+		return;
+
+	while (true) {
+		area = TAILQ_FIRST(utc->areas);
+		if (!area)
+			break;
+		TAILQ_REMOVE(utc->areas, area, link);
+		free_area(area);
+	}
+
+	free(utc->areas);
+}
+
+bool tee_pager_set_uta_area_attr(struct user_ta_ctx *utc, vaddr_t base,
+				 size_t size, uint32_t flags)
+{
+	bool ret;
+	vaddr_t b = base;
+	size_t s = size;
+	size_t s2;
+	struct tee_pager_area *area = find_area(utc->areas, b);
+	uint32_t exceptions;
+	struct tee_pager_pmem *pmem;
+	paddr_t pa;
+	uint32_t a;
+	uint32_t f;
+
+	f = (flags & TEE_MATTR_URWX) | TEE_MATTR_UR | TEE_MATTR_PR;
+	if (f & TEE_MATTR_UW)
+		f |= TEE_MATTR_PW;
+	f = get_area_mattr(f);
+
+	exceptions = pager_lock();
+
+	while (s) {
+		s2 = MIN(CORE_MMU_PGDIR_SIZE - (b & CORE_MMU_PGDIR_MASK), s);
+		if (!area || area->base != b || area->size != s2) {
+			ret = false;
+			goto out;
+		}
+		b += s2;
+		s -= s2;
+
+		TAILQ_FOREACH(pmem, &tee_pager_pmem_head, link) {
+			if (pmem->area != area)
+				continue;
+			area_get_entry(pmem->area, pmem->pgidx, &pa, &a);
+			if (a & TEE_MATTR_VALID_BLOCK)
+				assert(pa == get_pmem_pa(pmem));
+			else
+				pa = get_pmem_pa(pmem);
+			if (a == f)
+				continue;
+			area_set_entry(pmem->area, pmem->pgidx, 0, 0);
+			/* TODO only invalidate entries touched above */
+			core_tlb_maintenance(TLBINV_UNIFIEDTLB, 0);
+			if (!(flags & TEE_MATTR_UW))
+				tee_pager_save_page(pmem, a);
+
+			area_set_entry(pmem->area, pmem->pgidx, pa, f);
+
+			if (flags & TEE_MATTR_UX) {
+				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);
+			}
+		}
+
+		area->flags = f;
+		area = TAILQ_NEXT(area, link);
+	}
+
+	ret = true;
+out:
+	pager_unlock(exceptions);
+	return ret;
+}
+KEEP_PAGER(tee_pager_set_uta_area_attr);
+#endif /*CFG_PAGED_USER_TA*/
+
+static bool tee_pager_unhide_page(vaddr_t page_va)
+{
+	struct tee_pager_pmem *pmem;
+
+	TAILQ_FOREACH(pmem, &tee_pager_pmem_head, link) {
+		paddr_t pa;
+		uint32_t attr;
+
+		if (pmem->pgidx == INVALID_PGIDX)
+			continue;
+
+		area_get_entry(pmem->area, pmem->pgidx, &pa, &attr);
+
+		if (!(attr &
+		     (TEE_MATTR_HIDDEN_BLOCK | TEE_MATTR_HIDDEN_DIRTY_BLOCK)))
+			continue;
+
+		if (area_va2idx(pmem->area, page_va) == pmem->pgidx) {
+			uint32_t a = get_area_mattr(pmem->area->flags);
+
+			/* page is hidden, show and move to back */
+			if (pa != get_pmem_pa(pmem))
+				panic("unexpected pa");
+
+			/*
+			 * If it's not a dirty block, then it should be
+			 * read only.
+			 */
+			if (!(attr & TEE_MATTR_HIDDEN_DIRTY_BLOCK))
+				a &= ~(TEE_MATTR_PW | TEE_MATTR_UW);
+			else
+				FMSG("Unhide %#" PRIxVA, page_va);
+
+			if (page_va == 0x8000a000)
+				FMSG("unhide %#" PRIxVA " a %#" PRIX32,
+					page_va, a);
+			area_set_entry(pmem->area, pmem->pgidx, pa, a);
+
+			TAILQ_REMOVE(&tee_pager_pmem_head, pmem, link);
+			TAILQ_INSERT_TAIL(&tee_pager_pmem_head, pmem, link);
+
+			/* TODO only invalidate entry touched above */
+			core_tlb_maintenance(TLBINV_UNIFIEDTLB, 0);
+
+			incr_hidden_hits();
+			return true;
+		}
+	}
+
+	return false;
+}
+
+static void tee_pager_hide_pages(void)
+{
+	struct tee_pager_pmem *pmem;
+	size_t n = 0;
+
+	TAILQ_FOREACH(pmem, &tee_pager_pmem_head, link) {
+		paddr_t pa;
+		uint32_t attr;
+		uint32_t a;
+
+		if (n >= TEE_PAGER_NHIDE)
+			break;
+		n++;
+
+		/* we cannot hide pages when pmem->area is not defined. */
+		if (!pmem->area)
+			continue;
+
+		area_get_entry(pmem->area, pmem->pgidx, &pa, &attr);
+		if (!(attr & TEE_MATTR_VALID_BLOCK))
+			continue;
+
+		assert(pa == get_pmem_pa(pmem));
+		if (attr & (TEE_MATTR_PW | TEE_MATTR_UW)){
+			a = TEE_MATTR_HIDDEN_DIRTY_BLOCK;
+			FMSG("Hide %#" PRIxVA,
+			     area_idx2va(pmem->area, pmem->pgidx));
+		} else
+			a = TEE_MATTR_HIDDEN_BLOCK;
+		area_set_entry(pmem->area, pmem->pgidx, pa, a);
+	}
+
+	/* TODO only invalidate entries touched above */
+	core_tlb_maintenance(TLBINV_UNIFIEDTLB, 0);
+}
+
+/*
+ * Find mapped pmem, hide and move to pageble pmem.
+ * Return false if page was not mapped, and true if page was mapped.
+ */
+static bool tee_pager_release_one_phys(struct tee_pager_area *area,
+				       vaddr_t page_va)
+{
+	struct tee_pager_pmem *pmem;
+	unsigned pgidx;
+	paddr_t pa;
+	uint32_t attr;
+
+	pgidx = area_va2idx(area, page_va);
+	area_get_entry(area, pgidx, &pa, &attr);
+
+	FMSG("%" PRIxVA " : %" PRIxPA "|%x", page_va, pa, attr);
+
+	TAILQ_FOREACH(pmem, &tee_pager_lock_pmem_head, link) {
+		if (pmem->area != area || pmem->pgidx != pgidx)
+			continue;
+
+		assert(pa == get_pmem_pa(pmem));
+		area_set_entry(area, pgidx, 0, 0);
+		pgt_dec_used_entries(area->pgt);
+		TAILQ_REMOVE(&tee_pager_lock_pmem_head, pmem, link);
+		pmem->area = NULL;
+		pmem->pgidx = INVALID_PGIDX;
+		tee_pager_npages++;
+		set_npages();
+		TAILQ_INSERT_HEAD(&tee_pager_pmem_head, pmem, link);
+		incr_zi_released();
+		return true;
+	}
+
+	return false;
+}
+
+/* Finds the oldest page and unmats it from its old virtual address */
+static struct tee_pager_pmem *tee_pager_get_page(struct tee_pager_area *area)
+{
+	struct tee_pager_pmem *pmem;
+
+	pmem = TAILQ_FIRST(&tee_pager_pmem_head);
+	if (!pmem) {
+		EMSG("No pmem entries");
+		return NULL;
+	}
+	if (pmem->pgidx != INVALID_PGIDX) {
+		uint32_t a;
+
+		assert(pmem->area && pmem->area->pgt);
+		area_get_entry(pmem->area, pmem->pgidx, NULL, &a);
+		area_set_entry(pmem->area, pmem->pgidx, 0, 0);
+		pgt_dec_used_entries(pmem->area->pgt);
+		/* TODO only invalidate entries touched above */
+		core_tlb_maintenance(TLBINV_UNIFIEDTLB, 0);
+		tee_pager_save_page(pmem, a);
+	}
+
+	TAILQ_REMOVE(&tee_pager_pmem_head, pmem, link);
+	pmem->pgidx = INVALID_PGIDX;
+	pmem->area = NULL;
+	if (area->type == AREA_TYPE_LOCK) {
+		/* Move page to lock list */
+		if (tee_pager_npages <= 0)
+			panic("running out of page");
+		tee_pager_npages--;
+		set_npages();
+		TAILQ_INSERT_TAIL(&tee_pager_lock_pmem_head, pmem, link);
+	} else {
+		/* move page to back */
+		TAILQ_INSERT_TAIL(&tee_pager_pmem_head, pmem, link);
+	}
+
+	return pmem;
+}
+
+static bool pager_update_permissions(struct tee_pager_area *area,
+			struct abort_info *ai, bool *handled)
+{
+	unsigned int pgidx = area_va2idx(area, ai->va);
+	uint32_t attr;
+	paddr_t pa;
+
+	*handled = false;
+
+	area_get_entry(area, pgidx, &pa, &attr);
+
+	/* Not mapped */
+	if (!(attr & TEE_MATTR_VALID_BLOCK))
+		return false;
+
+	/* Not readable, should not happen */
+	if (abort_is_user_exception(ai)) {
+		if (!(attr & TEE_MATTR_UR))
+			return true;
+	} else {
+		if (!(attr & TEE_MATTR_PR)) {
+			abort_print_error(ai);
+			panic();
+		}
+	}
+
+	switch (core_mmu_get_fault_type(ai->fault_descr)) {
+	case CORE_MMU_FAULT_TRANSLATION:
+	case CORE_MMU_FAULT_READ_PERMISSION:
+		if (ai->abort_type == ABORT_TYPE_PREFETCH) {
+			/* Check attempting to execute from an NOX page */
+			if (abort_is_user_exception(ai)) {
+				if (!(attr & TEE_MATTR_UX))
+					return true;
+			} else {
+				if (!(attr & TEE_MATTR_PX)) {
+					abort_print_error(ai);
+					panic();
+				}
+			}
+		}
+		/* Since the page is mapped now it's OK */
+		break;
+	case CORE_MMU_FAULT_WRITE_PERMISSION:
+		/* Check attempting to write to an RO page */
+		if (abort_is_user_exception(ai)) {
+			if (!(area->flags & TEE_MATTR_UW))
+				return true;
+			if (!(attr & TEE_MATTR_UW)) {
+				FMSG("Dirty %p",
+				     (void *)(ai->va & ~SMALL_PAGE_MASK));
+				area_set_entry(area, pgidx, pa,
+					       get_area_mattr(area->flags));
+				/* TODO only invalidate entry above */
+				core_tlb_maintenance(TLBINV_UNIFIEDTLB, 0);
+			}
+
+		} else {
+			if (!(area->flags & TEE_MATTR_PW)) {
+				abort_print_error(ai);
+				panic();
+			}
+			if (!(attr & TEE_MATTR_PW)) {
+				FMSG("Dirty %p",
+				     (void *)(ai->va & ~SMALL_PAGE_MASK));
+				area_set_entry(area, pgidx, pa,
+					       get_area_mattr(area->flags));
+				/* TODO only invalidate entry above */
+				core_tlb_maintenance(TLBINV_UNIFIEDTLB, 0);
+			}
+		}
+		/* Since permissions has been updated now it's OK */
+		break;
+	default:
+		/* Some fault we can't deal with */
+		if (abort_is_user_exception(ai))
+			return true;
+		abort_print_error(ai);
+		panic();
+	}
+	*handled = true;
+	return true;
+}
+
+#ifdef CFG_TEE_CORE_DEBUG
+static void stat_handle_fault(void)
+{
+	static size_t num_faults;
+	static size_t min_npages = SIZE_MAX;
+	static size_t total_min_npages = SIZE_MAX;
+
+	num_faults++;
+	if ((num_faults % 1024) == 0 || tee_pager_npages < total_min_npages) {
+		DMSG("nfaults %zu npages %zu (min %zu)",
+		     num_faults, tee_pager_npages, min_npages);
+		min_npages = tee_pager_npages; /* reset */
+	}
+	if (tee_pager_npages < min_npages)
+		min_npages = tee_pager_npages;
+	if (tee_pager_npages < total_min_npages)
+		total_min_npages = tee_pager_npages;
+}
+#else
+static void stat_handle_fault(void)
+{
+}
+#endif
+
+bool tee_pager_handle_fault(struct abort_info *ai)
+{
+	struct tee_pager_area *area;
+	vaddr_t page_va = ai->va & ~SMALL_PAGE_MASK;
+	uint32_t exceptions;
+	bool ret;
+
+#ifdef TEE_PAGER_DEBUG_PRINT
+	abort_print(ai);
+#endif
+
+	/*
+	 * We're updating pages that can affect several active CPUs at a
+	 * time below. We end up here because a thread tries to access some
+	 * memory that isn't available. We have to be careful when making
+	 * that memory available as other threads may succeed in accessing
+	 * that address the moment after we've made it available.
+	 *
+	 * That means that we can't just map the memory and populate the
+	 * page, instead we use the aliased mapping to populate the page
+	 * and once everything is ready we map it.
+	 */
+	exceptions = pager_lock();
+
+	stat_handle_fault();
+
+	/* check if the access is valid */
+	if (abort_is_user_exception(ai)) {
+		area = find_uta_area(ai->va);
+
+	} else {
+		area = find_area(&tee_pager_area_head, ai->va);
+		if (!area)
+			area = find_uta_area(ai->va);
+	}
+	if (!area || !area->pgt) {
+		ret = false;
+		goto out;
+	}
+
+	if (!tee_pager_unhide_page(page_va)) {
+		struct tee_pager_pmem *pmem = NULL;
+		uint32_t attr;
+
+		/*
+		 * The page wasn't hidden, but some other core may have
+		 * updated the table entry before we got here or we need
+		 * to make a read-only page read-write (dirty).
+		 */
+		if (pager_update_permissions(area, ai, &ret)) {
+			/*
+			 * Nothing more to do with the abort. The problem
+			 * could already have been dealt with from another
+			 * core or if ret is false the TA will be paniced.
+			 */
+			goto out;
+		}
+
+		pmem = tee_pager_get_page(area);
+		if (!pmem) {
+			abort_print(ai);
+			panic();
+		}
+
+		/* load page code & data */
+		tee_pager_load_page(area, page_va, pmem->va_alias);
+
+		/*
+		 * We've updated the page using the aliased mapping and
+		 * some cache maintenence is now needed if it's an
+		 * executable page.
+		 *
+		 * Since the d-cache is a Physically-indexed,
+		 * physically-tagged (PIPT) cache we can clean the aliased
+		 * address instead of the real virtual address.
+		 *
+		 * The i-cache can also be PIPT, but may be something else
+		 * to, to keep it simple we invalidate the entire i-cache.
+		 * As a future optimization we may invalidate only the
+		 * aliased area if it a PIPT cache else the entire cache.
+		 */
+		if (area->flags & (TEE_MATTR_PX | TEE_MATTR_UX)) {
+			/*
+			 * 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);
+		}
+
+		pmem->area = area;
+		pmem->pgidx = area_va2idx(area, ai->va);
+		attr = get_area_mattr(area->flags) &
+			~(TEE_MATTR_PW | TEE_MATTR_UW);
+		area_set_entry(area, pmem->pgidx, get_pmem_pa(pmem), attr);
+		pgt_inc_used_entries(area->pgt);
+
+		FMSG("Mapped 0x%" PRIxVA " -> 0x%" PRIxPA,
+		     area_idx2va(area, pmem->pgidx), get_pmem_pa(pmem));
+
+	}
+
+	tee_pager_hide_pages();
+	ret = true;
+out:
+	pager_unlock(exceptions);
+	return ret;
+}
+
+void tee_pager_add_pages(vaddr_t vaddr, size_t npages, bool unmap)
+{
+	struct core_mmu_table_info *ti = &tee_pager_tbl_info;
+	size_t n;
+
+	DMSG("0x%" PRIxVA " - 0x%" PRIxVA " : %d",
+	     vaddr, vaddr + npages * SMALL_PAGE_SIZE, (int)unmap);
+
+	/* setup memory */
+	for (n = 0; n < npages; n++) {
+		struct tee_pager_pmem *pmem;
+		vaddr_t va = vaddr + n * SMALL_PAGE_SIZE;
+		unsigned pgidx = core_mmu_va2idx(ti, va);
+		paddr_t pa;
+		uint32_t attr;
+
+		/*
+		 * Note that we can only support adding pages in the
+		 * valid range of this table info, currently not a problem.
+		 */
+		core_mmu_get_entry(ti, pgidx, &pa, &attr);
+
+		/* Ignore unmapped pages/blocks */
+		if (!(attr & TEE_MATTR_VALID_BLOCK))
+			continue;
+
+		pmem = malloc(sizeof(struct tee_pager_pmem));
+		if (!pmem)
+			panic("out of mem");
+
+		pmem->va_alias = pager_add_alias_page(pa);
+
+		if (unmap) {
+			pmem->area = NULL;
+			pmem->pgidx = INVALID_PGIDX;
+			core_mmu_set_entry(ti, pgidx, 0, 0);
+			pgt_dec_used_entries(&pager_core_pgt);
+		} else {
+			/*
+			 * The page is still mapped, let's assign the area
+			 * and update the protection bits accordingly.
+			 */
+			pmem->area = find_area(&tee_pager_area_head, va);
+			assert(pmem->area->pgt == &pager_core_pgt);
+			pmem->pgidx = pgidx;
+			assert(pa == get_pmem_pa(pmem));
+			area_set_entry(pmem->area, pgidx, pa,
+				       get_area_mattr(pmem->area->flags));
+		}
+
+		tee_pager_npages++;
+		incr_npages_all();
+		set_npages();
+		TAILQ_INSERT_TAIL(&tee_pager_pmem_head, pmem, link);
+	}
+
+	/* Invalidate secure TLB */
+	core_tlb_maintenance(TLBINV_UNIFIEDTLB, 0);
+}
+
+#ifdef CFG_PAGED_USER_TA
+static struct pgt *find_pgt(struct pgt *pgt, vaddr_t va)
+{
+	struct pgt *p = pgt;
+
+	while (p && (va & ~CORE_MMU_PGDIR_MASK) != p->vabase)
+		p = SLIST_NEXT(p, link);
+	return p;
+}
+
+void tee_pager_assign_uta_tables(struct user_ta_ctx *utc)
+{
+	struct tee_pager_area *area;
+	struct pgt *pgt = SLIST_FIRST(&thread_get_tsd()->pgt_cache);
+
+	TAILQ_FOREACH(area, utc->areas, link) {
+		if (!area->pgt)
+			area->pgt = find_pgt(pgt, area->base);
+		else
+			assert(area->pgt == find_pgt(pgt, area->base));
+		if (!area->pgt)
+			panic();
+	}
+}
+
+static void pager_save_and_release_entry(struct tee_pager_pmem *pmem)
+{
+	uint32_t attr;
+
+	assert(pmem->area && pmem->area->pgt);
+
+	area_get_entry(pmem->area, pmem->pgidx, NULL, &attr);
+	area_set_entry(pmem->area, pmem->pgidx, 0, 0);
+	tee_pager_save_page(pmem, attr);
+	assert(pmem->area->pgt->num_used_entries);
+	pmem->area->pgt->num_used_entries--;
+	pmem->pgidx = INVALID_PGIDX;
+	pmem->area = NULL;
+}
+
+void tee_pager_pgt_save_and_release_entries(struct pgt *pgt)
+{
+	struct tee_pager_pmem *pmem;
+	struct tee_pager_area *area;
+	uint32_t exceptions = pager_lock();
+
+	if (!pgt->num_used_entries)
+		goto out;
+
+	TAILQ_FOREACH(pmem, &tee_pager_pmem_head, link) {
+		if (!pmem->area || pmem->pgidx == INVALID_PGIDX)
+			continue;
+		if (pmem->area->pgt == pgt)
+			pager_save_and_release_entry(pmem);
+	}
+	assert(!pgt->num_used_entries);
+
+out:
+	if (is_user_ta_ctx(pgt->ctx)) {
+		TAILQ_FOREACH(area, to_user_ta_ctx(pgt->ctx)->areas, link) {
+			if (area->pgt == pgt)
+				area->pgt = NULL;
+		}
+	}
+
+	pager_unlock(exceptions);
+}
+KEEP_PAGER(tee_pager_pgt_save_and_release_entries);
+#endif /*CFG_PAGED_USER_TA*/
+
+void tee_pager_release_phys(void *addr, size_t size)
+{
+	bool unmaped = false;
+	vaddr_t va = (vaddr_t)addr;
+	vaddr_t begin = ROUNDUP(va, SMALL_PAGE_SIZE);
+	vaddr_t end = ROUNDDOWN(va + size, SMALL_PAGE_SIZE);
+	struct tee_pager_area *area;
+	uint32_t exceptions;
+
+	if (!size)
+		return;
+
+	area = find_area(&tee_pager_area_head, begin);
+	if (!area ||
+	    area != find_area(&tee_pager_area_head, end - SMALL_PAGE_SIZE))
+		panic();
+
+	exceptions = pager_lock();
+
+	for (va = begin; va < end; va += SMALL_PAGE_SIZE)
+		unmaped |= tee_pager_release_one_phys(area, va);
+
+	/* Invalidate secure TLB */
+	if (unmaped)
+		core_tlb_maintenance(TLBINV_UNIFIEDTLB, 0);
+
+	pager_unlock(exceptions);
+}
+KEEP_PAGER(tee_pager_release_phys);
+
+void *tee_pager_alloc(size_t size, uint32_t flags)
+{
+	tee_mm_entry_t *mm;
+	uint32_t f = TEE_MATTR_PW | TEE_MATTR_PR | (flags & TEE_MATTR_LOCKED);
+
+	if (!size)
+		return NULL;
+
+	mm = tee_mm_alloc(&tee_mm_vcore, ROUNDUP(size, SMALL_PAGE_SIZE));
+	if (!mm)
+		return NULL;
+
+	tee_pager_add_core_area(tee_mm_get_smem(mm), tee_mm_get_bytes(mm),
+				f, NULL, NULL);
+
+	return (void *)tee_mm_get_smem(mm);
+}
diff --git a/core/arch/arm/plat-d02/conf.mk b/core/arch/arm/plat-d02/conf.mk
new file mode 100644
index 0000000..06a573c
--- /dev/null
+++ b/core/arch/arm/plat-d02/conf.mk
@@ -0,0 +1,34 @@
+CFG_NUM_THREADS ?= 16
+CFG_CRYPTO_WITH_CE ?= y
+CFG_WITH_STACK_CANARIES ?= y
+CFG_WITH_SOFTWARE_PRNG ?= n
+# Override default size of emulated TrustZone protected SRAM, 384 kB
+CFG_CORE_TZSRAM_EMUL_SIZE ?= 393216
+# Overrides default in mk/config.mk with 96 kB
+CFG_CORE_HEAP_SIZE ?= 98304
+
+$(call force,CFG_GENERIC_BOOT,y)
+$(call force,CFG_HWSUPP_MEM_PERM_PXN,y)
+$(call force,CFG_HI16XX_UART,y)
+$(call force,CFG_PM_STUBS,y)
+$(call force,CFG_SECURE_TIME_SOURCE_CNTPCT,y)
+$(call force,CFG_WITH_ARM_TRUSTED_FW,y)
+ifneq ($(CFG_WITH_SOFTWARE_PRNG),y)
+$(call force,CFG_HI16XX_RNG,y)
+endif
+
+# 32-bit flags
+arm32-platform-cpuarch		:= cortex-a57
+arm32-platform-cflags		+= -mcpu=$(arm32-platform-cpuarch)
+arm32-platform-aflags		+= -mcpu=$(arm32-platform-cpuarch)
+core_arm32-platform-aflags	+= -mfpu=neon
+
+ta-targets = ta_arm32
+
+ifeq ($(CFG_ARM64_core),y)
+$(call force,CFG_WITH_LPAE,y)
+ta-targets += ta_arm64
+else
+$(call force,CFG_ARM32_core,y)
+endif
+
diff --git a/core/arch/arm/plat-d02/kern.ld.S b/core/arch/arm/plat-d02/kern.ld.S
new file mode 100644
index 0000000..8d794ee
--- /dev/null
+++ b/core/arch/arm/plat-d02/kern.ld.S
@@ -0,0 +1 @@
+#include "../kernel/kern.ld.S"
diff --git a/core/arch/arm/plat-d02/link.mk b/core/arch/arm/plat-d02/link.mk
new file mode 100644
index 0000000..448ab89
--- /dev/null
+++ b/core/arch/arm/plat-d02/link.mk
@@ -0,0 +1 @@
+include core/arch/arm/kernel/link.mk
diff --git a/core/arch/arm/plat-d02/main.c b/core/arch/arm/plat-d02/main.c
new file mode 100644
index 0000000..95161d1
--- /dev/null
+++ b/core/arch/arm/plat-d02/main.c
@@ -0,0 +1,96 @@
+/*
+ * 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 <console.h>
+#include <drivers/hi16xx_uart.h>
+#include <kernel/generic_boot.h>
+#include <kernel/panic.h>
+#include <kernel/pm_stubs.h>
+#include <mm/tee_pager.h>
+#include <mm/core_memprot.h>
+#include <platform_config.h>
+#include <stdint.h>
+#include <tee/entry_std.h>
+#include <tee/entry_fast.h>
+
+static void main_fiq(void);
+
+static const struct thread_handlers handlers = {
+	.std_smc = tee_entry_std,
+	.fast_smc = tee_entry_fast,
+	.fiq = 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,
+};
+
+register_phys_mem(MEM_AREA_IO_NSEC, CONSOLE_UART_BASE, HI16XX_UART_REG_SIZE);
+
+const struct thread_handlers *generic_boot_get_handlers(void)
+{
+	return &handlers;
+}
+
+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());
+}
diff --git a/core/arch/arm/plat-d02/platform_config.h b/core/arch/arm/plat-d02/platform_config.h
new file mode 100644
index 0000000..fffa3d1
--- /dev/null
+++ b/core/arch/arm/plat-d02/platform_config.h
@@ -0,0 +1,137 @@
+/*
+ * 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.
+ */
+
+#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 */
+
+/* UART */
+#define PERI_SUB_CTRL_ADDR	0x80000000
+#define CONSOLE_UART_BASE       (PERI_SUB_CTRL_ADDR + 0x00300000)
+#define CONSOLE_BAUDRATE	115200
+#define CONSOLE_UART_CLK_IN_HZ	200000000
+
+/* ALG sub-controller */
+#define ALG_SC_BASE		0xD0000000
+#define ALG_SC_REG_SIZE		0xF010
+
+/* RNG */
+#define RNG_BASE		0xD1010000
+#define RNG_REG_SIZE		0x18
+
+/*
+ * HiSilicon D02 memory map
+ *
+ * Note: the physical address ranges below correspond to DRAM which is
+ * non-secure by default. Therefore, the terms TZDRAM and TZSRAM may not
+ * reflect the reality and only indicate areas that "would normally be"
+ * secure DRAM and secure SRAM in a more complete implementation.
+ * The memory map was defined like this for lack of better documentation.
+ * It is good enough for development/testing purposes.
+ *
+ * CFG_WITH_PAGER=n
+ *
+ *  0x7FC0_0000                                  -
+ *    Linux/other                                | DRAM1
+ *  0x5180_0000                                  -
+ *    TA RAM: 16 MiB                             |
+ *  0x5080_0000                                  | TZDRAM
+ *    TEE RAM: 4 MiB (CFG_TEE_RAM_VA_SIZE)       |
+ *  0x5040_0000 [TZDRAM_BASE, CFG_TEE_LOAD_ADDR] -
+ *    Shared memory: 4 MiB                       | SHMEM
+ *  0x5000_0000                                  -
+ *    Linux/other                                | DRAM0
+ *  0x0000_0000 [DRAM0_BASE]                     -
+ *
+ * CFG_WITH_PAGER=y
+ *
+ *  0x7FC0_0000                                  -
+ *    Linux/other                                | DRAM1
+ *  0x5180_0000                                  -
+ *    TA RAM: 20096 KiB (TZDRAM_SIZE)            | TZDRAM
+ *  0x5046_0000                                  -
+ *    TEE RAM: 384 KiB (TZSRAM_SIZE)             | TZSRAM
+ *  0x5040_0000 [TZSRAM_BASE, CFG_TEE_LOAD_ADDR] -
+ *    Shared memory: 4 MiB                       | SHMEM
+ *  0x5000_0000                                  -
+ *    Linux/other                                | DRAM0
+ *  0x0000_0000 [DRAM0_BASE]                     -
+ */
+
+#define DRAM0_BASE		0x00000000
+#define DRAM0_SIZE		0x50000000
+
+#define DRAM1_BASE		0x51800000
+#define DRAM1_SIZE		0x2E400000
+
+#ifdef CFG_WITH_PAGER
+
+#define TZSRAM_BASE		0x50400000
+#define TZSRAM_SIZE		CFG_CORE_TZSRAM_EMUL_SIZE
+
+#define TZDRAM_BASE		0x50460000
+#define TZDRAM_SIZE		(20096 * 1024)
+
+#define CFG_TEE_RAM_START	TZSRAM_BASE
+#define CFG_TEE_RAM_PH_SIZE	TZSRAM_SIZE
+#define CFG_TA_RAM_START	ROUNDUP(TZDRAM_BASE, CORE_MMU_DEVICE_SIZE)
+#define CFG_TA_RAM_SIZE		ROUNDDOWN(TZDRAM_SIZE, CORE_MMU_DEVICE_SIZE)
+
+#define CFG_TEE_RAM_VA_SIZE	(1 * 1024 * 1024)
+
+#else /* CFG_WITH_PAGER */
+
+#define TZDRAM_BASE		0x50400000
+#define TZDRAM_SIZE		(20 * 1024 * 1024)
+
+#define CFG_TEE_RAM_START	TZDRAM_BASE
+#define CFG_TEE_RAM_PH_SIZE	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),\
+					  CORE_MMU_DEVICE_SIZE)
+
+#define CFG_TEE_RAM_VA_SIZE	(4 * 1024 * 1024)
+
+#endif /* CFG_WITH_PAGER */
+
+#define CFG_SHMEM_START		0x50000000
+#define CFG_SHMEM_SIZE		(4 * 1024 * 1024)
+
+#define CFG_TEE_CORE_NB_CORE	16
+
+#define CFG_TEE_LOAD_ADDR	0x50400000
+
+#endif /* PLATFORM_CONFIG_H */
diff --git a/core/arch/arm/plat-d02/sub.mk b/core/arch/arm/plat-d02/sub.mk
new file mode 100644
index 0000000..8ddc2fd
--- /dev/null
+++ b/core/arch/arm/plat-d02/sub.mk
@@ -0,0 +1,2 @@
+global-incdirs-y += .
+srcs-y += main.c
diff --git a/core/arch/arm/plat-hikey/conf.mk b/core/arch/arm/plat-hikey/conf.mk
new file mode 100644
index 0000000..8a8ef2f
--- /dev/null
+++ b/core/arch/arm/plat-hikey/conf.mk
@@ -0,0 +1,42 @@
+# 32-bit flags
+arm32-platform-cpuarch		:= cortex-a53
+arm32-platform-cflags		+= -mcpu=$(arm32-platform-cpuarch)
+arm32-platform-aflags		+= -mcpu=$(arm32-platform-cpuarch)
+core_arm32-platform-aflags	+= -mfpu=neon
+
+$(call force,CFG_GENERIC_BOOT,y)
+$(call force,CFG_HWSUPP_MEM_PERM_PXN,y)
+$(call force,CFG_PL011,y)
+$(call force,CFG_PM_STUBS,y)
+$(call force,CFG_SECURE_TIME_SOURCE_CNTPCT,y)
+$(call force,CFG_WITH_ARM_TRUSTED_FW,y)
+
+ta-targets = ta_arm32
+
+ifeq ($(CFG_ARM64_core),y)
+$(call force,CFG_WITH_LPAE,y)
+ta-targets += ta_arm64
+else
+$(call force,CFG_ARM32_core,y)
+endif
+
+CFG_NUM_THREADS ?= 8
+CFG_CRYPTO_WITH_CE ?= y
+CFG_WITH_STACK_CANARIES ?= y
+
+CFG_PL061 ?= y
+CFG_PL022 ?= y
+CFG_SPI ?= y
+
+ifeq ($(CFG_SPI_TEST),y)
+$(call force,CFG_SPI,y)
+endif
+
+ifeq ($(CFG_SPI),y)
+$(call force,CFG_PL061,y)
+$(call force,CFG_PL022,y)
+endif
+
+ifeq ($(CFG_PL061),y)
+core-platform-cppflags		+= -DPLAT_PL061_MAX_GPIOS=160
+endif
diff --git a/core/arch/arm/plat-hikey/hikey_peripherals.h b/core/arch/arm/plat-hikey/hikey_peripherals.h
new file mode 100644
index 0000000..2d4500c
--- /dev/null
+++ b/core/arch/arm/plat-hikey/hikey_peripherals.h
@@ -0,0 +1,97 @@
+/*
+ * Copyright (c) 2016, Linaro Ltd and Contributors. All rights reserved.
+ * Copyright (c) 2016, Hisilicon Ltd and Contributors. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * 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.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE 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 __HIKEY_PERIPHERALS_H__
+#define __HIKEY_PERIPHERALS_H__
+
+#include <types_ext.h>
+
+#define PMUSSI_BASE	0xF8000000
+#define PERI_BASE	0xF7030000
+#define PMX0_BASE	0xF7010000
+#define PMX1_BASE	0xF7010800
+#define GPIO6_BASE	0xF7022000
+#define SPI_BASE	0xF7106000
+
+#define PMUSSI_REG_SIZE		0x1000
+#define PERI_BASE_REG_SIZE	0x2000
+#define PMX0_REG_SIZE		0x27c
+#define PMX1_REG_SIZE		0x28c
+
+/* register offsets */
+#define PMUSSI_LDO21_REG_ADJ	SHIFT_U32(0x86, 2)
+#define PMUSSI_ENA_LDO17_22	SHIFT_U32(0x2F, 2)
+
+#define PERI_SC_PERIPH_RSTDIS3	0x334
+#define PERI_SC_PERIPH_RSTSTAT3	0x338
+#define PERI_SC_PERIPH_CLKEN3	0x230
+#define PERI_SC_PERIPH_CLKSTAT3	0x238
+
+#define PMX0_IOMG104	0x1a0
+#define PMX0_IOMG105	0x1a4
+#define PMX0_IOMG106	0x1a8
+#define PMX0_IOMG107	0x1ac
+
+#define PMX1_IOCG104	0x1b0
+#define PMX1_IOCG105	0x1b4
+#define PMX1_IOCG106	0x1b8
+#define PMX1_IOCG107	0x1bc
+/* end register offsets */
+
+#define PMUSSI_LDO21_REG_VL_MASK	0x7
+#define PMUSSI_LDO21_REG_VL_1V8		0x3
+#define PMUSSI_ENA_LDO21		BIT(4)
+
+#define PERI_RST3_SSP	BIT(9)
+#define PERI_CLK3_SSP	BIT(9)
+
+#define PINMUX_GPIO	0
+#define PINMUX_SPI	1
+
+#define PINCFG_NOPULL	0
+#define PINCFG_PULLUP	1
+#define PINCFG_PULLDN	2
+
+#define GPIO6_2		50
+#define SPI_CLK_HZ	150000000 /* 150mhz */
+#define SPI_500_KHZ	500000
+#define SPI_10_KHZ	10000
+
+vaddr_t nsec_periph_base(paddr_t pa);
+
+#ifdef CFG_SPI
+void spi_init(void);
+#ifdef CFG_SPI_TEST
+void spi_test(void);
+#endif /* CFG_SPI_TEST */
+#endif /* CFG_SPI */
+
+#endif /* __HIKEY_PERIPHERALS_H__ */
diff --git a/core/arch/arm/plat-hikey/kern.ld.S b/core/arch/arm/plat-hikey/kern.ld.S
new file mode 100644
index 0000000..8d794ee
--- /dev/null
+++ b/core/arch/arm/plat-hikey/kern.ld.S
@@ -0,0 +1 @@
+#include "../kernel/kern.ld.S"
diff --git a/core/arch/arm/plat-hikey/link.mk b/core/arch/arm/plat-hikey/link.mk
new file mode 100644
index 0000000..448ab89
--- /dev/null
+++ b/core/arch/arm/plat-hikey/link.mk
@@ -0,0 +1 @@
+include core/arch/arm/kernel/link.mk
diff --git a/core/arch/arm/plat-hikey/main.c b/core/arch/arm/plat-hikey/main.c
new file mode 100644
index 0000000..36789ce
--- /dev/null
+++ b/core/arch/arm/plat-hikey/main.c
@@ -0,0 +1,207 @@
+/*
+ * 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 <console.h>
+#include <drivers/pl011.h>
+#ifdef CFG_SPI
+#include <drivers/pl022_spi.h>
+#include <drivers/pl061_gpio.h>
+#endif
+#include <hikey_peripherals.h>
+#include <initcall.h>
+#include <io.h>
+#include <kernel/generic_boot.h>
+#include <kernel/panic.h>
+#include <kernel/pm_stubs.h>
+#include <mm/tee_pager.h>
+#include <mm/core_memprot.h>
+#include <platform_config.h>
+#include <stdint.h>
+#include <tee/entry_std.h>
+#include <tee/entry_fast.h>
+
+static void main_fiq(void);
+
+static const struct thread_handlers handlers = {
+	.std_smc = tee_entry_std,
+	.fast_smc = tee_entry_fast,
+	.fiq = 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,
+};
+
+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
+register_phys_mem(MEM_AREA_IO_NSEC, PERI_BASE, PERI_BASE_REG_SIZE);
+register_phys_mem(MEM_AREA_IO_NSEC, PMX0_BASE, PMX0_REG_SIZE);
+register_phys_mem(MEM_AREA_IO_NSEC, PMX1_BASE, PMX1_REG_SIZE);
+register_phys_mem(MEM_AREA_IO_NSEC, GPIO6_BASE, PL061_REG_SIZE);
+register_phys_mem(MEM_AREA_IO_NSEC, SPI_BASE, PL022_REG_SIZE);
+#endif
+
+const struct thread_handlers *generic_boot_get_handlers(void)
+{
+	return &handlers;
+}
+
+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());
+}
+
+vaddr_t nsec_periph_base(paddr_t pa)
+{
+	if (cpu_mmu_enabled())
+		return (vaddr_t)phys_to_virt(pa, MEM_AREA_IO_NSEC);
+	return (vaddr_t)pa;
+}
+
+#ifdef CFG_SPI
+void spi_init(void)
+{
+	uint32_t shifted_val, read_val;
+	vaddr_t peri_base = nsec_periph_base(PERI_BASE);
+	vaddr_t pmx0_base = nsec_periph_base(PMX0_BASE);
+	vaddr_t pmx1_base = nsec_periph_base(PMX1_BASE);
+
+	DMSG("take SPI0 out of reset\n");
+	shifted_val = PERI_RST3_SSP;
+	/*
+	 * no need to read PERI_SC_PERIPH_RSTDIS3 first
+	 * as all the bits are processed and cleared after writing
+	 */
+	write32(shifted_val, peri_base + PERI_SC_PERIPH_RSTDIS3);
+	DMSG("PERI_SC_PERIPH_RSTDIS3: 0x%x\n",
+		read32(peri_base + PERI_SC_PERIPH_RSTDIS3));
+
+	/*
+	 * wait until the requested device is out of reset
+	 * and ready to be used
+	 */
+	do {
+		read_val = read32(peri_base + PERI_SC_PERIPH_RSTSTAT3);
+	} while (read_val & shifted_val);
+	DMSG("PERI_SC_PERIPH_RSTSTAT3: 0x%x\n", read_val);
+
+	DMSG("enable SPI clock\n");
+	/*
+	 * no need to read PERI_SC_PERIPH_CLKEN3 first
+	 * as all the bits are processed and cleared after writing
+	 */
+	shifted_val = PERI_CLK3_SSP;
+	write32(shifted_val, peri_base + PERI_SC_PERIPH_CLKEN3);
+	DMSG("PERI_SC_PERIPH_CLKEN3: 0x%x\n",
+		read32(peri_base + PERI_SC_PERIPH_CLKEN3));
+
+	DMSG("PERI_SC_PERIPH_CLKSTAT3: 0x%x\n",
+		read32(peri_base + PERI_SC_PERIPH_CLKSTAT3));
+
+	/*
+	 * GPIO6_2 can be configured as PINMUX_GPIO, but as PINMUX_SPI, HW IP
+	 * will control the chip select pin so we don't have to manually do it.
+	 * The only concern is that the IP will pulse it between each packet,
+	 * which might not work with certain clients. There seems to be no
+	 * option to configure it to stay enabled for the total duration of the
+	 * transfer.
+	 * ref: http://infocenter.arm.com/help/topic/com.arm.doc.ddi0194h/CJACFAFG.html
+	 */
+	DMSG("configure gpio6 pins 0-3 as SPI\n");
+	write32(PINMUX_SPI, pmx0_base + PMX0_IOMG104);
+	write32(PINMUX_SPI, pmx0_base + PMX0_IOMG105);
+	write32(PINMUX_SPI, pmx0_base + PMX0_IOMG106);
+	write32(PINMUX_SPI, pmx0_base + PMX0_IOMG107);
+
+	DMSG("configure gpio6 pins 0-3 as nopull\n");
+	write32(PINCFG_NOPULL, pmx1_base + PMX1_IOCG104);
+	write32(PINCFG_NOPULL, pmx1_base + PMX1_IOCG105);
+	write32(PINCFG_NOPULL, pmx1_base + PMX1_IOCG106);
+	write32(PINCFG_NOPULL, pmx1_base + PMX1_IOCG107);
+
+#ifdef CFG_SPI_TEST
+	spi_test();
+#endif
+}
+#endif
+
+static TEE_Result peripherals_init(void)
+{
+	vaddr_t pmussi_base = nsec_periph_base(PMUSSI_BASE);
+
+	DMSG("enable LD021_1V8 source (pin 35) on LS connector\n");
+	/*
+	 * Mezzanine cards usually use this to source level shifters for
+	 * UART, GPIO, SPI, I2C, etc so if not enabled, connected
+	 * peripherals will not work either (during bootloader stage)
+	 * until linux is booted.
+	 */
+	io_mask8(pmussi_base + PMUSSI_LDO21_REG_ADJ, PMUSSI_LDO21_REG_VL_1V8,
+		PMUSSI_LDO21_REG_VL_MASK);
+	write8(PMUSSI_ENA_LDO21, pmussi_base + PMUSSI_ENA_LDO17_22);
+
+#ifdef CFG_SPI
+	spi_init();
+#endif
+	return TEE_SUCCESS;
+}
+
+driver_init(peripherals_init);
diff --git a/core/arch/arm/plat-hikey/platform_config.h b/core/arch/arm/plat-hikey/platform_config.h
new file mode 100644
index 0000000..633759f
--- /dev/null
+++ b/core/arch/arm/plat-hikey/platform_config.h
@@ -0,0 +1,130 @@
+/*
+ * 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.
+ */
+
+#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 */
+
+/* PL011 UART */
+#if defined(CFG_CONSOLE_UART) && (CFG_CONSOLE_UART == 0)
+#define CONSOLE_UART_BASE       0xF8015000
+#elif !defined(CFG_CONSOLE_UART) || (CFG_CONSOLE_UART == 3)
+#define CONSOLE_UART_BASE       0xF7113000
+#else
+#error Unknown console UART
+#endif
+
+#define CONSOLE_BAUDRATE	115200
+#define CONSOLE_UART_CLK_IN_HZ	19200000
+
+/*
+ * HiKey memory map
+ *
+ * We use only non-secure DRAM (TZDRAM and TZSRAM are emulated).
+ *
+ * CFG_WITH_PAGER=n
+ *
+ *  0x4000_0000                               -
+ *    TA RAM: 15 MiB                          |
+ *  0x3F10_0000                               | TZDRAM
+ *    TEE RAM: 1 MiB (CFG_TEE_RAM_VA_SIZE)    |
+ *  0x3F00_0000 [TZDRAM_BASE, BL32_LOAD_ADDR] -
+ *    Shared memory: 1 MiB                    |
+ *  0x3EF0_0000                               | DRAM0
+ *    Available to Linux                      |
+ *  0x0000_0000 [DRAM0_BASE]                  -
+ *
+ * CFG_WITH_PAGER=y
+ *
+ *  0x4000_0000                               -
+ *    TA RAM: 15 MiB                          | TZDRAM
+ *  0x3F10_0000                               -
+ *    Unused
+ *  0x3F03_2000                               -
+ *    TEE RAM: 200 KiB                        | TZSRAM
+ *  0x3F00_0000 [TZSRAM_BASE, BL32_LOAD_ADDR] -
+ *    Shared memory: 1 MiB                    |
+ *  0x3EF0_0000                               | DRAM0
+ *    Available to Linux                      |
+ *  0x0000_0000 [DRAM0_BASE]                  -
+ */
+
+#define DRAM0_BASE		0x00000000
+#define DRAM0_SIZE		0x3F000000
+
+#ifdef CFG_WITH_PAGER
+
+#define TZSRAM_BASE		0x3F000000
+#define TZSRAM_SIZE		CFG_CORE_TZSRAM_EMUL_SIZE
+
+#define TZDRAM_BASE		0x3F100000
+#define TZDRAM_SIZE		(15 * 1024 * 1024)
+
+#else /* CFG_WITH_PAGER */
+
+#define TZDRAM_BASE		0x3F000000
+#define TZDRAM_SIZE		(16 * 1024 * 1024)
+
+#endif /* CFG_WITH_PAGER */
+
+#define CFG_SHMEM_START		0x3EE00000
+#define CFG_SHMEM_SIZE		(2 * 1024 * 1024)
+
+#define CFG_TEE_CORE_NB_CORE	8
+
+#define CFG_TEE_RAM_VA_SIZE	(1024 * 1024)
+
+#define CFG_TEE_LOAD_ADDR	0x3F000000
+
+#ifdef CFG_WITH_PAGER
+
+#define CFG_TEE_RAM_START	TZSRAM_BASE
+#define CFG_TEE_RAM_PH_SIZE	TZSRAM_SIZE
+#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 /* CFG_WITH_PAGER */
+
+#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		ROUNDDOWN((TZDRAM_SIZE - CFG_TEE_RAM_VA_SIZE),\
+					  CORE_MMU_DEVICE_SIZE)
+
+#endif /* CFG_WITH_PAGER */
+
+#endif /* PLATFORM_CONFIG_H */
diff --git a/core/arch/arm/plat-hikey/spi_test.c b/core/arch/arm/plat-hikey/spi_test.c
new file mode 100644
index 0000000..11843c0
--- /dev/null
+++ b/core/arch/arm/plat-hikey/spi_test.c
@@ -0,0 +1,292 @@
+/*
+ * Copyright (c) 2016, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <drivers/pl022_spi.h>
+#include <drivers/pl061_gpio.h>
+#include <hikey_peripherals.h>
+#include <io.h>
+#include <kernel/tee_time.h>
+#include <stdint.h>
+#include <trace.h>
+#include <util.h>
+
+#define PL022_STAT	0x00C
+#define PL022_STAT_BSY	SHIFT_U32(1, 4)
+
+static void spi_cs_callback(enum gpio_level value)
+{
+	static bool inited;
+	static struct pl061_data pd;
+	vaddr_t gpio6_base = nsec_periph_base(GPIO6_BASE);
+	vaddr_t spi_base = nsec_periph_base(SPI_BASE);
+
+	if (!inited) {
+		pl061_init(&pd);
+		pl061_register(gpio6_base, 6);
+		pl061_set_mode_control(GPIO6_2, PL061_MC_SW);
+		pd.chip.ops->set_interrupt(GPIO6_2, GPIO_INTERRUPT_DISABLE);
+		pd.chip.ops->set_direction(GPIO6_2, GPIO_DIR_OUT);
+		inited = true;
+	}
+
+	if (read8(spi_base + PL022_STAT) & PL022_STAT_BSY)
+		DMSG("pl022 busy - do NOT set CS!");
+	while (read8(spi_base + PL022_STAT) & PL022_STAT_BSY)
+		;
+	DMSG("pl022 done - set CS!");
+
+	pd.chip.ops->set_value(GPIO6_2, value);
+}
+
+static void spi_set_cs_mux(uint32_t val)
+{
+	uint32_t data;
+	vaddr_t pmx0_base = nsec_periph_base(PMX0_BASE);
+
+	if (val == PINMUX_SPI) {
+		DMSG("Configure gpio6 pin2 as SPI");
+		write32(PINMUX_SPI, pmx0_base + PMX0_IOMG106);
+	} else {
+		DMSG("Configure gpio6 pin2 as GPIO");
+		write32(PINMUX_GPIO, pmx0_base + PMX0_IOMG106);
+	}
+
+	data = read32(pmx0_base + PMX0_IOMG106);
+	if (data)
+		DMSG("gpio6 pin2 is SPI");
+	else
+		DMSG("gpio6 pin2 is GPIO");
+}
+
+static void spi_test_with_manual_cs_control(void)
+{
+	struct pl022_data pd;
+	vaddr_t spi_base = nsec_periph_base(SPI_BASE);
+	uint8_t tx[3] = {0x01, 0x80, 0x00};
+	uint8_t rx[3] = {0};
+	size_t i, j, len = 3;
+	enum spi_result res;
+
+	spi_set_cs_mux(PINMUX_GPIO);
+
+	DMSG("Set CS callback");
+	pd.cs_control = PL022_CS_CTRL_MANUAL;
+
+	DMSG("spi_base: 0x%" PRIxVA "\n", spi_base);
+	DMSG("Configure SPI");
+	pd.base = spi_base;
+	pd.clk_hz = SPI_CLK_HZ;
+	pd.speed_hz = SPI_10_KHZ;
+	pd.mode = SPI_MODE0;
+	pd.data_size_bits = 8;
+	pd.loopback = true;
+
+	pl022_init(&pd);
+	pd.chip.ops->configure(&pd.chip);
+	pd.chip.ops->start(&pd.chip);
+
+	/*
+	 * Pulse CS only once for the whole transmission.
+	 * This is the scheme used by the pl022 driver.
+	 */
+	spi_cs_callback(GPIO_LEVEL_HIGH);
+	tee_time_busy_wait(2);
+	spi_cs_callback(GPIO_LEVEL_LOW);
+	for (j = 0; j < 10; j++) {
+		DMSG("SPI test loop: %zu", j);
+		res = pd.chip.ops->txrx8(&pd.chip, tx, rx, len);
+		if (res) {
+			EMSG("SPI transceive error %d", res);
+			break;
+		}
+
+		for (i = 0; i < len; i++)
+			DMSG("rx[%zu] = 0x%x", i, rx[i]);
+
+		tee_time_busy_wait(20);
+	}
+	spi_cs_callback(GPIO_LEVEL_HIGH);
+
+	/* Pulse CS once per transfer */
+	spi_cs_callback(GPIO_LEVEL_HIGH);
+	tee_time_busy_wait(2);
+	for (j = 10; j < 20; j++) {
+		DMSG("SPI test loop: %zu", j);
+		spi_cs_callback(GPIO_LEVEL_LOW);
+		res = pd.chip.ops->txrx8(&pd.chip, tx, rx, len);
+		if (res) {
+			EMSG("SPI transceive error %d", res);
+			break;
+		}
+
+		for (i = 0; i < len; i++)
+			DMSG("rx[%zu] = 0x%x", i, rx[i]);
+
+		tee_time_busy_wait(20);
+		spi_cs_callback(GPIO_LEVEL_HIGH);
+	}
+
+	/* Pulse CS once per word/byte */
+	spi_set_cs_mux(PINMUX_SPI);
+	tee_time_busy_wait(2);
+	for (j = 20; j < 30; j++) {
+		DMSG("SPI test loop: %zu", j);
+		res = pd.chip.ops->txrx8(&pd.chip, tx, rx, len);
+		if (res) {
+			EMSG("SPI transceive error %d", res);
+			break;
+		}
+
+		for (i = 0; i < len; i++)
+			DMSG("rx[%zu] = 0x%x", i, rx[i]);
+
+		tee_time_busy_wait(20);
+	}
+
+	pd.chip.ops->end(&pd.chip);
+}
+
+static void spi_test_with_registered_cs_cb(void)
+{
+	struct pl022_data pd;
+	vaddr_t spi_base = nsec_periph_base(SPI_BASE);
+	uint8_t tx[3] = {0x01, 0x80, 0x00};
+	uint8_t rx[3] = {0};
+	size_t i, j, len = 3;
+	enum spi_result res;
+
+	spi_set_cs_mux(PINMUX_GPIO);
+
+	DMSG("Set CS callback");
+	pd.cs_data.cs_cb = spi_cs_callback;
+	pd.cs_control = PL022_CS_CTRL_CB;
+
+	DMSG("spi_base: 0x%" PRIxVA "\n", spi_base);
+	DMSG("Configure SPI");
+	pd.base = spi_base;
+	pd.clk_hz = SPI_CLK_HZ;
+	pd.speed_hz = SPI_10_KHZ;
+	pd.mode = SPI_MODE0;
+	pd.data_size_bits = 8;
+	pd.loopback = true;
+
+	pl022_init(&pd);
+	pd.chip.ops->configure(&pd.chip);
+	pd.chip.ops->start(&pd.chip);
+
+	for (j = 0; j < 20; j++) {
+		DMSG("SPI test loop: %zu", j);
+		res = pd.chip.ops->txrx8(&pd.chip, tx, rx, len);
+		if (res) {
+			EMSG("SPI transceive error %d", res);
+			break;
+		}
+
+		for (i = 0; i < len; i++)
+			DMSG("rx[%zu] = 0x%x", i, rx[i]);
+
+		tee_time_busy_wait(20);
+	}
+
+	pd.chip.ops->end(&pd.chip);
+}
+
+static void spi_test_with_builtin_cs_control(void)
+{
+	struct pl061_data pd061;
+	struct pl022_data pd022;
+	vaddr_t gpio6_base = nsec_periph_base(GPIO6_BASE);
+	vaddr_t spi_base = nsec_periph_base(SPI_BASE);
+	uint8_t tx[3] = {0x01, 0x80, 0x00};
+	uint8_t rx[3] = {0};
+	size_t i, j, len = 3;
+	enum spi_result res;
+
+	spi_set_cs_mux(PINMUX_GPIO);
+
+	DMSG("gpio6_base: 0x%" PRIxVA "\n", gpio6_base);
+	DMSG("Configure GPIO");
+	pl061_init(&pd061);
+	pl061_register(gpio6_base, 6);
+	DMSG("Enable software mode control for chip select");
+	pl061_set_mode_control(GPIO6_2, PL061_MC_SW);
+
+	pd022.cs_data.gpio_data.chip = &pd061.chip;
+	pd022.cs_data.gpio_data.pin_num = GPIO6_2;
+	pd022.cs_control = PL022_CS_CTRL_AUTO_GPIO;
+
+	DMSG("spi_base: 0x%" PRIxVA "\n", spi_base);
+	DMSG("Configure SPI");
+	pd022.base = spi_base;
+	pd022.clk_hz = SPI_CLK_HZ;
+	pd022.speed_hz = SPI_10_KHZ;
+	pd022.mode = SPI_MODE0;
+	pd022.data_size_bits = 8;
+	pd022.loopback = true;
+
+	pl022_init(&pd022);
+	pd022.chip.ops->configure(&pd022.chip);
+	pd022.chip.ops->start(&pd022.chip);
+
+	for (j = 0; j < 20; j++) {
+		DMSG("SPI test loop: %zu", j);
+		res = pd022.chip.ops->txrx8(&pd022.chip, tx, rx, len);
+		if (res) {
+			EMSG("SPI transceive error %d", res);
+			break;
+		}
+
+		for (i = 0; i < len; i++)
+			DMSG("rx[%zu] = 0x%x", i, rx[i]);
+
+		tee_time_busy_wait(20);
+	}
+
+	pd022.chip.ops->end(&pd022.chip);
+}
+
+/*
+ * spi_init() MUST be run before calling this function!
+ *
+ * spi_test runs some loopback tests, so the SPI module will just receive
+ * what is transmitted, i.e. 0x01, 0x80, 0x00.
+ *
+ * In non-loopback mode, the transmitted value will elicit a readback of
+ * the measured value from the ADC chip on the Linksprite 96Boards
+ * Mezzanine card [1], which can be connected to either a sliding
+ * rheostat [2] or photoresistor [3].
+ *
+ * [1] http://linksprite.com/wiki/index.php5?title=Linker_Mezzanine_card_for_96board
+ * [2] http://learn.linksprite.com/96-board/sliding-rheostat
+ * [3] http://learn.linksprite.com/96-board/photoresistor
+ */
+void spi_test(void)
+{
+	spi_test_with_builtin_cs_control();
+	spi_test_with_registered_cs_cb();
+	spi_test_with_manual_cs_control();
+}
diff --git a/core/arch/arm/plat-hikey/sub.mk b/core/arch/arm/plat-hikey/sub.mk
new file mode 100644
index 0000000..5c23b86
--- /dev/null
+++ b/core/arch/arm/plat-hikey/sub.mk
@@ -0,0 +1,3 @@
+global-incdirs-y += .
+srcs-y += main.c
+srcs-$(CFG_SPI_TEST) += spi_test.c
diff --git a/core/arch/arm/plat-imx/a9_plat_init.S b/core/arch/arm/plat-imx/a9_plat_init.S
new file mode 100644
index 0000000..64d03f5
--- /dev/null
+++ b/core/arch/arm/plat-imx/a9_plat_init.S
@@ -0,0 +1,109 @@
+/*
+ * Copyright (c) 2014, STMicroelectronics International N.V.
+ * All rights reserved.
+ * Copyright (c) 2016, Wind River Systems.
+ * 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.
+ */
+
+/*
+ * Entry points for the A9 inits, A9 revision specific or not.
+ * It is assume no stack is available when these routines are called.
+ * It is assume each routine is called with return address in LR
+ * and with ARM registers R0, R1, R2, R3 being scratchable.
+ */
+
+#include <arm32.h>
+#include <arm32_macros.S>
+#include <arm32_macros_cortex_a9.S>
+#include <asm.S>
+#include <kernel/tz_ssvce_def.h>
+#include <kernel/unwind.h>
+#include <platform_config.h>
+
+.section .text
+.balign 4
+.code 32
+
+/*
+ * Cortex A9 early configuration
+ *
+ * Use registers R0-R3.
+ * No stack usage.
+ * LR store return address.
+ * Trap CPU in case of error.
+ */
+FUNC plat_cpu_reset_early , :
+UNWIND(	.fnstart)
+
+	/*
+	 * Disallow NSec to mask FIQ [bit4: FW=0]
+	 * Allow NSec to manage Imprecise Abort [bit5: AW=1]
+	 * Imprecise Abort trapped to Abort Mode [bit3: EA=0]
+	 * In Sec world, FIQ trapped to FIQ Mode [bit2: FIQ=0]
+	 * IRQ always trapped to IRQ Mode [bit1: IRQ=0]
+	 * Secure World [bit0: NS=0]
+	 */
+	mov r0, #SCR_AW
+	write_scr r0
+
+	/*
+	 * Mandated HW config loaded
+	 *
+	 * SCTLR = 0x00004000
+	 * - Round-Robin replac. for icache, btac, i/duTLB (bit14: RoundRobin)
+	 *
+	 * ACTRL = 0x00000041
+	 * - core always in full SMP (FW bit0=1, SMP bit6=1)
+	 * - L2 write full line of zero disabled (bit3=0)
+	 *   (keep WFLZ low. Will be set once outer L2 is ready)
+	 *
+	 * NSACR = 0x00020C00
+	 * - NSec cannot change ACTRL.SMP (NS_SMP bit18=0)
+	 * - Nsec can lockdown TLB (TL bit17=1)
+	 * - NSec cannot access PLE (PLE bit16=0)
+	 * - NSec can use SIMD/VFP (CP10/CP11) (bit15:14=2b00, bit11:10=2b11)
+	 *
+	 * PCR = 0x00000001
+	 * - no change latency, enable clk gating
+	 */
+	movw r0, #0x4000
+	movt r0, #0x0000
+	write_sctlr r0
+
+	movw r0, #0x0041
+	movt r0, #0x0000
+	write_actlr r0
+
+	movw r0, #0x0C00
+	movt r0, #0x0002
+	write_nsacr r0
+
+	movw r0, #0x0000
+	movt r0, #0x0001
+	write_pcr r0
+
+	mov pc, lr
+UNWIND(	.fnend)
+END_FUNC plat_cpu_reset_early
diff --git a/core/arch/arm/plat-imx/conf.mk b/core/arch/arm/plat-imx/conf.mk
new file mode 100644
index 0000000..785736a
--- /dev/null
+++ b/core/arch/arm/plat-imx/conf.mk
@@ -0,0 +1,34 @@
+PLATFORM_FLAVOR ?= mx6ulevk
+
+ifeq ($(PLATFORM_FLAVOR),mx6ulevk)
+arm32-platform-cpuarch		:= cortex-a7
+endif
+ifeq ($(PLATFORM_FLAVOR),$(filter $(PLATFORM_FLAVOR),mx6qsabrelite mx6qsabresd))
+arm32-platform-cpuarch		:= cortex-a9
+endif
+arm32-platform-cflags		+= -mcpu=$(arm32-platform-cpuarch)
+arm32-platform-aflags		+= -mcpu=$(arm32-platform-cpuarch)
+core_arm32-platform-aflags	+= -mfpu=neon
+
+$(call force,CFG_ARM32_core,y)
+$(call force,CFG_GENERIC_BOOT,y)
+$(call force,CFG_GIC,y)
+$(call force,CFG_IMX_UART,y)
+$(call force,CFG_PM_STUBS,y)
+$(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))
+$(call force,CFG_PL310,y)
+$(call force,CFG_PL310_LOCKED,y)
+$(call force,CFG_SECURE_TIME_SOURCE_REE,y)
+
+CFG_BOOT_SYNC_CPU ?= y
+CFG_BOOT_SECONDARY_REQUEST ?= y
+endif
+
+ta-targets = ta_arm32
+
+CFG_CRYPTO_SIZE_OPTIMIZATION ?= n
+CFG_WITH_STACK_CANARIES ?= y
diff --git a/core/arch/arm/plat-imx/imx6ul.c b/core/arch/arm/plat-imx/imx6ul.c
new file mode 100644
index 0000000..795c750
--- /dev/null
+++ b/core/arch/arm/plat-imx/imx6ul.c
@@ -0,0 +1,57 @@
+/*
+ * Copyright (C) 2016 Freescale Semiconductor, Inc.
+ * All rights reserved.
+ *
+ * Peng Fan <peng.fan@nxp.com>
+ *
+ * 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 <io.h>
+#include <kernel/generic_boot.h>
+#include <platform_config.h>
+#include <stdint.h>
+
+static void init_csu(void)
+{
+	uintptr_t addr;
+
+	/* first grant all peripherals */
+	for (addr = CSU_BASE + CSU_CSL_START;
+	     addr != CSU_BASE + CSU_CSL_END;
+	     addr += 4)
+		write32(CSU_ACCESS_ALL, addr);
+
+	/* lock the settings */
+	for (addr = CSU_BASE + CSU_CSL_START;
+	     addr != CSU_BASE + CSU_CSL_END;
+	     addr += 4)
+		write32(read32(addr) | CSU_SETTING_LOCK, addr);
+}
+
+/* MMU not enabled now */
+void plat_cpu_reset_late(void)
+{
+	init_csu();
+}
diff --git a/core/arch/arm/plat-imx/imx_pl310.c b/core/arch/arm/plat-imx/imx_pl310.c
new file mode 100644
index 0000000..fcad225
--- /dev/null
+++ b/core/arch/arm/plat-imx/imx_pl310.c
@@ -0,0 +1,63 @@
+/*
+ * Copyright (C) 2016 Freescale Semiconductor, Inc.
+ * All rights reserved.
+ *
+ * Peng Fan <peng.fan@nxp.com>
+ *
+ * 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 <io.h>
+#include <kernel/generic_boot.h>
+#include <kernel/tz_ssvce_def.h>
+#include <kernel/tz_ssvce_pl310.h>
+#include <platform_config.h>
+#include <stdint.h>
+
+void arm_cl2_config(vaddr_t pl310_base)
+{
+	/* Disable PL310 */
+	write32(0, pl310_base + PL310_CTRL);
+
+	write32(PL310_TAG_RAM_CTRL_INIT, pl310_base + PL310_TAG_RAM_CTRL);
+	write32(PL310_DATA_RAM_CTRL_INIT, pl310_base + PL310_DATA_RAM_CTRL);
+	write32(PL310_AUX_CTRL_INIT, pl310_base + PL310_AUX_CTRL);
+	write32(PL310_PREFETCH_CTRL_INIT, pl310_base + PL310_PREFETCH_CTRL);
+	write32(PL310_POWER_CTRL_INIT, pl310_base + PL310_POWER_CTRL);
+
+	/* invalidate all cache ways */
+	arm_cl2_invbyway(pl310_base);
+}
+
+void arm_cl2_enable(vaddr_t pl310_base)
+{
+	uint32_t val;
+
+	/* Enable PL310 ctrl -> only set lsb bit */
+	write32(1, pl310_base + PL310_CTRL);
+
+	/* if L2 FLZW enable, enable in L1 */
+	val = read32(pl310_base + PL310_AUX_CTRL);
+	if (val & 1)
+		write_actlr(read_actlr() | (1 << 3));
+}
diff --git a/core/arch/arm/plat-imx/kern.ld.S b/core/arch/arm/plat-imx/kern.ld.S
new file mode 100644
index 0000000..8d794ee
--- /dev/null
+++ b/core/arch/arm/plat-imx/kern.ld.S
@@ -0,0 +1 @@
+#include "../kernel/kern.ld.S"
diff --git a/core/arch/arm/plat-imx/link.mk b/core/arch/arm/plat-imx/link.mk
new file mode 100644
index 0000000..448ab89
--- /dev/null
+++ b/core/arch/arm/plat-imx/link.mk
@@ -0,0 +1 @@
+include core/arch/arm/kernel/link.mk
diff --git a/core/arch/arm/plat-imx/main.c b/core/arch/arm/plat-imx/main.c
new file mode 100644
index 0000000..edfbc37
--- /dev/null
+++ b/core/arch/arm/plat-imx/main.c
@@ -0,0 +1,202 @@
+/*
+ * Copyright (C) 2015 Freescale Semiconductor, Inc.
+ * All rights reserved.
+ * Copyright (c) 2016, Wind River Systems.
+ * 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 <console.h>
+#include <drivers/gic.h>
+#include <drivers/imx_uart.h>
+#include <io.h>
+#include <kernel/generic_boot.h>
+#include <kernel/misc.h>
+#include <kernel/panic.h>
+#include <kernel/pm_stubs.h>
+#include <mm/core_mmu.h>
+#include <mm/core_memprot.h>
+#include <platform_config.h>
+#include <stdint.h>
+#include <sm/optee_smc.h>
+#include <tee/entry_fast.h>
+#include <tee/entry_std.h>
+
+#if defined(PLATFORM_FLAVOR_mx6qsabrelite) || \
+	defined(PLATFORM_FLAVOR_mx6qsabresd)
+#include <kernel/tz_ssvce_pl310.h>
+#endif
+
+static void main_fiq(void);
+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,
+	.cpu_on = pm_panic,
+	.cpu_off = pm_panic,
+	.cpu_suspend = pm_panic,
+	.cpu_resume = pm_panic,
+	.system_off = pm_panic,
+	.system_reset = pm_panic,
+};
+
+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)
+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
+
+const struct thread_handlers *generic_boot_get_handlers(void)
+{
+	return &handlers;
+}
+
+static void main_fiq(void)
+{
+	panic();
+}
+
+#if defined(PLATFORM_FLAVOR_mx6qsabrelite) || \
+	defined(PLATFORM_FLAVOR_mx6qsabresd)
+void plat_cpu_reset_late(void)
+{
+	uintptr_t addr;
+
+	if (!get_core_pos()) {
+		/* primary core */
+#if defined(CFG_BOOT_SYNC_CPU)
+		/* set secondary entry address and release core */
+		write32(CFG_TEE_LOAD_ADDR, SRC_BASE + SRC_GPR1 + 8);
+		write32(CFG_TEE_LOAD_ADDR, SRC_BASE + SRC_GPR1 + 16);
+		write32(CFG_TEE_LOAD_ADDR, SRC_BASE + SRC_GPR1 + 24);
+
+		write32(SRC_SCR_CPU_ENABLE_ALL, SRC_BASE + SRC_SCR);
+#endif
+
+		/* SCU config */
+		write32(SCU_INV_CTRL_INIT, SCU_BASE + SCU_INV_SEC);
+		write32(SCU_SAC_CTRL_INIT, SCU_BASE + SCU_SAC);
+		write32(SCU_NSAC_CTRL_INIT, SCU_BASE + SCU_NSAC);
+
+		/* SCU enable */
+		write32(read32(SCU_BASE + SCU_CTRL) | 0x1,
+			SCU_BASE + SCU_CTRL);
+
+		/* configure imx6 CSU */
+
+		/* first grant all peripherals */
+		for (addr = CSU_BASE + CSU_CSL_START;
+			 addr != CSU_BASE + CSU_CSL_END;
+			 addr += 4)
+			write32(CSU_ACCESS_ALL, addr);
+
+		/* lock the settings */
+		for (addr = CSU_BASE + CSU_CSL_START;
+			 addr != CSU_BASE + CSU_CSL_END;
+			 addr += 4)
+			write32(read32(addr) | CSU_SETTING_LOCK, addr);
+	}
+}
+#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);
+}
+
+void main_init_gic(void)
+{
+	vaddr_t gicc_base;
+	vaddr_t gicd_base;
+
+	gicc_base = (vaddr_t)phys_to_virt(GIC_BASE + GICC_OFFSET,
+					  MEM_AREA_IO_SEC);
+	gicd_base = (vaddr_t)phys_to_virt(GIC_BASE + GICD_OFFSET,
+					  MEM_AREA_IO_SEC);
+
+	if (!gicc_base || !gicd_base)
+		panic();
+
+	/* Initialize GIC */
+	gic_init(&gic_data, gicc_base, gicd_base);
+	itr_init(&gic_data.chip);
+}
+
+#if defined(PLATFORM_FLAVOR_mx6qsabrelite) || \
+	defined(PLATFORM_FLAVOR_mx6qsabresd)
+vaddr_t pl310_base(void)
+{
+	static void *va __early_bss;
+
+	if (cpu_mmu_enabled()) {
+		if (!va)
+			va = phys_to_virt(PL310_BASE, MEM_AREA_IO_SEC);
+		return (vaddr_t)va;
+	}
+	return PL310_BASE;
+}
+
+void main_secondary_init_gic(void)
+{
+	gic_cpu_init(&gic_data);
+}
+#endif
diff --git a/core/arch/arm/plat-imx/platform_config.h b/core/arch/arm/plat-imx/platform_config.h
new file mode 100644
index 0000000..8e55ee8
--- /dev/null
+++ b/core/arch/arm/plat-imx/platform_config.h
@@ -0,0 +1,410 @@
+/*
+ * Copyright (C) 2015 Freescale Semiconductor, Inc.
+ * All rights reserved.
+ * Copyright (c) 2016, Wind River Systems.
+ * 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 PLATFORM_CONFIG_H
+#define PLATFORM_CONFIG_H
+
+#define STACK_ALIGNMENT			64
+
+/* For i.MX 6UltraLite EVK board */
+
+#if defined(PLATFORM_FLAVOR_mx6ulevk)
+
+#ifdef CFG_WITH_PAGER
+#error "Pager not supported for platform mx6ulevk"
+#endif
+#ifdef CFG_WITH_LPAE
+#error "LPAE not supported for now"
+#endif
+
+#define GIC_BASE			0xA00000
+#define GIC_SIZE			0x8000
+#define GICC_OFFSET			0x2000
+#define GICD_OFFSET			0x1000
+#define UART0_BASE			0x2020000
+#define UART1_BASE			0x21E8000
+#define UART2_BASE			0x21EC000
+
+#define AHB1_BASE			0x02000000
+#define AHB1_SIZE			0x100000
+#define AHB2_BASE			0x02100000
+#define AHB2_SIZE			0x100000
+#define AHB3_BASE			0x02200000
+#define AHB3_SIZE			0x100000
+
+#define AIPS_TZ1_BASE_ADDR	0x02000000
+#define AIPS1_OFF_BASE_ADDR	(AIPS_TZ1_BASE_ADDR + 0x80000)
+
+#define DRAM0_BASE			0x80000000
+#define DRAM0_SIZE			0x20000000
+
+#define CFG_TEE_CORE_NB_CORE		1
+
+#define DDR_PHYS_START			DRAM0_BASE
+#define DDR_SIZE			DRAM0_SIZE
+
+#define CFG_DDR_START			DDR_PHYS_START
+#define CFG_DDR_SIZE			DDR_SIZE
+
+/* Full GlobalPlatform test suite requires CFG_SHMEM_SIZE to be at least 2MB */
+#define CFG_SHMEM_START			(TZDRAM_BASE - 0x100000)
+#define CFG_SHMEM_SIZE			0x100000
+
+/* Location of trusted dram on imx */
+#define TZDRAM_BASE			(0x9c100000)
+#define TZDRAM_SIZE			(0x03000000)
+
+#define CFG_TEE_RAM_VA_SIZE		(1024 * 1024)
+
+#ifndef CFG_TEE_LOAD_ADDR
+#define CFG_TEE_LOAD_ADDR		CFG_TEE_RAM_START
+#endif
+
+/*
+ * Everything is in TZDRAM.
+ * +------------------+
+ * |        | TEE_RAM |
+ * + TZDRAM +---------+
+ * |        | TA_RAM  |
+ * +--------+---------+
+ */
+#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		ROUNDDOWN((TZDRAM_SIZE - CFG_TEE_RAM_VA_SIZE), \
+						CORE_MMU_DEVICE_SIZE)
+
+#define CONSOLE_UART_BASE		(UART0_BASE)
+
+/* Central Security Unit register values */
+#define CSU_BASE			0x021C0000
+#define CSU_CSL_START			0x0
+#define CSU_CSL_END			0xA0
+#define CSU_ACCESS_ALL			0x00FF00FF
+#define CSU_SETTING_LOCK		0x01000100
+
+/* For i.MX6 Quad SABRE Lite and Smart Device board */
+
+#elif defined(PLATFORM_FLAVOR_mx6qsabrelite) || \
+	defined(PLATFORM_FLAVOR_mx6qsabresd)
+
+#define SCU_BASE			0x00A00000
+#define PL310_BASE			0x00A02000
+#define SRC_BASE			0x020D8000
+#define SRC_SCR				0x000
+#define SRC_GPR1			0x020
+#define SRC_SCR_CPU_ENABLE_ALL		SHIFT_U32(0x7, 22)
+#define SRC_SCR_CORE1_RST_OFFSET	14
+#define SRC_SCR_CORE1_ENABLE_OFFSET	22
+#define GIC_BASE			0x00A00000
+#define GICC_OFFSET			0x100
+#define GICD_OFFSET			0x1000
+#define GIC_CPU_BASE			(GIC_BASE + GICC_OFFSET)
+#define GIC_DIST_BASE			(GIC_BASE + GICD_OFFSET)
+#define UART1_BASE			0x02020000
+#define UART2_BASE			0x021E8000
+
+/* Central Security Unit register values */
+#define CSU_BASE			0x021C0000
+#define CSU_CSL_START			0x0
+#define CSU_CSL_END			0xA0
+#define CSU_CSL5			0x14
+#define CSU_CSL16			0x40
+#define	CSU_ACCESS_ALL			0x00FF00FF
+#define CSU_SETTING_LOCK		0x01000100
+
+#if defined(PLATFORM_FLAVOR_mx6qsabrelite)
+#define CONSOLE_UART_BASE		UART2_BASE
+#endif
+#if defined(PLATFORM_FLAVOR_mx6qsabresd)
+#define CONSOLE_UART_BASE		UART1_BASE
+#endif
+#define DRAM0_BASE			0x10000000
+#define DRAM0_SIZE			0x40000000
+
+#define CFG_TEE_RAM_VA_SIZE		(1024 * 1024)
+
+#define CFG_TEE_CORE_NB_CORE		4
+
+#define DDR_PHYS_START			DRAM0_BASE
+#define DDR_SIZE			DRAM0_SIZE
+
+#define CFG_DDR_START			DDR_PHYS_START
+#define CFG_DDR_SIZE			DDR_SIZE
+
+/*
+ * PL310 TAG RAM Control Register
+ *
+ * bit[10:8]:1 - 2 cycle of write accesses latency
+ * bit[6:4]:1 - 2 cycle of read accesses latency
+ * bit[2:0]:1 - 2 cycle of setup latency
+ */
+#ifndef PL310_TAG_RAM_CTRL_INIT
+#define PL310_TAG_RAM_CTRL_INIT		0x00000111
+#endif
+
+/*
+ * PL310 DATA RAM Control Register
+ *
+ * bit[10:8]:2 - 3 cycle of write accesses latency
+ * bit[6:4]:2 - 3 cycle of read accesses latency
+ * bit[2:0]:2 - 3 cycle of setup latency
+ */
+#ifndef PL310_DATA_RAM_CTRL_INIT
+#define PL310_DATA_RAM_CTRL_INIT	0x00000222
+#endif
+
+/*
+ * PL310 Auxiliary Control Register
+ *
+ * I/Dcache prefetch enabled (bit29:28=2b11)
+ * NS can access interrupts (bit27=1)
+ * NS can lockown cache lines (bit26=1)
+ * Pseudo-random replacement policy (bit25=0)
+ * Force write allocated (default)
+ * Shared attribute internally ignored (bit22=1, bit13=0)
+ * Parity disabled (bit21=0)
+ * Event monitor disabled (bit20=0)
+ * Platform fmavor specific way config:
+ * - 64kb way size (bit19:17=3b011)
+ * - 16-way associciativity (bit16=1)
+ * Store buffer device limitation enabled (bit11=1)
+ * Cacheable accesses have high prio (bit10=0)
+ * Full Line Zero (FLZ) disabled (bit0=0)
+ */
+#ifndef PL310_AUX_CTRL_INIT
+#define PL310_AUX_CTRL_INIT		0x3C470800
+#endif
+
+/*
+ * PL310 Prefetch Control Register
+ *
+ * Double linefill disabled (bit30=0)
+ * I/D prefetch enabled (bit29:28=2b11)
+ * Prefetch drop enabled (bit24=1)
+ * Incr double linefill disable (bit23=0)
+ * Prefetch offset = 7 (bit4:0)
+ */
+#define PL310_PREFETCH_CTRL_INIT	0x31000007
+
+/*
+ * PL310 Power Register
+ *
+ * Dynamic clock gating enabled
+ * Standby mode enabled
+ */
+#define PL310_POWER_CTRL_INIT		0x00000003
+
+/*
+ * SCU Invalidate Register
+ *
+ * Invalidate all registers
+ */
+#define	SCU_INV_CTRL_INIT		0xFFFFFFFF
+
+/*
+ * SCU Access Register
+ * - both secure CPU access SCU
+ */
+#define SCU_SAC_CTRL_INIT		0x0000000F
+
+/*
+ * SCU NonSecure Access Register
+ * - both nonsec cpu access SCU, private and global timer
+ */
+#define SCU_NSAC_CTRL_INIT		0x00000FFF
+
+/* define the memory areas */
+
+#ifdef CFG_WITH_PAGER
+
+/*
+ * TEE/TZ RAM layout:
+ *
+ *  +---------------------------------------+  <- CFG_CORE_TZSRAM_EMUL_START
+ *  | TEE private highly | TEE_RAM          |   ^
+ *  |   secure memory    |                  |   | CFG_CORE_TZSRAM_EMUL_SIZE
+ *  +---------------------------------------+   v
+ *
+ *  +---------------------------------------+  <- CFG_DDR_TEETZ_RESERVED_START
+ *  | TEE private secure |  TA_RAM          |   ^
+ *  |   external memory  |                  |   |
+ *  +---------------------------------------+   | CFG_DDR_TEETZ_RESERVED_SIZE
+ *  |     Non secure     |  SHM             |   |
+ *  |   shared memory    |                  |   |
+ *  +---------------------------------------+   v
+ *
+ *  TEE_RAM : default 256kByte
+ *  TA_RAM  : all what is left in DDR TEE reserved area
+ *  PUB_RAM : default 2MByte
+ */
+
+/* emulated SRAM, at start of secure DDR */
+
+#define CFG_CORE_TZSRAM_EMUL_START	0x4E000000
+
+#define TZSRAM_BASE			CFG_CORE_TZSRAM_EMUL_START
+#define TZSRAM_SIZE			CFG_CORE_TZSRAM_EMUL_SIZE
+
+/* Location of trusted dram */
+
+#define CFG_DDR_TEETZ_RESERVED_START	0x4E100000
+#define CFG_DDR_TEETZ_RESERVED_SIZE	0x01F00000
+
+#define CFG_PUB_RAM_SIZE		(1 * 1024 * 1024)
+#define CFG_TEE_RAM_PH_SIZE		TZSRAM_SIZE
+
+#define TZDRAM_BASE			(CFG_DDR_TEETZ_RESERVED_START)
+#define TZDRAM_SIZE			(CFG_DDR_TEETZ_RESERVED_SIZE - \
+				CFG_PUB_RAM_SIZE)
+
+#define CFG_TA_RAM_START		TZDRAM_BASE
+#define CFG_TA_RAM_SIZE			TZDRAM_SIZE
+
+#define CFG_SHMEM_START			(CFG_DDR_TEETZ_RESERVED_START + \
+						TZDRAM_SIZE)
+#define CFG_SHMEM_SIZE			CFG_PUB_RAM_SIZE
+
+#define CFG_TEE_RAM_START		TZSRAM_BASE
+
+#ifndef CFG_TEE_LOAD_ADDR
+#define CFG_TEE_LOAD_ADDR		TZSRAM_BASE
+#endif
+
+#else /* CFG_WITH_PAGER */
+
+/*
+ * TEE/TZ RAM layout:
+ *
+ *  +---------------------------------------+  <- CFG_DDR_TEETZ_RESERVED_START
+ *  | TEE private secure |  TEE_RAM         |   ^
+ *  |   external memory  +------------------+   |
+ *  |                    |  TA_RAM          |   |
+ *  +---------------------------------------+   | CFG_DDR_TEETZ_RESERVED_SIZE
+ *  |     Non secure     |  SHM             |   |
+ *  |   shared memory    |                  |   |
+ *  +---------------------------------------+   v
+ *
+ *  TEE_RAM : default 1MByte
+ *  PUB_RAM : default 2MByte
+ *  TA_RAM  : all what is left
+ */
+
+#define CFG_DDR_TEETZ_RESERVED_START	0x4E000000
+#define CFG_DDR_TEETZ_RESERVED_SIZE	0x02000000
+
+#define CFG_PUB_RAM_SIZE		(1 * 1024 * 1024)
+#define CFG_TEE_RAM_PH_SIZE		(1 * 1024 * 1024)
+
+#define TZDRAM_BASE			(CFG_DDR_TEETZ_RESERVED_START)
+#define TZDRAM_SIZE			(CFG_DDR_TEETZ_RESERVED_SIZE - \
+				CFG_PUB_RAM_SIZE)
+
+#define CFG_TA_RAM_START		(CFG_DDR_TEETZ_RESERVED_START + \
+				CFG_TEE_RAM_PH_SIZE)
+#define CFG_TA_RAM_SIZE			(CFG_DDR_TEETZ_RESERVED_SIZE - \
+				CFG_TEE_RAM_PH_SIZE - \
+				CFG_PUB_RAM_SIZE)
+
+#define CFG_SHMEM_START			(CFG_DDR_TEETZ_RESERVED_START + \
+				TZDRAM_SIZE)
+#define CFG_SHMEM_SIZE			CFG_PUB_RAM_SIZE
+
+#define CFG_TEE_RAM_START		TZDRAM_BASE
+
+#ifndef CFG_TEE_LOAD_ADDR
+#define CFG_TEE_LOAD_ADDR		TZDRAM_BASE
+#endif
+
+#endif /* CFG_WITH_PAGER */
+
+#else
+#error "Unknown platform flavor"
+#endif /* defined(PLATFORM_FLAVOR_mx6ulevk) */
+
+#ifdef CFG_PL310
+/*
+ * PL310 TAG RAM Control Register
+ *
+ * bit[10:8]:1 - 2 cycle of write accesses latency
+ * bit[6:4]:1 - 2 cycle of read accesses latency
+ * bit[2:0]:1 - 2 cycle of setup latency
+ */
+#define PL310_TAG_RAM_CTRL_INIT		0x00000111
+
+/*
+ * DATA RAM Control Register
+ *
+ * bit[10:8]:2 - 3 cycle of write accesses latency
+ * bit[6:4]:2 - 3 cycle of read accesses latency
+ * bit[2:0]:2 - 3 cycle of setup latency
+ */
+#define PL310_DATA_RAM_CTRL_INIT	0x00000222
+
+/*
+ * Auxiliary Control Register
+ *
+ * I/Dcache prefetch enabled (bit29:28=2b11)
+ * NS can access interrupts (bit27=1)
+ * NS can lockown cache lines (bit26=1)
+ * Pseudo-random replacement policy (bit25=0)
+ * Force write allocated (default)
+ * Shared attribute internally ignored (bit22=1, bit13=0)
+ * Parity disabled (bit21=0)
+ * Event monitor disabled (bit20=0)
+ * 64kB ways, 16-way associativity (bit19:17=3b011 bit16=1)
+ * Store buffer device limitation enabled (bit11=1)
+ * Cacheable accesses have high prio (bit10=0)
+ * Full Line Zero (FLZ) disabled (bit0=0)
+ */
+#define PL310_AUX_CTRL_INIT		0x3C470800
+
+/*
+ * Prefetch Control Register
+ *
+ * Double linefill disabled (bit30=0)
+ * I/D prefetch enabled (bit29:28=2b11)
+ * Prefetch drop enabled (bit24=1)
+ * Incr double linefill disable (bit23=0)
+ * Prefetch offset = 7 (bit4:0)
+ */
+#define PL310_PREFETCH_CTRL_INIT	0x31000007
+
+/*
+ * Power Register = 0x00000003
+ *
+ * Dynamic clock gating enabled
+ * Standby mode enabled
+ */
+#define PL310_POWER_CTRL_INIT		0x00000003
+
+#endif
+
+#endif /*PLATFORM_CONFIG_H*/
diff --git a/core/arch/arm/plat-imx/psci.c b/core/arch/arm/plat-imx/psci.c
new file mode 100644
index 0000000..065555b
--- /dev/null
+++ b/core/arch/arm/plat-imx/psci.c
@@ -0,0 +1,78 @@
+/*
+ * Copyright (C) 2016 Freescale Semiconductor, Inc.
+ * All rights reserved.
+ *
+ * Peng Fan <peng.fan@nxp.com>
+ *
+ * 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 <console.h>
+#include <drivers/imx_uart.h>
+#include <io.h>
+#include <kernel/generic_boot.h>
+#include <kernel/panic.h>
+#include <kernel/pm_stubs.h>
+#include <mm/core_mmu.h>
+#include <mm/core_memprot.h>
+#include <platform_config.h>
+#include <stdint.h>
+#include <sm/optee_smc.h>
+#include <sm/psci.h>
+#include <tee/entry_std.h>
+#include <tee/entry_fast.h>
+
+static vaddr_t src_base(void)
+{
+	static void *va __data; /* in case it's used before .bss is cleared */
+
+	if (cpu_mmu_enabled()) {
+		if (!va)
+			va = phys_to_virt(SRC_BASE, MEM_AREA_IO_SEC);
+		return (vaddr_t)va;
+	}
+	return SRC_BASE;
+}
+
+int psci_cpu_on(uint32_t core_idx, uint32_t entry,
+		uint32_t context_id __attribute__((unused)))
+{
+	uint32_t val;
+	vaddr_t va = src_base();
+
+	if ((core_idx == 0) || (core_idx >= CFG_TEE_CORE_NB_CORE))
+		return PSCI_RET_INVALID_PARAMETERS;
+
+	/* set secondary cores' NS entry addresses */
+	ns_entry_addrs[core_idx] = entry;
+
+	/* boot secondary cores from OP-TEE load address */
+	write32((uint32_t)CFG_TEE_LOAD_ADDR, va + SRC_GPR1 + core_idx * 8);
+
+	/* release secondary core */
+	val = read32(va + SRC_SCR);
+	val |=  BIT32(SRC_SCR_CORE1_ENABLE_OFFSET + (core_idx - 1));
+	val |=  BIT32(SRC_SCR_CORE1_RST_OFFSET + (core_idx - 1));
+	write32(val, va + SRC_SCR);
+
+	return PSCI_RET_SUCCESS;
+}
diff --git a/core/arch/arm/plat-imx/sub.mk b/core/arch/arm/plat-imx/sub.mk
new file mode 100644
index 0000000..d0a2f51
--- /dev/null
+++ b/core/arch/arm/plat-imx/sub.mk
@@ -0,0 +1,9 @@
+global-incdirs-y += .
+srcs-y += main.c
+
+srcs-$(CFG_PL310) += imx_pl310.c
+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_mx6ulevk) += imx6ul.c
diff --git a/core/arch/arm/plat-ls/conf.mk b/core/arch/arm/plat-ls/conf.mk
new file mode 100644
index 0000000..e7385fd
--- /dev/null
+++ b/core/arch/arm/plat-ls/conf.mk
@@ -0,0 +1,20 @@
+PLATFORM_FLAVOR ?= ls1021atwr
+
+arm32-platform-cpuarch		:= cortex-a7
+arm32-platform-cflags		+= -mcpu=$(arm32-platform-cpuarch)
+arm32-platform-aflags		+= -mcpu=$(arm32-platform-cpuarch)
+core_arm32-platform-aflags	+= -mfpu=neon
+
+$(call force,CFG_GENERIC_BOOT,y)
+$(call force,CFG_ARM32_core,y)
+$(call force,CFG_SECURE_TIME_SOURCE_CNTPCT,y)
+$(call force,CFG_GIC,y)
+$(call force,CFG_16550_UART,y)
+$(call force,CFG_PM_STUBS,y)
+
+ta-targets = ta_arm32
+
+CFG_BOOT_SYNC_CPU ?= y
+CFG_BOOT_SECONDARY_REQUEST ?= y
+CFG_CRYPTO_SIZE_OPTIMIZATION ?= n
+CFG_WITH_STACK_CANARIES ?= y
diff --git a/core/arch/arm/plat-ls/kern.ld.S b/core/arch/arm/plat-ls/kern.ld.S
new file mode 100644
index 0000000..8d794ee
--- /dev/null
+++ b/core/arch/arm/plat-ls/kern.ld.S
@@ -0,0 +1 @@
+#include "../kernel/kern.ld.S"
diff --git a/core/arch/arm/plat-ls/link.mk b/core/arch/arm/plat-ls/link.mk
new file mode 100644
index 0000000..448ab89
--- /dev/null
+++ b/core/arch/arm/plat-ls/link.mk
@@ -0,0 +1 @@
+include core/arch/arm/kernel/link.mk
diff --git a/core/arch/arm/plat-ls/ls_core_pos.S b/core/arch/arm/plat-ls/ls_core_pos.S
new file mode 100644
index 0000000..21cfb4b
--- /dev/null
+++ b/core/arch/arm/plat-ls/ls_core_pos.S
@@ -0,0 +1,41 @@
+/*
+ * Copyright (c) 2015, Freescale Semiconductor, Inc.
+ * 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 <asm.S>
+#include <arm.h>
+#include <arm32_macros.S>
+#include <kernel/unwind.h>
+
+/* Layerscape platform specific function to calculate core position. */
+FUNC get_core_pos , :
+UNWIND(	.fnstart)
+	read_mpidr r0
+	/* Calculate CorePos =  CoreId */
+	and	r0, r0, #MPIDR_CPU_MASK
+	bx	lr
+UNWIND(	.fnend)
+END_FUNC get_core_pos
diff --git a/core/arch/arm/plat-ls/main.c b/core/arch/arm/plat-ls/main.c
new file mode 100644
index 0000000..23ac0c6
--- /dev/null
+++ b/core/arch/arm/plat-ls/main.c
@@ -0,0 +1,178 @@
+/*
+ * Copyright (C) 2015 Freescale Semiconductor, Inc.
+ * 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 <arm32.h>
+#include <console.h>
+#include <drivers/gic.h>
+#include <drivers/ns16550.h>
+#include <io.h>
+#include <kernel/generic_boot.h>
+#include <kernel/misc.h>
+#include <kernel/panic.h>
+#include <kernel/pm_stubs.h>
+#include <kernel/thread.h>
+#include <kernel/tz_ssvce_def.h>
+#include <mm/core_memprot.h>
+#include <sm/optee_smc.h>
+#include <tee/entry_fast.h>
+#include <tee/entry_std.h>
+
+static void main_fiq(void);
+
+static const struct thread_handlers handlers = {
+	.std_smc = tee_entry_std,
+	.fast_smc = tee_entry_fast,
+	.fiq = main_fiq,
+	.cpu_on = pm_panic,
+	.cpu_off = pm_panic,
+	.cpu_suspend = pm_panic,
+	.cpu_resume = pm_panic,
+	.system_off = pm_panic,
+	.system_reset = pm_panic,
+};
+
+static struct gic_data gic_data;
+
+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);
+
+const struct thread_handlers *generic_boot_get_handlers(void)
+{
+	return &handlers;
+}
+
+static void main_fiq(void)
+{
+	panic();
+}
+
+void plat_cpu_reset_late(void)
+{
+	static uint32_t cntfrq __early_bss;
+	vaddr_t addr;
+
+	if (!get_core_pos()) {
+		/* read cnt freq */
+		cntfrq = read_cntfrq();
+
+#if defined(CFG_BOOT_SECONDARY_REQUEST)
+		/* set secondary entry address */
+		write32(__compiler_bswap32(CFG_TEE_LOAD_ADDR),
+				DCFG_BASE + DCFG_SCRATCHRW1);
+
+		/* release secondary cores */
+		write32(__compiler_bswap32(0x1 << 1), /* cpu1 */
+				DCFG_BASE + DCFG_CCSR_BRR);
+		dsb();
+		sev();
+#endif
+
+		/* configure CSU */
+
+		/* first grant all peripherals */
+		for (addr = CSU_BASE + CSU_CSL_START;
+			 addr != CSU_BASE + CSU_CSL_END;
+			 addr += 4)
+			write32(__compiler_bswap32(CSU_ACCESS_ALL), addr);
+
+		/* restrict key preipherals from NS */
+		write32(__compiler_bswap32(CSU_ACCESS_SEC_ONLY),
+			CSU_BASE + CSU_CSL30);
+		write32(__compiler_bswap32(CSU_ACCESS_SEC_ONLY),
+			CSU_BASE + CSU_CSL37);
+
+		/* lock the settings */
+		for (addr = CSU_BASE + CSU_CSL_START;
+			 addr != CSU_BASE + CSU_CSL_END;
+			 addr += 4)
+			write32(read32(addr) |
+				__compiler_bswap32(CSU_SETTING_LOCK),
+				addr);
+	} else {
+		/* program the cntfrq, the cntfrq is banked for each core */
+		write_cntfrq(cntfrq);
+	}
+}
+
+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());
+}
+
+void main_init_gic(void)
+{
+	vaddr_t gicc_base;
+	vaddr_t gicd_base;
+
+	gicc_base = (vaddr_t)phys_to_virt(GIC_BASE + GICC_OFFSET,
+					  MEM_AREA_IO_SEC);
+	gicd_base = (vaddr_t)phys_to_virt(GIC_BASE + GICD_OFFSET,
+					  MEM_AREA_IO_SEC);
+
+	if (!gicc_base || !gicd_base)
+		panic();
+
+	/* Initialize GIC */
+	gic_init(&gic_data, gicc_base, gicd_base);
+	itr_init(&gic_data.chip);
+}
+
+void main_secondary_init_gic(void)
+{
+	gic_cpu_init(&gic_data);
+}
diff --git a/core/arch/arm/plat-ls/plat_init.S b/core/arch/arm/plat-ls/plat_init.S
new file mode 100644
index 0000000..81ba7d7
--- /dev/null
+++ b/core/arch/arm/plat-ls/plat_init.S
@@ -0,0 +1,93 @@
+/*
+ * Copyright (c) 2016, Wind River Systems.
+ * 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.
+ */
+
+/*
+ * Entry points for the A9 inits, A9 revision specific or not.
+ * It is assume no stack is available when these routines are called.
+ * It is assume each routine is called with return address in LR
+ * and with ARM registers R0, R1, R2, R3 being scratchable.
+ */
+
+#include <arm32.h>
+#include <arm32_macros.S>
+#include <asm.S>
+#include <kernel/unwind.h>
+#include <platform_config.h>
+
+.section .text
+.balign 4
+.code 32
+
+/*
+ * platform early configuration
+ *
+ * Use scratables registers R0-R3.
+ * No stack usage.
+ * LR store return address.
+ * Trap CPU in case of error.
+ */
+FUNC plat_cpu_reset_early , :
+UNWIND(	.fnstart)
+
+	/*
+	 * Disallow NSec to mask FIQ [bit4: FW=0]
+	 * Allow NSec to manage Imprecise Abort [bit5: AW=1]
+	 * Imprecise Abort trapped to Abort Mode [bit3: EA=0]
+	 * In Sec world, FIQ trapped to FIQ Mode [bit2: FIQ=0]
+	 * IRQ always trapped to IRQ Mode [bit1: IRQ=0]
+	 * Secure World [bit0: NS=0]
+	 */
+	mov r0, #SCR_AW
+	write_scr r0		/* write Secure Configuration Register */
+
+	/*
+	 * Mandated HW config loaded
+	 *
+	 * SCTLR = 0x00000000
+	 *
+	 * ACTRL = 0x00000041
+	 * - core always in full SMP (FW bit0=1)
+	 *
+	 * NSACR = 0x00020C00
+	 * - 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
+	write_sctlr r0
+
+	movw r0, #0x0040
+	movt r0, #0x0000
+	write_actlr r0
+
+	movw r0, #0x0C00
+	movt r0, #0x0000
+	write_nsacr r0
+
+	mov pc, lr
+UNWIND(	.fnend)
+END_FUNC plat_cpu_reset_early
diff --git a/core/arch/arm/plat-ls/platform_config.h b/core/arch/arm/plat-ls/platform_config.h
new file mode 100644
index 0000000..7f0adc6
--- /dev/null
+++ b/core/arch/arm/plat-ls/platform_config.h
@@ -0,0 +1,147 @@
+/*
+ * Copyright (C) 2015 Freescale Semiconductor, Inc.
+ * 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 PLATFORM_CONFIG_H
+#define PLATFORM_CONFIG_H
+
+#define STACK_ALIGNMENT			64
+
+#define GIC_BASE			0x01400000
+#define GICC_OFFSET			0x2000
+#define GICD_OFFSET			0x1000
+
+#define DCFG_BASE			0x01EE0000
+#define DCFG_CCSR_BRR			0xE4
+#define DCFG_SCRATCHRW1			0x200
+
+#define CSU_BASE			0x01510000
+#define CSU_CSL_START			0x0
+#define CSU_CSL_END			0xE8
+#define CSU_CSL30			0x78
+#define CSU_CSL37			0x94
+
+/* Central Security Unit register values */
+#define	CSU_ACCESS_ALL			0x00FF00FF
+#define	CSU_ACCESS_SEC_ONLY		0x003F003F
+#define CSU_SETTING_LOCK		0x01000100
+
+/*  DUART 1 */
+#define UART0_BASE			0x021C0500
+/*  DUART 2 */
+#define UART1_BASE			0x021D0500
+/*  LPUART 1 */
+#define UART2_BASE			0x02950000
+/*  LPUART 2 */
+#define UART3_BASE			0x02960000
+
+
+/* console uart define */
+#define CONSOLE_UART_BASE		UART0_BASE
+
+#define DRAM0_BASE			0x80000000
+#if defined(PLATFORM_FLAVOR_ls1021aqds)
+#define DRAM0_SIZE			0x80000000
+#endif
+
+#if defined(PLATFORM_FLAVOR_ls1021atwr)
+#define DRAM0_SIZE			0x40000000
+#endif
+
+/* Location of trusted dram on layerscape */
+
+#if defined(PLATFORM_FLAVOR_ls1021atwr)
+#define CFG_DDR_TEETZ_RESERVED_START	0xBC000000
+#endif
+
+#if defined(PLATFORM_FLAVOR_ls1021aqds)
+#define CFG_DDR_TEETZ_RESERVED_START	0xFC000000
+#endif
+
+#define CFG_DDR_TEETZ_RESERVED_SIZE	0x03F00000
+
+#define CFG_TEE_RAM_VA_SIZE		(1024 * 1024)
+
+#define CFG_TEE_CORE_NB_CORE		2
+
+#define DDR_PHYS_START			DRAM0_BASE
+#define DDR_SIZE			DRAM0_SIZE
+
+#define CFG_DDR_START			DDR_PHYS_START
+#define CFG_DDR_SIZE			DDR_SIZE
+
+#ifndef CFG_DDR_TEETZ_RESERVED_START
+#error "TEETZ reserved DDR start address undef: CFG_DDR_TEETZ_RESERVED_START"
+#endif
+#ifndef CFG_DDR_TEETZ_RESERVED_SIZE
+#error "TEETZ reserved DDR siez undefined: CFG_DDR_TEETZ_RESERVED_SIZE"
+#endif
+
+/*
+ * TEE/TZ RAM layout:
+ *
+ *  +-----------------------------------------+  <- CFG_DDR_TEETZ_RESERVED_START
+ *  | TEETZ private RAM  |  TEE_RAM           |   ^
+ *  |                    +--------------------+   |
+ *  |                    |  TA_RAM            |   |
+ *  +-----------------------------------------+   | CFG_DDR_TEETZ_RESERVED_SIZE
+ *  |                    |      teecore alloc |   |
+ *  |  TEE/TZ and NSec   |  PUB_RAM   --------|   |
+ *  |   shared memory    |         NSec alloc |   |
+ *  +-----------------------------------------+   v
+ *
+ *  TEE_RAM : 1MByte
+ *  PUB_RAM : 1MByte
+ *  TA_RAM  : all what is left (at least 2MByte !)
+ */
+
+/* define the several memory area sizes */
+#if (CFG_DDR_TEETZ_RESERVED_SIZE < (4 * 1024 * 1024))
+#error "Invalid CFG_DDR_TEETZ_RESERVED_SIZE: at least 4MB expected"
+#endif
+
+/* Full GlobalPlatform test suite requires CFG_SHMEM_SIZE to be at least 2MB */
+#define CFG_PUB_RAM_SIZE		(1 * 1024 * 1024)
+#define CFG_TEE_RAM_PH_SIZE		(1 * 1024 * 1024)
+#define CFG_TA_RAM_SIZE			(CFG_DDR_TEETZ_RESERVED_SIZE - \
+					 CFG_TEE_RAM_PH_SIZE - CFG_PUB_RAM_SIZE)
+
+/* define the secure/unsecure memory areas */
+#define TZDRAM_BASE			(CFG_DDR_TEETZ_RESERVED_START)
+#define TZDRAM_SIZE			(CFG_TEE_RAM_PH_SIZE + CFG_TA_RAM_SIZE)
+
+#define CFG_SHMEM_START			(TZDRAM_BASE + TZDRAM_SIZE)
+#define CFG_SHMEM_SIZE			 CFG_PUB_RAM_SIZE
+
+/* define the memory areas (TEE_RAM must start at reserved DDR start addr */
+#define CFG_TEE_RAM_START		TZDRAM_BASE
+#define CFG_TA_RAM_START		(CFG_TEE_RAM_START + \
+					 CFG_TEE_RAM_PH_SIZE)
+#ifndef CFG_TEE_LOAD_ADDR
+#define CFG_TEE_LOAD_ADDR		CFG_TEE_RAM_START
+#endif
+
+#endif /*PLATFORM_CONFIG_H*/
diff --git a/core/arch/arm/plat-ls/sub.mk b/core/arch/arm/plat-ls/sub.mk
new file mode 100644
index 0000000..8bee406
--- /dev/null
+++ b/core/arch/arm/plat-ls/sub.mk
@@ -0,0 +1,4 @@
+global-incdirs-y += .
+srcs-y += main.c
+srcs-y += ls_core_pos.S
+srcs-y += plat_init.S
diff --git a/core/arch/arm/plat-mediatek/conf.mk b/core/arch/arm/plat-mediatek/conf.mk
new file mode 100644
index 0000000..81642b6
--- /dev/null
+++ b/core/arch/arm/plat-mediatek/conf.mk
@@ -0,0 +1,25 @@
+PLATFORM_FLAVOR ?= mt8173
+
+# 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
+
+$(call force,CFG_8250_UART,y)
+$(call force,CFG_GENERIC_BOOT,y)
+$(call force,CFG_HWSUPP_MEM_PERM_PXN,y)
+$(call force,CFG_PM_STUBS,y)
+$(call force,CFG_SECURE_TIME_SOURCE_CNTPCT,y)
+$(call force,CFG_WITH_ARM_TRUSTED_FW,y)
+
+ta-targets = ta_arm32
+
+ifeq ($(CFG_ARM64_core),y)
+$(call force,CFG_WITH_LPAE,y)
+ta-targets += ta_arm64
+else
+$(call force,CFG_ARM32_core,y)
+endif
+
+CFG_WITH_STACK_CANARIES ?= y
diff --git a/core/arch/arm/plat-mediatek/kern.ld.S b/core/arch/arm/plat-mediatek/kern.ld.S
new file mode 100644
index 0000000..8d794ee
--- /dev/null
+++ b/core/arch/arm/plat-mediatek/kern.ld.S
@@ -0,0 +1 @@
+#include "../kernel/kern.ld.S"
diff --git a/core/arch/arm/plat-mediatek/link.mk b/core/arch/arm/plat-mediatek/link.mk
new file mode 100644
index 0000000..448ab89
--- /dev/null
+++ b/core/arch/arm/plat-mediatek/link.mk
@@ -0,0 +1 @@
+include core/arch/arm/kernel/link.mk
diff --git a/core/arch/arm/plat-mediatek/main.c b/core/arch/arm/plat-mediatek/main.c
new file mode 100644
index 0000000..7780591
--- /dev/null
+++ b/core/arch/arm/plat-mediatek/main.c
@@ -0,0 +1,93 @@
+/*
+ * 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 <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 <platform_config.h>
+#include <stdint.h>
+#include <tee/entry_std.h>
+#include <tee/entry_fast.h>
+
+static void main_fiq(void);
+
+static const struct thread_handlers handlers = {
+	.std_smc = tee_entry_std,
+	.fast_smc = tee_entry_fast,
+	.fiq = 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;
+}
+
+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());
+}
diff --git a/core/arch/arm/plat-mediatek/mt8173_core_pos_a32.S b/core/arch/arm/plat-mediatek/mt8173_core_pos_a32.S
new file mode 100644
index 0000000..227deb3
--- /dev/null
+++ b/core/arch/arm/plat-mediatek/mt8173_core_pos_a32.S
@@ -0,0 +1,51 @@
+/*
+ * 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 <asm.S>
+#include <arm.h>
+#include <arm32_macros.S>
+#include <kernel/unwind.h>
+
+FUNC get_core_pos , :
+UNWIND(	.fnstart)
+	read_mpidr r0
+	and	r1, r0, #MPIDR_CPU_MASK
+	and	r0, r0, #MPIDR_CLUSTER_MASK
+	/*
+	 * Number of cores in cluster is 2,
+	 * we should have the following mapping:
+	 * MPIDR     core_pos
+	 * 0x0000 -> 0
+	 * 0x0001 -> 1
+	 * 0x0100 -> 2
+	 * 0x0101 -> 3
+	 */
+	add	r0, r1, r0, LSR #7
+	bx	lr
+UNWIND(	.fnend)
+END_FUNC get_core_pos
+
diff --git a/core/arch/arm/plat-mediatek/mt8173_core_pos_a64.S b/core/arch/arm/plat-mediatek/mt8173_core_pos_a64.S
new file mode 100644
index 0000000..a1c3b3e
--- /dev/null
+++ b/core/arch/arm/plat-mediatek/mt8173_core_pos_a64.S
@@ -0,0 +1,47 @@
+/*
+ * 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 <asm.S>
+#include <arm.h>
+
+FUNC get_core_pos , :
+	mrs	x0, mpidr_el1
+	and	x1, x0, #MPIDR_CPU_MASK
+	and	x0, x0, #MPIDR_CLUSTER_MASK
+	/*
+	 * Number of cores in cluster is 2,
+	 * we should have the following mapping:
+	 * MPIDR     core_pos
+	 * 0x0000 -> 0
+	 * 0x0001 -> 1
+	 * 0x0100 -> 2
+	 * 0x0101 -> 3
+	 */
+	add	x0, x1, x0, LSR #7
+	ret
+END_FUNC get_core_pos
+
diff --git a/core/arch/arm/plat-mediatek/platform_config.h b/core/arch/arm/plat-mediatek/platform_config.h
new file mode 100644
index 0000000..7fe2f93
--- /dev/null
+++ b/core/arch/arm/plat-mediatek/platform_config.h
@@ -0,0 +1,108 @@
+/*
+ * 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.
+ */
+
+#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*/
+
+#if defined(PLATFORM_FLAVOR_mt8173)
+
+#define GIC_BASE		0x10220000
+#define GICC_OFFSET		0x2000
+#define GICD_OFFSET		0x1000
+
+#define UART0_BASE		0x11002000
+#define UART1_BASE		0x11003000
+#define UART2_BASE		0x11004000
+#define UART3_BASE		0x11005000
+
+#define CONSOLE_UART_BASE	UART0_BASE
+#define CONSOLE_BAUDRATE	921600
+#define CONSOLE_UART_CLK_IN_HZ	26000000
+
+#define DRAM0_BASE		0x40000000
+#define DRAM0_SIZE		0x80000000
+
+/* Location of trusted dram */
+#define TZDRAM_BASE		0xBE000000
+#define TZDRAM_SIZE		0x02000000
+
+#define CFG_TEE_CORE_NB_CORE	4
+
+/* Full GlobalPlatform test suite requires CFG_SHMEM_SIZE to be at least 2MB */
+#define CFG_SHMEM_START		(TZDRAM_BASE - 0x200000)
+#define CFG_SHMEM_SIZE		0x200000
+
+#else
+#error "Unknown platform flavor"
+#endif
+
+#define CFG_TEE_RAM_VA_SIZE	(1024 * 1024)
+
+#ifndef CFG_TEE_LOAD_ADDR
+#define CFG_TEE_LOAD_ADDR	CFG_TEE_RAM_START
+#endif
+
+/*
+ * Everything is in TZDRAM.
+ * +------------------+
+ * |        | TEE_RAM |
+ * + TZDRAM +---------+
+ * |        | TA_RAM  |
+ * +--------+---------+
+ */
+#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		ROUNDDOWN((TZDRAM_SIZE - CFG_TEE_RAM_VA_SIZE), \
+					  CORE_MMU_DEVICE_SIZE)
+
+#define DEVICE0_PA_BASE		ROUNDDOWN(CONSOLE_UART_BASE, \
+					  CORE_MMU_DEVICE_SIZE)
+#define DEVICE0_VA_BASE		DEVICE0_PA_BASE
+#define DEVICE0_SIZE		CORE_MMU_DEVICE_SIZE
+#define DEVICE0_TYPE		MEM_AREA_IO_NSEC
+
+#define DEVICE1_PA_BASE		ROUNDDOWN(GIC_BASE, CORE_MMU_DEVICE_SIZE)
+#define DEVICE1_VA_BASE		DEVICE1_PA_BASE
+#define DEVICE1_SIZE		CORE_MMU_DEVICE_SIZE
+#define DEVICE1_TYPE		MEM_AREA_IO_SEC
+
+#ifdef CFG_WITH_LPAE
+#define MAX_XLAT_TABLES		5
+#endif
+
+#endif /*PLATFORM_CONFIG_H*/
diff --git a/core/arch/arm/plat-mediatek/sub.mk b/core/arch/arm/plat-mediatek/sub.mk
new file mode 100644
index 0000000..95a8571
--- /dev/null
+++ b/core/arch/arm/plat-mediatek/sub.mk
@@ -0,0 +1,6 @@
+global-incdirs-y += .
+srcs-y += main.c
+ifeq ($(PLATFORM_FLAVOR),mt8173)
+srcs-$(CFG_ARM32_core) += mt8173_core_pos_a32.S
+srcs-$(CFG_ARM64_core) += mt8173_core_pos_a64.S
+endif
diff --git a/core/arch/arm/plat-rcar/conf.mk b/core/arch/arm/plat-rcar/conf.mk
new file mode 100644
index 0000000..caf26c8
--- /dev/null
+++ b/core/arch/arm/plat-rcar/conf.mk
@@ -0,0 +1,27 @@
+PLATFORM_FLAVOR ?= h3
+
+# 32-bit flags
+arm32-platform-cpuarch	:= cortex-a57
+arm32-platform-cflags	+= -mcpu=$(arm32-platform-cpuarch)
+arm32-platform-aflags	+= -mcpu=$(arm32-platform-cpuarch)
+arm32-platform-aflags	+= -mfpu=neon
+
+$(call force,CFG_GENERIC_BOOT,y)
+$(call force,CFG_HWSUPP_MEM_PERM_PXN,y)
+$(call force,CFG_PM_STUBS,y)
+$(call force,CFG_SECURE_TIME_SOURCE_CNTPCT,y)
+$(call force,CFG_WITH_ARM_TRUSTED_FW,y)
+$(call force,CFG_SCIF,y)
+
+ifeq ($(CFG_ARM64_core),y)
+$(call force,CFG_WITH_LPAE,y)
+ta-targets += ta_arm64
+else
+$(call force,CFG_ARM32_core,y)
+endif
+
+ifeq ($(CFG_ARM32_core),y)
+ta-targets = ta_arm32
+endif
+
+CFG_WITH_STACK_CANARIES ?= y
diff --git a/core/arch/arm/plat-rcar/kern.ld.S b/core/arch/arm/plat-rcar/kern.ld.S
new file mode 100644
index 0000000..8d794ee
--- /dev/null
+++ b/core/arch/arm/plat-rcar/kern.ld.S
@@ -0,0 +1 @@
+#include "../kernel/kern.ld.S"
diff --git a/core/arch/arm/plat-rcar/link.mk b/core/arch/arm/plat-rcar/link.mk
new file mode 100644
index 0000000..763298f
--- /dev/null
+++ b/core/arch/arm/plat-rcar/link.mk
@@ -0,0 +1,7 @@
+include core/arch/arm/kernel/link.mk
+
+all: $(link-out-dir)/tee.srec
+cleanfiles += $(link-out-dir)/tee.srec
+$(link-out-dir)/tee.srec: $(link-out-dir)/tee.elf
+	@$(cmd-echo-silent) '  GEN     $@'
+	$(q)$(OBJCOPYcore) -O srec $< $@
diff --git a/core/arch/arm/plat-rcar/main.c b/core/arch/arm/plat-rcar/main.c
new file mode 100644
index 0000000..8f9482e
--- /dev/null
+++ b/core/arch/arm/plat-rcar/main.c
@@ -0,0 +1,95 @@
+/*
+ * Copyright (c) 2016, GlobalLogic
+ * 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 <console.h>
+#include <kernel/generic_boot.h>
+#include <kernel/panic.h>
+#include <kernel/pm_stubs.h>
+#include <mm/core_memprot.h>
+#include <platform_config.h>
+#include <stdint.h>
+#include <tee/entry_std.h>
+#include <tee/entry_fast.h>
+#include <drivers/scif.h>
+#include <drivers/gic.h>
+
+register_phys_mem(MEM_AREA_IO_SEC, CONSOLE_UART_BASE, SCIF_REG_SIZE);
+register_phys_mem(MEM_AREA_IO_SEC, GICD_BASE, GIC_DIST_REG_SIZE);
+register_phys_mem(MEM_AREA_IO_SEC, GICC_BASE, GIC_DIST_REG_SIZE);
+
+static void main_fiq(void);
+
+static const struct thread_handlers handlers = {
+	.std_smc = tee_entry_std,
+	.fast_smc = tee_entry_fast,
+	.fiq = 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;
+}
+
+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());
+}
diff --git a/core/arch/arm/plat-rcar/platform_config.h b/core/arch/arm/plat-rcar/platform_config.h
new file mode 100644
index 0000000..287689d
--- /dev/null
+++ b/core/arch/arm/plat-rcar/platform_config.h
@@ -0,0 +1,81 @@
+/*
+ * Copyright (c) 2016, GlobalLogic
+ * 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 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*/
+
+#define GIC_BASE		0xF1000000
+#define GICC_BASE		0xF1020000
+#define GICD_BASE		0xF1010000
+
+#define CONSOLE_UART_BASE	0xE6E88000
+
+#define DRAM0_BASE		0x44000000
+#define DRAM0_SIZE		0x04000000
+
+/* Location of trusted dram */
+#define TZDRAM_BASE		0x44000000
+#define TZDRAM_SIZE		0x03E00000
+
+#define CFG_TEE_CORE_NB_CORE	8
+
+/* Full GlobalPlatform test suite requires CFG_SHMEM_SIZE to be at least 2MB */
+#define CFG_SHMEM_START		(TZDRAM_BASE + TZDRAM_SIZE)
+#define CFG_SHMEM_SIZE		0x100000
+
+#define CFG_TEE_RAM_VA_SIZE	(1024 * 1024)
+
+#ifndef CFG_TEE_LOAD_ADDR
+#define CFG_TEE_LOAD_ADDR	CFG_TEE_RAM_START
+#endif
+
+/*
+ * Everything is in TZDRAM.
+ * +------------------+
+ * |        | TEE_RAM |
+ * + TZDRAM +---------+
+ * |        | TA_RAM  |
+ * +--------+---------+
+ */
+#define CFG_TEE_RAM_PH_SIZE	CFG_TEE_RAM_VA_SIZE
+#define CFG_TEE_RAM_START	(TZDRAM_BASE + 0x00100000)
+#define CFG_TA_RAM_START	ROUNDUP((CFG_TEE_RAM_START + \
+					CFG_TEE_RAM_VA_SIZE), \
+					CORE_MMU_DEVICE_SIZE)
+#define CFG_TA_RAM_SIZE		ROUNDDOWN((TZDRAM_SIZE - CFG_TEE_RAM_VA_SIZE), \
+					  CORE_MMU_DEVICE_SIZE)
+
+#endif /*PLATFORM_CONFIG_H*/
diff --git a/core/arch/arm/plat-rcar/sub.mk b/core/arch/arm/plat-rcar/sub.mk
new file mode 100644
index 0000000..8ddc2fd
--- /dev/null
+++ b/core/arch/arm/plat-rcar/sub.mk
@@ -0,0 +1,2 @@
+global-incdirs-y += .
+srcs-y += main.c
diff --git a/core/arch/arm/plat-rpi3/conf.mk b/core/arch/arm/plat-rpi3/conf.mk
new file mode 100644
index 0000000..567680a
--- /dev/null
+++ b/core/arch/arm/plat-rpi3/conf.mk
@@ -0,0 +1,39 @@
+# 32-bit flags
+arm32-platform-cpuarch		:= cortex-a53
+arm32-platform-cflags		+= -mcpu=$(arm32-platform-cpuarch)
+arm32-platform-aflags		+= -mcpu=$(arm32-platform-cpuarch)
+core_arm32-platform-aflags	+= -mfpu=neon
+
+$(call force,CFG_8250_UART,y)
+$(call force,CFG_GENERIC_BOOT,y)
+$(call force,CFG_HWSUPP_MEM_PERM_PXN,y)
+$(call force,CFG_PM_STUBS,y)
+$(call force,CFG_SECURE_TIME_SOURCE_CNTPCT,y)
+$(call force,CFG_WITH_ARM_TRUSTED_FW,y)
+
+ta-targets = ta_arm32
+
+ifeq ($(CFG_ARM64_core),y)
+$(call force,CFG_WITH_LPAE,y)
+ta-targets += ta_arm64
+else
+$(call force,CFG_ARM32_core,y)
+endif
+
+CFG_NUM_THREADS ?= 4
+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
+
+arm32-platform-cflags += -Wno-error=cast-align
+arm64-platform-cflags += -Wno-error=cast-align
+
+$(call force,CFG_CRYPTO_SHA256_ARM32_CE,n)
+$(call force,CFG_CRYPTO_SHA256_ARM64_CE,n)
+$(call force,CFG_CRYPTO_SHA1_ARM32_CE,n)
+$(call force,CFG_CRYPTO_SHA1_ARM64_CE,n)
+$(call force,CFG_CRYPTO_AES_ARM64_CE,n)
diff --git a/core/arch/arm/plat-rpi3/kern.ld.S b/core/arch/arm/plat-rpi3/kern.ld.S
new file mode 100644
index 0000000..8d794ee
--- /dev/null
+++ b/core/arch/arm/plat-rpi3/kern.ld.S
@@ -0,0 +1 @@
+#include "../kernel/kern.ld.S"
diff --git a/core/arch/arm/plat-rpi3/link.mk b/core/arch/arm/plat-rpi3/link.mk
new file mode 100644
index 0000000..448ab89
--- /dev/null
+++ b/core/arch/arm/plat-rpi3/link.mk
@@ -0,0 +1 @@
+include core/arch/arm/kernel/link.mk
diff --git a/core/arch/arm/plat-rpi3/main.c b/core/arch/arm/plat-rpi3/main.c
new file mode 100644
index 0000000..9270e19
--- /dev/null
+++ b/core/arch/arm/plat-rpi3/main.c
@@ -0,0 +1,94 @@
+/*
+ * Copyright (c) 2016, Sequitur Labs Inc. All rights reserved.
+ * 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 <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,
+	.fiq = 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;
+}
+
+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());
+}
diff --git a/core/arch/arm/plat-rpi3/platform_config.h b/core/arch/arm/plat-rpi3/platform_config.h
new file mode 100644
index 0000000..ac53e26
--- /dev/null
+++ b/core/arch/arm/plat-rpi3/platform_config.h
@@ -0,0 +1,95 @@
+/*
+ * Copyright (c) 2016, Sequitur Labs Inc. All rights reserved.
+ * 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 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 */
+
+/* 16550 UART */
+#define CONSOLE_UART_BASE	0x3f215040 /* UART0 */
+#define CONSOLE_BAUDRATE	115200
+#define CONSOLE_UART_CLK_IN_HZ	19200000
+
+/*
+ * RPi memory map
+ *
+ * No secure memory on RPi...
+ *
+ *
+ *    Available to Linux <above>
+ *  0x0a00_0000
+ *    TA RAM: 16 MiB                          |
+ *  0x0842_0000                               | TZDRAM
+ *    TEE RAM: 4 MiB (CFG_TEE_RAM_VA_SIZE)    |
+ *  0x0840_0000 [ARM Trusted Firmware       ] -
+ *  0x0840_0000 [TZDRAM_BASE, BL32_LOAD_ADDR] -
+ *    Shared memory: 4 MiB                    |
+ *  0x0800_0000                               | DRAM0
+ *    Available to Linux                      |
+ *  0x0000_0000 [DRAM0_BASE]                  -
+ *
+ */
+
+#define DRAM0_BASE		0x00000000
+#define DRAM0_SIZE		0x40000000
+
+/* Below ARM-TF */
+#define CFG_SHMEM_START		(0x08000000)
+#define CFG_SHMEM_SIZE		(4 * 1024 * 1024)
+
+#define TZDRAM_BASE		(CFG_SHMEM_START + CFG_SHMEM_SIZE)
+#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)
+
+#define DEVICE0_BASE		ROUNDDOWN(CONSOLE_UART_BASE, \
+					  CORE_MMU_DEVICE_SIZE)
+#define DEVICE0_PA_BASE         DEVICE0_BASE
+#define DEVICE0_SIZE		CORE_MMU_DEVICE_SIZE
+#define DEVICE0_TYPE		MEM_AREA_IO_NSEC
+
+#endif /* PLATFORM_CONFIG_H */
diff --git a/core/arch/arm/plat-rpi3/sub.mk b/core/arch/arm/plat-rpi3/sub.mk
new file mode 100644
index 0000000..8ddc2fd
--- /dev/null
+++ b/core/arch/arm/plat-rpi3/sub.mk
@@ -0,0 +1,2 @@
+global-incdirs-y += .
+srcs-y += main.c
diff --git a/core/arch/arm/plat-sprd/conf.mk b/core/arch/arm/plat-sprd/conf.mk
new file mode 100644
index 0000000..137214f
--- /dev/null
+++ b/core/arch/arm/plat-sprd/conf.mk
@@ -0,0 +1,28 @@
+PLATFORM_FLAVOR ?= sc9860
+
+# 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
+
+$(call force,CFG_WITH_ARM_TRUSTED_FW,y)
+
+ta-targets = ta_arm32
+
+ifeq ($(CFG_ARM64_core),y)
+$(call force,CFG_WITH_LPAE,y)
+ta-targets += ta_arm64
+else
+$(call force,CFG_ARM32_core,y)
+endif
+
+$(call force,CFG_GENERIC_BOOT,y)
+$(call force,CFG_GIC,y)
+$(call force,CFG_SPRD_UART,y)
+$(call force,CFG_PM_STUBS,y)
+$(call force,CFG_SECURE_TIME_SOURCE_CNTPCT,y)
+
+CFG_WITH_STACK_CANARIES ?= y
+# Overrides default in mk/config.mk with 128 kB
+CFG_CORE_HEAP_SIZE ?= 131072
diff --git a/core/arch/arm/plat-sprd/console.c b/core/arch/arm/plat-sprd/console.c
new file mode 100644
index 0000000..3263d69
--- /dev/null
+++ b/core/arch/arm/plat-sprd/console.c
@@ -0,0 +1,58 @@
+/*
+ * Copyright (c) 2016, Spreadtrum Communications Inc.
+ * 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 <console.h>
+#include <drivers/sprd_uart.h>
+#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;
+}
+
+/* Do nothing in this function */
+void console_init(void)
+{
+}
+
+void console_putc(int ch)
+{
+	sprd_uart_putc(console_base(), (unsigned char)(ch & 0xff));
+}
+
+void console_flush(void)
+{
+	sprd_uart_flush(console_base());
+}
diff --git a/core/arch/arm/plat-sprd/kern.ld.S b/core/arch/arm/plat-sprd/kern.ld.S
new file mode 100644
index 0000000..8d794ee
--- /dev/null
+++ b/core/arch/arm/plat-sprd/kern.ld.S
@@ -0,0 +1 @@
+#include "../kernel/kern.ld.S"
diff --git a/core/arch/arm/plat-sprd/link.mk b/core/arch/arm/plat-sprd/link.mk
new file mode 100644
index 0000000..448ab89
--- /dev/null
+++ b/core/arch/arm/plat-sprd/link.mk
@@ -0,0 +1 @@
+include core/arch/arm/kernel/link.mk
diff --git a/core/arch/arm/plat-sprd/main.c b/core/arch/arm/plat-sprd/main.c
new file mode 100644
index 0000000..bf3a62d
--- /dev/null
+++ b/core/arch/arm/plat-sprd/main.c
@@ -0,0 +1,79 @@
+/*
+ * Copyright (c) 2016, Spreadtrum Communications Inc.
+ * 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 <drivers/gic.h>
+#include <kernel/generic_boot.h>
+#include <kernel/panic.h>
+#include <kernel/pm_stubs.h>
+#include <mm/core_memprot.h>
+#include <platform_config.h>
+#include <trace.h>
+#include <tee/entry_fast.h>
+#include <tee/entry_std.h>
+
+static void main_fiq(void);
+
+static const struct thread_handlers handlers = {
+	.std_smc = tee_entry_std,
+	.fast_smc = tee_entry_fast,
+	.fiq = 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,
+};
+
+static struct gic_data gic_data;
+
+const struct thread_handlers *generic_boot_get_handlers(void)
+{
+	return &handlers;
+}
+
+void main_init_gic(void)
+{
+	vaddr_t gicc_base;
+	vaddr_t gicd_base;
+
+	gicc_base = (vaddr_t)phys_to_virt(GIC_BASE + GICC_OFFSET,
+					  MEM_AREA_IO_SEC);
+	gicd_base = (vaddr_t)phys_to_virt(GIC_BASE + GICD_OFFSET,
+					  MEM_AREA_IO_SEC);
+	if (!gicc_base || !gicd_base)
+		panic();
+
+	gic_init_base_addr(&gic_data, gicc_base, gicd_base);
+
+	itr_init(&gic_data.chip);
+}
+
+static void main_fiq(void)
+{
+	gic_it_handle(&gic_data);
+}
diff --git a/core/arch/arm/plat-sprd/platform_config.h b/core/arch/arm/plat-sprd/platform_config.h
new file mode 100644
index 0000000..b30583e
--- /dev/null
+++ b/core/arch/arm/plat-sprd/platform_config.h
@@ -0,0 +1,105 @@
+/*
+ * Copyright (c) 2016, Spreadtrum Communications Inc.
+ * 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 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*/
+
+#if defined(PLATFORM_FLAVOR_sc9860)
+
+#define GIC_BASE		0x12000000
+#define UART0_BASE		0x70000000
+#define UART1_BASE		0x70100000
+#define UART2_BASE		0x70200000
+#define UART3_BASE		0x70300000
+
+#define CONSOLE_UART_BASE	UART1_BASE
+
+#define DRAM0_BASE		0x80000000
+#define DRAM0_SIZE		0x20000000
+
+#define TZDRAM_BASE		0x8f600000
+#define TZDRAM_SIZE		(0x02000000 - CFG_SHMEM_SIZE)
+
+#define CFG_TEE_CORE_NB_CORE	8
+
+#define CFG_SHMEM_START		(TZDRAM_BASE + TZDRAM_SIZE)
+#define CFG_SHMEM_SIZE		0x200000
+
+#define GICC_OFFSET		0x2000
+#define GICD_OFFSET		0x1000
+
+#else
+#error "Unknown platform flavor"
+#endif
+
+#define CFG_TEE_RAM_VA_SIZE	(1024 * 1024)
+
+#ifndef CFG_TEE_LOAD_ADDR
+#define CFG_TEE_LOAD_ADDR	CFG_TEE_RAM_START
+#endif
+
+/*
+ * +------------------+
+ * |        | TEE_RAM |
+ * + TZDRAM +---------+
+ * |        | TA_RAM  |
+ * +--------+---------+
+ */
+#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		ROUNDDOWN((TZDRAM_SIZE - CFG_TEE_RAM_VA_SIZE), \
+					  CORE_MMU_DEVICE_SIZE)
+
+#define DEVICE0_PA_BASE		ROUNDDOWN(CONSOLE_UART_BASE, \
+					  CORE_MMU_DEVICE_SIZE)
+#define DEVICE0_VA_BASE		DEVICE0_PA_BASE
+#define DEVICE0_SIZE		CORE_MMU_DEVICE_SIZE
+#define DEVICE0_TYPE		MEM_AREA_IO_NSEC
+
+#define DEVICE1_PA_BASE		ROUNDDOWN(GIC_BASE, CORE_MMU_DEVICE_SIZE)
+#define DEVICE1_VA_BASE		DEVICE1_PA_BASE
+#define DEVICE1_SIZE		CORE_MMU_DEVICE_SIZE
+#define DEVICE1_TYPE		MEM_AREA_IO_SEC
+
+#define DEVICE2_PA_BASE		ROUNDDOWN(GIC_BASE + GICD_OFFSET, \
+					  CORE_MMU_DEVICE_SIZE)
+#define DEVICE2_VA_BASE		DEVICE2_PA_BASE
+#define DEVICE2_SIZE		CORE_MMU_DEVICE_SIZE
+#define DEVICE2_TYPE		MEM_AREA_IO_SEC
+
+#endif /*PLATFORM_CONFIG_H*/
diff --git a/core/arch/arm/plat-sprd/sub.mk b/core/arch/arm/plat-sprd/sub.mk
new file mode 100644
index 0000000..3a8214b
--- /dev/null
+++ b/core/arch/arm/plat-sprd/sub.mk
@@ -0,0 +1,3 @@
+global-incdirs-y += .
+srcs-y += main.c
+srcs-y += console.c
diff --git a/core/arch/arm/plat-stm/.gitignore b/core/arch/arm/plat-stm/.gitignore
new file mode 100644
index 0000000..49b7bb9
--- /dev/null
+++ b/core/arch/arm/plat-stm/.gitignore
@@ -0,0 +1 @@
+System.map
diff --git a/core/arch/arm/plat-stm/asc.S b/core/arch/arm/plat-stm/asc.S
new file mode 100644
index 0000000..3f2b6aa
--- /dev/null
+++ b/core/arch/arm/plat-stm/asc.S
@@ -0,0 +1,108 @@
+/*
+ * 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/asc.h b/core/arch/arm/plat-stm/asc.h
new file mode 100644
index 0000000..bbf574c
--- /dev/null
+++ b/core/arch/arm/plat-stm/asc.h
@@ -0,0 +1,35 @@
+/*
+ * 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.
+ */
+#ifndef ASC_H
+#define ASC_H
+
+#include <types_ext.h>
+
+extern int __asc_xmit_char(const char p, vaddr_t base);
+extern void __asc_flush(vaddr_t base);
+
+#endif
diff --git a/core/arch/arm/plat-stm/conf.mk b/core/arch/arm/plat-stm/conf.mk
new file mode 100644
index 0000000..4afe256
--- /dev/null
+++ b/core/arch/arm/plat-stm/conf.mk
@@ -0,0 +1,30 @@
+PLATFORM_FLAVOR ?= b2260
+
+arm32-platform-cpuarch		:= cortex-a9
+arm32-platform-cflags		+= -mcpu=$(arm32-platform-cpuarch)
+arm32-platform-aflags		+= -mcpu=$(arm32-platform-cpuarch)
+core_arm32-platform-aflags	+= -mfpu=neon
+
+$(call force,CFG_ARM32_core,y)
+$(call force,CFG_SECURE_TIME_SOURCE_REE,y)
+$(call force,CFG_PL310,y)
+$(call force,CFG_CACHE_API,y)
+$(call force,CFG_PM_STUBS,y)
+$(call force,CFG_GENERIC_BOOT,y)
+$(call force,CFG_WITH_LPAE,n)
+$(call force,CFG_GIC,y)
+
+ta-targets = ta_arm32
+
+CFG_WITH_PAGER ?= n
+CFG_BOOT_SYNC_CPU ?= y
+CFG_TEE_CORE_EMBED_INTERNAL_TESTS ?= y
+CFG_WITH_STACK_CANARIES ?= y
+CFG_WITH_STATS ?= y
+CFG_WITH_SOFTWARE_PRNG ?= n
+
+ifeq ($(PLATFORM_FLAVOR),b2260)
+CFG_PL310_LOCKED ?= y
+else
+CFG_PL310_LOCKED ?= n
+endif
diff --git a/core/arch/arm/plat-stm/kern.ld.S b/core/arch/arm/plat-stm/kern.ld.S
new file mode 100644
index 0000000..8d794ee
--- /dev/null
+++ b/core/arch/arm/plat-stm/kern.ld.S
@@ -0,0 +1 @@
+#include "../kernel/kern.ld.S"
diff --git a/core/arch/arm/plat-stm/link.mk b/core/arch/arm/plat-stm/link.mk
new file mode 100644
index 0000000..448ab89
--- /dev/null
+++ b/core/arch/arm/plat-stm/link.mk
@@ -0,0 +1 @@
+include core/arch/arm/kernel/link.mk
diff --git a/core/arch/arm/plat-stm/main.c b/core/arch/arm/plat-stm/main.c
new file mode 100644
index 0000000..e569e07
--- /dev/null
+++ b/core/arch/arm/plat-stm/main.c
@@ -0,0 +1,201 @@
+/*
+ * Copyright (c) 2014-2016, 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 <arm32.h>
+#include <asc.h>
+#include <console.h>
+#include <drivers/gic.h>
+#include <drivers/pl011.h>
+#include <io.h>
+#include <kernel/generic_boot.h>
+#include <kernel/misc.h>
+#include <kernel/panic.h>
+#include <kernel/pm_stubs.h>
+#include <kernel/tz_ssvce_pl310.h>
+#include <mm/core_mmu.h>
+#include <mm/core_memprot.h>
+#include <platform_config.h>
+#include <stdint.h>
+#include <tee/entry_std.h>
+#include <tee/entry_fast.h>
+#include <trace.h>
+#include <util.h>
+
+register_phys_mem(MEM_AREA_IO_SEC, CPU_IOMEM_BASE, CORE_MMU_DEVICE_SIZE);
+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 void main_fiq(void);
+
+#if defined(PLATFORM_FLAVOR_b2260)
+#define stm_tee_entry_std	tee_entry_std
+static bool ns_resources_ready(void)
+{
+	return true;
+}
+#else
+/* some nonsecure resource might not be ready (uart) */
+static int boot_is_completed __early_bss;
+static bool ns_resources_ready(void)
+{
+	return !!boot_is_completed;
+}
+static void stm_tee_entry_std(struct thread_smc_args *smc_args)
+{
+	boot_is_completed = 1;
+	tee_entry_std(smc_args);
+}
+#endif
+
+static const struct thread_handlers handlers = {
+	.std_smc = stm_tee_entry_std,
+	.fast_smc = tee_entry_fast,
+	.fiq = main_fiq,
+	.cpu_on = pm_panic,
+	.cpu_off = pm_panic,
+	.cpu_suspend = pm_panic,
+	.cpu_resume = pm_panic,
+	.system_off = pm_panic,
+	.system_reset = pm_panic,
+};
+
+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)
+{
+}
+
+void console_putc(int ch)
+{
+	if (ns_resources_ready()) {
+		if (ch == '\n')
+			__asc_xmit_char('\r', console_base());
+		__asc_xmit_char((char)ch, console_base());
+	}
+}
+
+void console_flush(void)
+{
+	if (ns_resources_ready())
+		__asc_flush(console_base());
+}
+
+vaddr_t pl310_base(void)
+{
+	static void *va __early_bss;
+
+	if (cpu_mmu_enabled()) {
+		if (!va)
+			va = phys_to_virt(PL310_BASE, MEM_AREA_IO_SEC);
+		return (vaddr_t)va;
+	}
+	return PL310_BASE;
+}
+
+void arm_cl2_config(vaddr_t pl310)
+{
+	/* pl310 off */
+	write32(0, pl310 + PL310_CTRL);
+
+	/* config PL310 */
+	write32(PL310_TAG_RAM_CTRL_INIT, pl310 + PL310_TAG_RAM_CTRL);
+	write32(PL310_DATA_RAM_CTRL_INIT, pl310 + PL310_DATA_RAM_CTRL);
+	write32(PL310_AUX_CTRL_INIT, pl310 + PL310_AUX_CTRL);
+	write32(PL310_PREFETCH_CTRL_INIT, pl310 + PL310_PREFETCH_CTRL);
+	write32(PL310_POWER_CTRL_INIT, pl310 + PL310_POWER_CTRL);
+
+	/* invalidate all pl310 cache ways */
+	arm_cl2_invbyway(pl310);
+}
+
+void plat_cpu_reset_late(void)
+{
+	int i;
+
+	assert(!cpu_mmu_enabled());
+
+	/* Allow NSec to Imprecise abort */
+	write_scr(SCR_AW);
+
+	if (get_core_pos())
+		return;
+
+	write32(SCU_SAC_INIT, SCU_BASE + SCU_SAC);
+	write32(SCU_NSAC_INIT, SCU_BASE + SCU_NSAC);
+	write32(CPU_PORT_FILT_END, SCU_BASE + SCU_FILT_EA);
+	write32(CPU_PORT_FILT_START, SCU_BASE + SCU_FILT_SA);
+	write32(SCU_CTRL_INIT, SCU_BASE + SCU_CTRL);
+
+	write32(CPU_PORT_FILT_END, pl310_base() + PL310_ADDR_FILT_END);
+	write32(CPU_PORT_FILT_START | PL310_CTRL_ENABLE_BIT,
+				   pl310_base() + PL310_ADDR_FILT_START);
+
+	/* TODO: gic_init scan fails, pre-init all SPIs are nonsecure */
+	for (i = 0; i < (31 * 4); i += 4)
+		write32(0xFFFFFFFF, GIC_DIST_BASE + GIC_DIST_ISR1 + i);
+}
+
+void main_init_gic(void)
+{
+	vaddr_t gicc_base;
+	vaddr_t gicd_base;
+
+	gicc_base = (vaddr_t)phys_to_virt(GIC_CPU_BASE, MEM_AREA_IO_SEC);
+	gicd_base = (vaddr_t)phys_to_virt(GIC_DIST_BASE, MEM_AREA_IO_SEC);
+
+	if (!gicc_base || !gicd_base)
+		panic();
+
+	gic_init(&gic_data, gicc_base, gicd_base);
+	itr_init(&gic_data.chip);
+}
+
+void main_secondary_init_gic(void)
+{
+	gic_cpu_init(&gic_data);
+}
+
+static void main_fiq(void)
+{
+	gic_it_handle(&gic_data);
+}
diff --git a/core/arch/arm/plat-stm/platform_config.h b/core/arch/arm/plat-stm/platform_config.h
new file mode 100644
index 0000000..407a412
--- /dev/null
+++ b/core/arch/arm/plat-stm/platform_config.h
@@ -0,0 +1,340 @@
+/*
+ * Copyright (c) 2014-2016, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef PLATFORM_CONFIG_H
+#define PLATFORM_CONFIG_H
+
+/* Below are platform/SoC settings specific to stm platform flavors */
+
+#if defined(PLATFORM_FLAVOR_b2260)
+
+#define CFG_TEE_CORE_NB_CORE	2
+
+#ifndef CFG_DDR_START
+#define CFG_DDR_START		0x40000000
+#define CFG_DDR_SIZE		0x40000000
+#endif
+#ifndef CFG_DDR_TEETZ_RESERVED_START
+#define CFG_DDR_TEETZ_RESERVED_START	0x7E000000
+#define CFG_DDR_TEETZ_RESERVED_SIZE	0x01E00000
+#endif
+#ifndef CFG_CORE_TZSRAM_EMUL_START
+#define CFG_CORE_TZSRAM_EMUL_START	0x7FE00000
+#endif
+
+#define CPU_IOMEM_BASE		0x08760000
+#define CPU_PORT_FILT_START	0x40000000
+#define CPU_PORT_FILT_END	0xC0000000
+#define STXHxxx_LPM_PERIPH_BASE	0x09700000
+#define RNG_BASE		0x08A89000
+
+#define ASC_NUM			21
+#define UART_CONSOLE_BASE	ST_ASC21_REGS_BASE
+
+#elif defined(PLATFORM_FLAVOR_cannes)
+
+#define CFG_TEE_CORE_NB_CORE	2
+
+#ifndef CFG_DDR_START
+#define CFG_DDR_START		0x40000000
+#define CFG_DDR_SIZE		0x80000000
+#endif
+#ifndef CFG_DDR_TEETZ_RESERVED_START
+#define CFG_DDR_TEETZ_RESERVED_START	0x93a00000
+#define CFG_DDR_TEETZ_RESERVED_SIZE	0x01000000
+#endif
+#ifndef CFG_CORE_TZSRAM_EMUL_START
+#define CFG_CORE_TZSRAM_EMUL_START	0x94a00000
+#endif
+
+#define CPU_IOMEM_BASE		0x08760000
+#define CPU_PORT_FILT_START	0x40000000
+#define CPU_PORT_FILT_END	0xC0000000
+#define STXHxxx_LPM_PERIPH_BASE	0x09400000
+#define RNG_BASE		0x08A89000
+
+#define ASC_NUM			20
+#define UART_CONSOLE_BASE	ST_ASC20_REGS_BASE
+
+#elif defined(PLATFORM_FLAVOR_orly2)
+
+#define CFG_TEE_CORE_NB_CORE	2
+
+#ifndef CFG_DDR_START
+#define CFG_DDR_START		0x40000000
+#define CFG_DDR_SIZE		0x40000000
+#define CFG_DDR1_START		0x80000000
+#define CFG_DDR1_SIZE		0x40000000
+#endif
+#ifndef CFG_DDR_TEETZ_RESERVED_START
+#define CFG_DDR_TEETZ_RESERVED_START	0x8F000000
+#define CFG_DDR_TEETZ_RESERVED_SIZE	0x00800000
+#endif
+
+#define CPU_IOMEM_BASE		0xFFFE0000
+#define CPU_PORT_FILT_START	0x40000000
+#define CPU_PORT_FILT_END	0x80000000
+#define STXHxxx_LPM_PERIPH_BASE	0xFE400000
+#define RNG_BASE		0xFEE80000
+
+#define ASC_NUM			21
+#define UART_CONSOLE_BASE	ST_ASC21_REGS_BASE
+
+#else /* defined(PLATFORM_FLAVOR_xxx) */
+
+#error "Unknown platform flavor"
+
+#endif /* defined(PLATFORM_FLAVOR_xxx) */
+
+/* Below are settings common to stm platform flavors */
+
+/*
+ * CP15 Secure ConTroL Register (SCTLR
+ *
+ * - Round-Robin replac. for icache, btac, i/duTLB (bit14: RoundRobin)
+ */
+#define CPU_SCTLR_INIT			0x00004000
+
+/*
+ * CP15 Auxiliary ConTroL Register (ACTRL)
+ *
+ * - core always in full SMP (FW bit0=1, SMP bit6=1)
+ * - L2 write full line of zero disabled (bit3=0)
+ *   (keep WFLZ low. Will be set once outer L2 is ready)
+ */
+
+#define CPU_ACTLR_INIT			0x00000041
+
+/*
+ * CP15 NonSecure Access Control Register (NSACR)
+ *
+ * - NSec cannot change ACTRL.SMP (NS_SMP bit18=0)
+ * - Nsec can lockdown TLB (TL bit17=1)
+ * - NSec cannot access PLE (PLE bit16=0)
+ * - NSec can use SIMD/VFP (CP10/CP11) (bit15:14=2b00, bit11:10=2b11)
+ */
+#define CPU_NSACR_INIT			0x00020C00
+
+/*
+ * CP15 Power Control Register (PCR)
+ *
+ * - no change latency, enable clk gating
+ */
+#define CPU_PCR_INIT			0x00000001
+
+
+/*
+ * SCU Secure Access Control / NonSecure Access Control
+ *
+ * SAC:  Both secure CPU access SCU (bit[3:0]).
+ * NSAC: Both nonsec cpu access SCU (bit[3:0]), private timers (bit[7:4])
+ *       and global timers (bit[11:8]).
+ */
+#if !defined(SCU_SAC_INIT) || !defined(SCU_NSAC_INIT)
+#define SCU_CPUS_MASK		(SHIFT_U32(1, CFG_TEE_CORE_NB_CORE) - 1)
+
+#define SCU_SAC_INIT	SCU_CPUS_MASK
+#define SCU_NSAC_INIT	(SHIFT_U32(SCU_CPUS_MASK, SCU_NSAC_SCU_SHIFT) | \
+			SHIFT_U32(SCU_CPUS_MASK, SCU_NSAC_PTIMER_SHIFT) | \
+			SHIFT_U32(SCU_CPUS_MASK, SCU_NSAC_GTIMER_SHIFT))
+#endif
+
+/*
+ * PL310 TAG RAM Control Register
+ *
+ * bit[10:8]:1 - 2 cycle of write accesses latency
+ * bit[6:4]:1 - 2 cycle of read accesses latency
+ * bit[2:0]:1 - 2 cycle of setup latency
+ */
+#ifndef PL310_TAG_RAM_CTRL_INIT
+#define PL310_TAG_RAM_CTRL_INIT		0x00000111
+#endif
+
+/*
+ * PL310 DATA RAM Control Register
+ *
+ * bit[10:8]:2 - 3 cycle of write accesses latency
+ * bit[6:4]:2 - 3 cycle of read accesses latency
+ * bit[2:0]:2 - 3 cycle of setup latency
+ */
+#ifndef PL310_DATA_RAM_CTRL_INIT
+#define PL310_DATA_RAM_CTRL_INIT	0x00000222
+#endif
+
+/*
+ * PL310 Auxiliary Control Register
+ *
+ * I/Dcache prefetch enabled (bit29:28=2b11)
+ * NS can access interrupts (bit27=1)
+ * NS can lockown cache lines (bit26=1)
+ * Pseudo-random replacement policy (bit25=0)
+ * Force write allocated (default)
+ * Shared attribute internally ignored (bit22=1, bit13=0)
+ * Parity disabled (bit21=0)
+ * Event monitor disabled (bit20=0)
+ * Platform fmavor specific way config:
+ * - way size (bit19:17)
+ * - way associciativity (bit16)
+ * Store buffer device limitation enabled (bit11=1)
+ * Cacheable accesses have high prio (bit10=0)
+ * Full Line Zero (FLZ) disabled (bit0=0)
+ */
+#ifndef PL310_AUX_CTRL_INIT
+#define PL310_AUX_CTRL_INIT		0x3C480800
+#endif
+
+/*
+ * PL310 Prefetch Control Register
+ *
+ * Double linefill disabled (bit30=0)
+ * I/D prefetch enabled (bit29:28=2b11)
+ * Prefetch drop enabled (bit24=1)
+ * Incr double linefill disable (bit23=0)
+ * Prefetch offset = 7 (bit4:0)
+ */
+#define PL310_PREFETCH_CTRL_INIT	0x31000007
+
+/*
+ * PL310 Power Register
+ *
+ * Dynamic clock gating enabled
+ * Standby mode enabled
+ */
+#define PL310_POWER_CTRL_INIT		0x00000003
+
+/*
+ * SCU Control Register : CTRL = 0x00000065
+ * - ic stanby enable=1
+ * - scu standby enable=1
+ * - scu enable=1
+ */
+#define SCU_CTRL_INIT			0x00000065
+
+/*
+ * TEE RAM layout without CFG_WITH_PAGER:
+ *
+ *  +---------------------------------------+  <- CFG_DDR_TEETZ_RESERVED_START
+ *  | TEE private secure |  TEE_RAM         |   ^
+ *  |   external memory  +------------------+   |
+ *  |                    |  TA_RAM          |   |
+ *  +---------------------------------------+   | CFG_DDR_TEETZ_RESERVED_SIZE
+ *  |     Non secure     |  SHM             |   |
+ *  |   shared memory    |                  |   |
+ *  +---------------------------------------+   v
+ *
+ *  TEE_RAM : default 1MByte
+ *  PUB_RAM : default 2MByte
+ *  TA_RAM  : all what is left
+ *
+ * ----------------------------------------------------------------------------
+ * TEE RAM layout with CFG_WITH_PAGER=y:
+ *
+ *  +---------------------------------------+  <- CFG_CORE_TZSRAM_EMUL_START
+ *  | TEE private highly | TEE_RAM          |   ^
+ *  |   secure memory    |                  |   | CFG_CORE_TZSRAM_EMUL_SIZE
+ *  +---------------------------------------+   v
+ *
+ *  +---------------------------------------+  <- CFG_DDR_TEETZ_RESERVED_START
+ *  | TEE private secure |  TA_RAM          |   ^
+ *  |   external memory  |                  |   |
+ *  +---------------------------------------+   | CFG_DDR_TEETZ_RESERVED_SIZE
+ *  |     Non secure     |  SHM             |   |
+ *  |   shared memory    |                  |   |
+ *  +---------------------------------------+   v
+ *
+ *  TEE_RAM : default 256kByte
+ *  TA_RAM  : all what is left in DDR TEE reserved area
+ *  PUB_RAM : default 2MByte
+ */
+
+/* default locate shared memory at the end of the TEE reserved DDR */
+#ifndef CFG_SHMEM_SIZE
+#define CFG_SHMEM_SIZE		(2 * 1024 * 1024)
+#endif
+
+#ifndef CFG_SHMEM_START
+#define CFG_SHMEM_START		(CFG_DDR_TEETZ_RESERVED_START + \
+				CFG_DDR_TEETZ_RESERVED_SIZE - \
+				CFG_SHMEM_SIZE)
+#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 CFG_TEE_RAM_START	TZSRAM_BASE
+#define CFG_TEE_RAM_PH_SIZE	TZSRAM_SIZE
+
+#define CFG_TA_RAM_START	TZDRAM_BASE
+#define CFG_TA_RAM_SIZE		TZDRAM_SIZE
+
+#else  /* CFG_WITH_PAGER */
+
+#define TZDRAM_BASE		CFG_DDR_TEETZ_RESERVED_START
+#define TZDRAM_SIZE		(CFG_DDR_TEETZ_RESERVED_SIZE - CFG_SHMEM_SIZE)
+
+#define CFG_TEE_RAM_START	TZDRAM_BASE
+#ifndef CFG_TEE_RAM_PH_SIZE
+#define CFG_TEE_RAM_PH_SIZE	(1 * 1024 * 1024)
+#endif
+
+#define CFG_TA_RAM_START	(TZDRAM_BASE + CFG_TEE_RAM_PH_SIZE)
+#define CFG_TA_RAM_SIZE		(TZDRAM_SIZE - CFG_TEE_RAM_PH_SIZE)
+
+#endif /* !CFG_WITH_PAGER */
+
+/* External DDR dies */
+#define DRAM0_BASE		CFG_DDR_START
+#define DRAM0_SIZE		CFG_DDR_SIZE
+#ifdef CFG_DDR1_START
+#define DRAM1_BASE		CFG_DDR1_START
+#define DRAM1_SIZE		CFG_DDR1_SIZE
+#endif
+
+#ifndef CFG_TEE_RAM_VA_SIZE
+#define CFG_TEE_RAM_VA_SIZE	(1024 * 1024)
+#endif
+
+#ifndef CFG_TEE_LOAD_ADDR
+#define CFG_TEE_LOAD_ADDR	CFG_TEE_RAM_START
+#endif
+
+#define PL310_BASE		(CPU_IOMEM_BASE + 0x2000)
+#define GIC_DIST_BASE		(CPU_IOMEM_BASE + 0x1000)
+#define SCU_BASE		(CPU_IOMEM_BASE + 0x0000)
+#define GIC_CPU_BASE		(CPU_IOMEM_BASE + 0x0100)
+#define ST_ASC20_REGS_BASE	(STXHxxx_LPM_PERIPH_BASE + 0x00130000)
+#define ST_ASC21_REGS_BASE	(STXHxxx_LPM_PERIPH_BASE + 0x00131000)
+
+/* Make stacks aligned to data cache line length */
+#define STACK_ALIGNMENT		32
+
+#endif /* PLATFORM_CONFIG_H */
diff --git a/core/arch/arm/plat-stm/rng_support.c b/core/arch/arm/plat-stm/rng_support.c
new file mode 100644
index 0000000..25b3893
--- /dev/null
+++ b/core/arch/arm/plat-stm/rng_support.c
@@ -0,0 +1,147 @@
+/*
+ * Copyright (c) 2014-2016, 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 <io.h>
+#include <kernel/panic.h>
+#include <mm/core_mmu.h>
+#include <mm/core_memprot.h>
+#include <platform_config.h>
+#include <trace.h>
+
+#include "rng_support.h"
+
+/* Address of the register to read in the RNG IP */
+#define RNG_VAL_OFFSET             0x24
+#define RNG_STATUS_OFFSET          0x20
+
+#define RNG_STATUS_ERR0		BIT32(0)
+#define RNG_STATUS_ERR1		BIT32(1)
+#define RNG_STATUS_FULL		BIT32(5)
+
+static vaddr_t rng_base(void)
+{
+	static void *va __early_bss;
+
+	if (cpu_mmu_enabled()) {
+		if (!va)
+			va = phys_to_virt(RNG_BASE, MEM_AREA_IO_SEC);
+		return (vaddr_t)va;
+	}
+	return RNG_BASE;
+}
+
+static inline int hwrng_waithost_fifo_full(void)
+{
+	uint32_t status;
+
+	do {
+		status = read32(rng_base() + RNG_STATUS_OFFSET);
+	} while (!(status & RNG_STATUS_FULL));
+
+	if (status & (RNG_STATUS_ERR0 | RNG_STATUS_ERR1))
+		return 1;
+
+	return 0;
+}
+
+uint8_t hw_get_random_byte(void)
+{
+	/*
+	 * Only the HW RNG IP is used to generate the value through the
+	 * HOST interface.
+	 *
+	 * @see the document rng_fspec_revG_120720.pdf for details
+	 *
+	 * - HOST FIFO size = 8x8b (64b)
+	 * - LSB (16b) of the RNG_VAL register allows to read 16b
+	 * - bit5 of the RNG_STATUS register allows to known if the HOST
+	 *   FIFO is full or not.
+	 * - bit1,0 of the RNG_STATUS register allows to known if the
+	 *   data are valid.
+	 *
+	 * Main principle:
+	 *  For performance reason, a local SW fifo is used to store the
+	 *  content of the HOST FIFO (max size = 8bytes). When a random
+	 *  value is expected, this SW fifo is used to return a stored value.
+	 *  When the local SW fifo is empty, it is filled with the HOST FIFO
+	 *  according the following sequence:
+	 *
+	 *  - wait HOST FIFO full
+	 *      o Indicates that max 8-bytes (64b) are available
+	 *      o This is mandatory to guarantee that a valid data is
+	 *      available. No STATUS bit to indicate that the HOST FIFO
+	 *      is empty is provided.
+	 *  - check STATUS bits
+	 *  - update the local SW fifo with the HOST FIFO
+	 *
+	 *  This avoid to wait at each iteration that a valid random value is
+	 *  available. _LOCAL_FIFO_SIZE indicates the size of the local SW fifo.
+	 *
+	 */
+
+
+#define _LOCAL_FIFO_SIZE 8     /* min 2, 4, 6, max 8 */
+
+	static uint8_t lfifo[_LOCAL_FIFO_SIZE];     /* local fifo */
+	static int pos = -1;
+
+	static int nbcall;  /* debug purpose - 0 is the initial value*/
+
+	volatile uint32_t tmpval[_LOCAL_FIFO_SIZE/2];
+	uint8_t value;
+	int i;
+
+	nbcall++;
+
+	/* Retrieve data from local fifo */
+	if (pos >= 0) {
+		pos++;
+		value = lfifo[pos];
+		if (pos == (_LOCAL_FIFO_SIZE - 1))
+			pos = -1;
+		return value;
+	}
+
+	if (hwrng_waithost_fifo_full())
+		return 0;
+
+	/* Read the FIFO according the number of expected element */
+	for (i = 0; i < _LOCAL_FIFO_SIZE / 2; i++)
+		tmpval[i] = read32(rng_base() + RNG_VAL_OFFSET) & 0xFFFF;
+
+	/* Update the local SW fifo for next request */
+	pos = 0;
+	for (i = 0; i < _LOCAL_FIFO_SIZE / 2; i++) {
+		lfifo[pos] = tmpval[i] & 0xFF;
+		pos++;
+		lfifo[pos] = (tmpval[i] >> 8) & 0xFF;
+		pos++;
+	};
+
+	pos = 0;
+	return lfifo[pos];
+}
diff --git a/core/arch/arm/plat-stm/sub.mk b/core/arch/arm/plat-stm/sub.mk
new file mode 100644
index 0000000..d16bb72
--- /dev/null
+++ b/core/arch/arm/plat-stm/sub.mk
@@ -0,0 +1,6 @@
+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
new file mode 100644
index 0000000..aee7dbe
--- /dev/null
+++ b/core/arch/arm/plat-stm/tz_a9init.S
@@ -0,0 +1,101 @@
+/*
+ * Copyright (c) 2014-2016, 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 <arm32.h>
+#include <arm32_macros.S>
+#include <arm32_macros_cortex_a9.S>
+#include <asm.S>
+#include <kernel/tz_ssvce_def.h>
+#include <kernel/unwind.h>
+#include <platform_config.h>
+
+.section .text
+.balign 4
+.code 32
+
+/*
+ * void arm_cl2_enable(vaddr_t pl310_base) - Memory Cache Level2 Enable Function
+ *
+ * If PL310 supports FZLW, enable also FZL in A9 core
+ *
+ * Use scratables registers R0-R3.
+ * No stack usage.
+ * LR store return address.
+ * Trap CPU in case of error.
+ * TODO: to be moved to PL310 code (tz_svce_pl310.S ?)
+ */
+FUNC arm_cl2_enable , :
+UNWIND(	.fnstart)
+
+	/* Enable PL310 ctrl -> only set lsb bit */
+	mov  r1, #0x1
+	str  r1, [r0, #PL310_CTRL]
+
+	/* if L2 FLZW enable, enable in L1 */
+	ldr  r1, [r0, #PL310_AUX_CTRL]
+	tst  r1, #(1 << 0) /* test AUX_CTRL[FLZ] */
+	read_actlr r0
+	orrne r0, r0, #(1 << 3) /* enable ACTLR[FLZW] */
+	write_actlr r0
+
+	mov pc, lr
+
+UNWIND(	.fnend)
+END_FUNC arm_cl2_enable
+
+/*
+ * Cortex A9 configuration early configuration
+ *
+ * Use scratables registers R0-R3.
+ * No stack usage.
+ * LR store return address.
+ * Trap CPU in case of error.
+ */
+FUNC plat_cpu_reset_early , :
+UNWIND(	.fnstart)
+
+	movw r0, #(CPU_SCTLR_INIT & 0xFFFF)
+	movt r0, #((CPU_SCTLR_INIT >> 16) & 0xFFFF)
+	write_sctlr r0
+
+	movw r0, #(CPU_ACTLR_INIT & 0xFFFF)
+	movt r0, #((CPU_ACTLR_INIT >> 16) & 0xFFFF)
+	write_actlr r0
+
+	movw r0, #(CPU_NSACR_INIT & 0xFFFF)
+	movt r0, #((CPU_NSACR_INIT >> 16) & 0xFFFF)
+	write_nsacr r0
+
+	movw r0, #(CPU_PCR_INIT & 0xFFFF)
+	movt r0, #((CPU_PCR_INIT >> 16) & 0xFFFF)
+	write_pcr r0
+
+	mov pc, lr
+
+UNWIND(	.fnend)
+END_FUNC plat_cpu_reset_early
+
diff --git a/core/arch/arm/plat-sunxi/conf.mk b/core/arch/arm/plat-sunxi/conf.mk
new file mode 100644
index 0000000..b2a9dc1
--- /dev/null
+++ b/core/arch/arm/plat-sunxi/conf.mk
@@ -0,0 +1,17 @@
+arm32-platform-cpuarch		:= cortex-a15
+arm32-platform-cflags	 	+= -mcpu=$(arm32-platform-cpuarch)
+arm32-platform-aflags	 	+= -mcpu=$(arm32-platform-cpuarch)
+core_arm32-platform-aflags	+= -mfpu=neon
+
+$(call force,CFG_ARM32_core,y)
+$(call force,CFG_SECURE_TIME_SOURCE_CNTPCT,y)
+$(call force,CFG_SUNXI_UART,y)
+$(call force,CFG_PM_STUBS,y)
+$(call force,CFG_GIC,y)
+
+ta-targets = ta_arm32
+
+CFG_CRYPTO_SIZE_OPTIMIZATION ?= n
+CFG_NUM_THREADS ?= 4
+CFG_WITH_STACK_CANARIES ?= y
+CFG_WITH_STATS ?= y
diff --git a/core/arch/arm/plat-sunxi/console.c b/core/arch/arm/plat-sunxi/console.c
new file mode 100644
index 0000000..b985316
--- /dev/null
+++ b/core/arch/arm/plat-sunxi/console.c
@@ -0,0 +1,59 @@
+/*
+ * Copyright (c) 2014, Allwinner Technology Co., Ltd.
+ * 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 <drivers/sunxi_uart.h>
+#include <mm/core_memprot.h>
+#include <console.h>
+
+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)
+{
+	sunxi_uart_init(console_base());
+}
+
+void console_putc(int ch)
+{
+	sunxi_uart_putc(ch, console_base());
+}
+
+void console_flush(void)
+{
+	sunxi_uart_flush(console_base());
+}
diff --git a/core/arch/arm/plat-sunxi/entry.S b/core/arch/arm/plat-sunxi/entry.S
new file mode 100644
index 0000000..bff2b8b
--- /dev/null
+++ b/core/arch/arm/plat-sunxi/entry.S
@@ -0,0 +1,107 @@
+/*
+ * Copyright (c) 2014, Allwinner Technology Co., Ltd.
+ * 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 <arm.h>
+#include <arm32_macros.S>
+#include <sm/optee_smc.h>
+#include <sm/teesmc_opteed_macros.h>
+#include <sm/teesmc_opteed.h>
+#include <kernel/unwind.h>
+
+.section .text.boot
+.align	5
+FUNC _start , :
+	b	reset
+	b	.	/* Undef */
+	b	.	/* Syscall */
+	b	.	/* Prefetch abort */
+	b	.	/* Data abort */
+	b	.	/* Reserved */
+	b	.	/* IRQ */
+	b	.	/* FIQ */
+END_FUNC _start
+
+LOCAL_FUNC reset , :
+UNWIND(	.fnstart)
+UNWIND(	.cantunwind)
+        read_sctlr  r0
+	orr	    r0, r0, #SCTLR_A
+	write_sctlr r0
+
+	ldr	   r0, =_start
+	write_vbar r0
+	
+	mov	r4, r1
+	bl	get_core_pos
+	add	r0, r0, #1
+	ldr	r2, =stack_tmp_stride
+	ldr	r1, [r2]
+	mul	r2, r0, r1
+	ldr	r1, =stack_tmp
+	ldr	sp, [r1, r2]
+
+    	/* NSACR configuration */
+    	read_nsacr  r1
+	orr     r1, r1, #NSACR_CP10
+	orr     r1, r1, #NSACR_CP11
+	orr     r1, r1, #NSACR_NS_SMP
+    	write_nsacr r1
+
+	/* Enable SMP bit */
+	read_actlr  r0
+	orr     r0, r0, #ACTLR_SMP
+	write_actlr  r0
+
+	bl	core_init_mmu_map
+	bl	core_init_mmu_regs
+	bl	cpu_mmu_enable
+	bl	cpu_mmu_enable_icache
+	bl	cpu_mmu_enable_dcache
+
+	/* init BSS section */
+	ldr     r0, =__bss_start
+	ldr     r2, =__bss_end
+	sub     r2, r2, r0
+	ldr     r1, =0
+	bl      memset
+	
+	/* r4: the return address of normal world */
+	mov	r0, r4
+	bl	main_init
+
+	mov	r1, #0
+	mov	r2, #0
+	mov	r3, #0
+	mov	r0, #TEESMC_OPTEED_RETURN_ENTRY_DONE
+	smc	#0
+	b	.	/* SMC should never return */
+UNWIND(	.fnend)
+END_FUNC reset
+
diff --git a/core/arch/arm/plat-sunxi/head.c b/core/arch/arm/plat-sunxi/head.c
new file mode 100644
index 0000000..838dbf4
--- /dev/null
+++ b/core/arch/arm/plat-sunxi/head.c
@@ -0,0 +1,60 @@
+/*
+ * Copyright (c) 2014, Allwinner Technology Co., Ltd.
+ * 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.
+ */
+
+/** 
+ * Header for optee, use for secure bootloader.
+ **/
+
+#include <platform_config.h>
+
+/******************************************************************************/
+/*               the control information stored in file head                  */
+/******************************************************************************/
+struct spare_boot_ctrl_head {
+	unsigned int  jump_instruction;   /* one intruction jumping to real code */
+	unsigned char magic[8];           /* ="optee"                            */
+	unsigned int  check_sum;          /* generated by PC                     */
+	unsigned int  align_size;	  /* align size in byte                  */
+	unsigned int  length;             /* the size of all file                */
+	unsigned int  optee_length;       /* the size of optee                   */
+	unsigned char version[8];         /* optee version                       */
+	unsigned char platform[8];        /* platform information                */
+	int           reserved[1];        /* stamp space, 16bytes align          */
+};
+
+const struct spare_boot_ctrl_head  tee_spare_head 
+             __attribute__ ((section(".text.head"))) = {
+		(0xEA000000 | (((sizeof(struct spare_boot_ctrl_head) + sizeof(int) - 1) / sizeof(int) - 2) & 0x00FFFFFF)),
+		"optee",
+		0,
+		0,
+		0,
+		0,
+		"2.0",
+		"optee",
+		{TZDRAM_BASE}
+};
diff --git a/core/arch/arm/plat-sunxi/kern.ld.S b/core/arch/arm/plat-sunxi/kern.ld.S
new file mode 100644
index 0000000..600ed09
--- /dev/null
+++ b/core/arch/arm/plat-sunxi/kern.ld.S
@@ -0,0 +1,198 @@
+/*
+ * Copyright (c) 2014, Allwinner Technology Co., Ltd.
+ * 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.
+ */
+
+/*
+ * Copyright (c) 2008-2010 Travis Geiselbrecht
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining
+ * a copy of this software and associated documentation files
+ * (the "Software"), to deal in the Software without restriction,
+ * including without limitation the rights to use, copy, modify, merge,
+ * publish, distribute, sublicense, and/or sell copies of the Software,
+ * and to permit persons to whom the Software is furnished to do so,
+ * subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
+ * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
+ * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
+ * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
+ * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ */
+
+#include <platform_config.h>
+
+OUTPUT_FORMAT(CFG_KERN_LINKER_FORMAT)
+OUTPUT_ARCH(CFG_KERN_LINKER_ARCH)
+
+ENTRY(_start)
+SECTIONS
+{
+	. = TEE_RAM_START;
+
+	/* text/read-only data */
+	.text : {
+		__text_start = .;
+		KEEP(*(.text.head))
+		KEEP(*(.text.boot.vectab1))
+		KEEP(*(.text.boot.vectab2))
+		KEEP(*(.text.boot))
+		*(.text* .sram.text.glue_7* .gnu.linkonce.t.*)
+		__text_end = .;
+
+		. = ALIGN(4);
+		__initcall_start = .;
+		*(.initcall1)
+		*(.initcall2)
+		*(.initcall3)
+		*(.initcall4)
+		__initcall_end = .;
+	}
+
+	.interp : { *(.interp) }
+	.hash : { *(.hash) }
+	.dynsym : { *(.dynsym) }
+	.dynstr : { *(.dynstr) }
+	.rel.text : { *(.rel.text) *(.rel.gnu.linkonce.t*) }
+	.rela.text : { *(.rela.text) *(.rela.gnu.linkonce.t*) }
+	.rel.data : { *(.rel.data) *(.rel.gnu.linkonce.d*) }
+	.rela.data : { *(.rela.data) *(.rela.gnu.linkonce.d*) }
+	.rel.rodata : { *(.rel.rodata) *(.rel.gnu.linkonce.r*) }
+	.rela.rodata : { *(.rela.rodata) *(.rela.gnu.linkonce.r*) }
+	.rel.got : { *(.rel.got) }
+	.rela.got : { *(.rela.got) }
+	.rel.ctors : { *(.rel.ctors) }
+	.rela.ctors : { *(.rela.ctors) }
+	.rel.dtors : { *(.rel.dtors) }
+	.rela.dtors : { *(.rela.dtors) }
+	.rel.init : { *(.rel.init) }
+	.rela.init : { *(.rela.init) }
+	.rel.fini : { *(.rel.fini) }
+	.rela.fini : { *(.rela.fini) }
+	.rel.bss : { *(.rel.bss) }
+	.rela.bss : { *(.rela.bss) }
+	.rel.plt : { *(.rel.plt) }
+	.rela.plt : { *(.rela.plt) }
+	.init : { *(.init) } =0x9090
+	.plt : { *(.plt) }
+
+	/* .ARM.exidx is sorted, so has to go in its own output section.  */
+	__exidx_start = .;
+	.ARM.exidx : { *(.ARM.exidx* .gnu.linkonce.armexidx.*) }
+	__exidx_end = .;
+
+	.ARM.extab : {
+		__extab_start = .;
+		*(.ARM.extab*)
+		__extab_end = .;
+	}
+
+	.rodata : ALIGN(4) {
+		__rodata_start = .;
+		*(.rodata .rodata.* .gnu.linkonce.r.*)
+
+		/*
+		 * 8 to avoid unwanted padding between __start_ta_head_section
+		 * and the first structure in ta_head_section, in 64-bit
+		 * builds
+		 */
+		. = ALIGN(8);
+		__start_ta_head_section = . ;
+		KEEP(*(ta_head_section))
+		__stop_ta_head_section = . ;
+		. = ALIGN(8);
+		__start_phys_mem_map_section = . ;
+		KEEP(*(phys_mem_map_section))
+		__end_phys_mem_map_section = . ;
+
+		. = ALIGN(4);
+		__rodata_end = .;
+	}
+
+
+	.data : ALIGN(4) {
+		/* writable data  */
+		__data_start_rom = .;
+		/* in one segment binaries, the rom data address is on top of the ram data address */
+		__data_start = .;
+		*(.data .data.* .gnu.linkonce.d.*)
+	}
+
+	.ctors : ALIGN(4) {
+		__ctor_list = .;
+		*(.ctors)
+		__ctor_end = .;
+	}
+	.dtors : ALIGN(4) {
+		__dtor_list = .;
+		*(.dtors)
+		__dtor_end = .;
+	}
+	.got : { *(.got.plt) *(.got) }
+	.dynamic : { *(.dynamic) }
+
+	__data_end = .;
+
+	/* unintialized data (in same segment as writable data) */
+	.bss : ALIGN(4) {
+		KEEP(*(.bss.prebss.*))
+		. = ALIGN(4);
+		__bss_start = .;
+		*(.bss .bss.*)
+		*(.gnu.linkonce.b.*)
+		*(COMMON)
+		. = ALIGN(4);
+		__bss_end = .;
+	}
+
+        /*
+	 * Uninitialized data that shouldn't be zero initialized at
+	 * runtime.
+	 *
+	 * L1 mmu table requires 16 KiB alignment
+	 */
+	.nozi : ALIGN(16 * 1024) {
+		__nozi_pad_end = .;
+		KEEP(*(.nozi .nozi.*))
+	}
+
+	teecore_heap_start = .;
+	. += 0x40000 /*256KiB*/;
+	teecore_heap_end = .;
+
+	_end = .;
+
+	. = TEE_RAM_START + TEE_RAM_SIZE;
+	_end_of_ram = .;
+
+	/* Strip unnecessary stuff */
+	/DISCARD/ : { *(.comment .note .eh_frame) }
+}
diff --git a/core/arch/arm/plat-sunxi/link.mk b/core/arch/arm/plat-sunxi/link.mk
new file mode 100644
index 0000000..2e289e6
--- /dev/null
+++ b/core/arch/arm/plat-sunxi/link.mk
@@ -0,0 +1,54 @@
+link-out-dir = $(out-dir)/core
+
+link-script = $(platform-dir)/kern.ld.S
+link-script-pp = $(link-out-dir)/kern.ld
+link-script-dep = $(link-out-dir)/.kern.ld.d
+
+AWK	 = awk
+
+all: $(link-out-dir)/tee.elf $(link-out-dir)/tee.dmp $(link-out-dir)/tee.bin
+all: $(link-out-dir)/tee.symb_sizes
+cleanfiles += $(link-out-dir)/tee.elf $(link-out-dir)/tee.dmp $(link-out-dir)/tee.map
+cleanfiles += $(link-out-dir)/tee.bin
+cleanfiles += $(link-out-dir)/tee.symb_sizes
+cleanfiles += $(link-script-pp) $(link-script-dep)
+
+link-ldflags  = $(LDFLAGS)
+link-ldflags += -T $(link-script-pp) -Map=$(link-out-dir)/tee.map
+link-ldflags += --sort-section=alignment
+
+link-ldadd  = $(LDADD)
+link-ldadd += $(addprefix -L,$(libdirs))
+link-ldadd += $(addprefix -l,$(libnames))
+ldargs-tee.elf := $(link-ldflags) $(objs) $(link-ldadd) $(libgcccore)
+
+link-script-cppflags :=  \
+	$(filter-out $(CPPFLAGS_REMOVE) $(cppflags-remove), \
+		$(nostdinccore) $(CPPFLAGS) \
+		$(addprefix -I,$(incdirscore)) $(cppflagscore))
+
+
+-include $(link-script-dep)
+
+$(link-script-pp): $(link-script) $(conf-file)
+	@$(cmd-echo-silent) '  CPP     $@'
+	@mkdir -p $(dir $@)
+	$(q)$(CPPcore) -Wp,-P,-MT,$@,-MD,$(link-script-dep) \
+		$(link-script-cppflags) $< > $@
+
+
+$(link-out-dir)/tee.elf: $(objs) $(libdeps) $(link-script-pp)
+	@$(cmd-echo-silent) '  LD      $@'
+	$(q)$(LDcore) $(ldargs-tee.elf) -o $@
+
+$(link-out-dir)/tee.dmp: $(link-out-dir)/tee.elf
+	@$(cmd-echo-silent) '  OBJDUMP $@'
+	$(q)$(OBJDUMPcore) -l -x -d $< > $@
+
+$(link-out-dir)/tee.bin: $(link-out-dir)/tee.elf
+	@$(cmd-echo-silent) '  OBJCOPY $@'
+	$(q)$(OBJCOPYcore) -O binary $< $@
+
+$(link-out-dir)/tee.symb_sizes: $(link-out-dir)/tee.elf
+	@$(cmd-echo-silent) '  GEN     $@'
+	$(q)$(NMcore) --print-size --reverse-sort --size-sort $< > $@
diff --git a/core/arch/arm/plat-sunxi/main.c b/core/arch/arm/plat-sunxi/main.c
new file mode 100644
index 0000000..3954d9d
--- /dev/null
+++ b/core/arch/arm/plat-sunxi/main.c
@@ -0,0 +1,177 @@
+/*
+ * Copyright (c) 2014, Allwinner Technology Co., Ltd.
+ * 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 <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 <kernel/panic.h>
+#include <kernel/pm_stubs.h>
+#include <kernel/misc.h>
+#include <mm/tee_mmu.h>
+#include <mm/core_mmu.h>
+#include <tee/entry_std.h>
+#include <tee/entry_fast.h>
+#include <platform.h>
+#include <util.h>
+#include <trace.h>
+#include <malloc.h>
+
+/* teecore heap address/size is defined in scatter file */
+extern unsigned char teecore_heap_start;
+extern unsigned char teecore_heap_end;
+
+static void main_fiq(void);
+static void main_tee_entry_std(struct thread_smc_args *args);
+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,
+	.cpu_on = pm_panic,
+	.cpu_off = pm_panic,
+	.cpu_suspend = pm_panic,
+	.cpu_resume = pm_panic,
+	.system_off = pm_panic,
+	.system_reset = pm_panic,
+};
+
+void main_init(uint32_t nsec_entry); /* called from assembly only */
+void main_init(uint32_t nsec_entry)
+{
+	struct sm_nsec_ctx *nsec_ctx;
+	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
+	 * with the temporary stack. The thread subsystem also asserts that
+	 * IRQ is blocked when using most if its functions.
+	 */
+	thread_mask_exceptions(THREAD_EXCP_FIQ | THREAD_EXCP_IRQ);
+
+	if (pos == 0) {
+		thread_init_primary(&handlers);
+
+		/* initialize platform */
+		platform_init();
+	}
+
+	thread_init_per_cpu();
+
+	/* Initialize secure monitor */
+	nsec_ctx = sm_get_nsec_ctx();
+	nsec_ctx->mon_lr = nsec_entry;
+	nsec_ctx->mon_spsr = CPSR_MODE_SVC | CPSR_I;
+
+	if (pos == 0) {
+		unsigned long a, s;
+		/* core malloc pool init */
+#ifdef CFG_TEE_MALLOC_START
+		a = CFG_TEE_MALLOC_START;
+		s = CFG_TEE_MALLOC_SIZE;
+#else
+		a = (unsigned long)&teecore_heap_start;
+		s = (unsigned long)&teecore_heap_end;
+		a = ((a + 1) & ~0x0FFFF) + 0x10000;	/* 64kB aligned */
+		s = s & ~0x0FFFF;	/* 64kB aligned */
+		s = s - a;
+#endif
+		malloc_add_pool((void *)a, s);
+
+		teecore_init_ta_ram();
+
+		if (init_teecore() != TEE_SUCCESS) {
+			panic();
+		}
+	}
+
+	IMSG("optee initialize finished\n");
+}
+
+static void main_fiq(void)
+{
+	panic();
+}
+
+static void main_tee_entry_fast(struct thread_smc_args *args)
+{
+	/* TODO move to main_init() */
+	if (init_teecore() != TEE_SUCCESS)
+		panic();
+
+	/* SiP Service Call Count */
+	if (args->a0 == OPTEE_SMC_SIP_SUNXI_CALLS_COUNT) {
+		args->a0 = 1;
+		return;
+	}
+
+	/*  SiP Service Call UID */
+	if (args->a0 == OPTEE_SMC_SIP_SUNXI_CALLS_UID) {
+		args->a0 = OPTEE_SMC_SIP_SUNXI_UID_R0;
+		args->a1 = OPTEE_SMC_SIP_SUNXI_UID_R1;
+		args->a2 = OPTEE_SMC_SIP_SUNXI_UID_R2;
+		args->a3 = OPTEE_SMC_SIP_SUNXI_UID_R3;
+		return;
+	}
+
+	/* SiP Service Calls */
+	if (args->a0 == OPTEE_SMC_OPTEE_FAST_CALL_SIP_SUNXI) {
+		platform_smc_handle(args);
+		return;
+	}
+
+	tee_entry_fast(args);
+}
+
+
+
+static void main_tee_entry_std(struct thread_smc_args *args)
+{
+	/* TODO move to main_init() */
+	if (init_teecore() != TEE_SUCCESS)
+		panic();
+
+	tee_entry_std(args);
+}
+
+/* main_tee_entry_fast() supports 3 platform-specific functions */
+void tee_entry_get_api_call_count(struct thread_smc_args *args)
+{
+	args->a0 = tee_entry_generic_get_api_call_count() + 3;
+}
diff --git a/core/arch/arm/plat-sunxi/platform.c b/core/arch/arm/plat-sunxi/platform.c
new file mode 100644
index 0000000..e46541a
--- /dev/null
+++ b/core/arch/arm/plat-sunxi/platform.c
@@ -0,0 +1,125 @@
+/*
+ * Copyright (c) 2014, Allwinner Technology Co., Ltd.
+ * 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 <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 <kernel/panic.h>
+#include <kernel/misc.h>
+#include <mm/tee_pager.h>
+#include <mm/core_mmu.h>
+#include <mm/core_memprot.h>
+
+#include <drivers/gic.h>
+#include <drivers/sunxi_uart.h>
+
+#include <trace.h>
+#include <io.h>
+#include <assert.h>
+#include <util.h>
+#include <platform.h>
+#include <console.h>
+
+void sunxi_secondary_entry(void);
+
+uint32_t sunxi_secondary_ns_entry;
+
+struct gic_data gic_data;
+
+static int platform_smp_init(void)
+{
+	vaddr_t base = (vaddr_t)phys_to_virt(PRCM_BASE, MEM_AREA_IO_SEC);
+
+	assert(base);
+	write32((uint32_t)sunxi_secondary_entry,
+		base + PRCM_CPU_SOFT_ENTRY_REG);
+
+	return 0;
+}
+
+void platform_init(void)
+{
+	vaddr_t gicc_base;
+	vaddr_t gicd_base;
+	vaddr_t cci400_base;
+
+	gicc_base = (vaddr_t)phys_to_virt(GIC_BASE + GICC_OFFSET,
+					  MEM_AREA_IO_SEC);
+	gicd_base = (vaddr_t)phys_to_virt(GIC_BASE + GICD_OFFSET,
+					  MEM_AREA_IO_SEC);
+	cci400_base = (vaddr_t)phys_to_virt(CCI400_BASE, MEM_AREA_IO_SEC);
+	if (!gicc_base || !gicd_base || !cci400_base)
+		panic();
+
+	/*
+	 * GIC configuration is initialized in Secure bootloader,
+	 * Initialize GIC base address here for debugging.
+	 */
+	gic_init_base_addr(&gic_data, gicc_base, gicd_base);
+	itr_init(&gic_data.chip);
+
+	/* platform smp initialize */
+	platform_smp_init();
+
+	/* enable non-secure access cci-400 registers */
+	write32(0x1, cci400_base + CCI400_SECURE_ACCESS_REG);
+
+	/* Initialize uart */
+	console_init();
+
+	return ;
+}
+
+/**
+ * handle platform special smc commands.
+ */
+uint32_t platform_smc_handle(struct thread_smc_args *smc_args)
+{
+	uint32_t ret = TEE_SUCCESS;
+	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));
+		break;
+	default:
+		ret = OPTEE_SMC_RETURN_EBADCMD;
+		break;
+	}
+	smc_args->a0 = ret;
+	return ret;
+}
+
diff --git a/core/arch/arm/plat-sunxi/platform.h b/core/arch/arm/plat-sunxi/platform.h
new file mode 100644
index 0000000..c6db14b
--- /dev/null
+++ b/core/arch/arm/plat-sunxi/platform.h
@@ -0,0 +1,85 @@
+/*
+ * Copyright (c) 2014, Allwinner Technology Co., Ltd.
+ * 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 PLATFORM_H
+#define PLATFORM_H
+
+/*
+ * Function specified by SMC Calling convention.
+ *
+ * SiP Service Calls
+ *
+ * Call register usage:
+ * r0	SMC Function ID, OPTEE_SMC_FUNCID_SIP_SUNXI
+ * r1	OPTEE_SMC_SIP_SUNXI_SET_SMP_BOOTENTRY set smp bootup entry
+ */
+#define OPTEE_SMC_SIP_SUNXI_SET_SMP_BOOTENTRY	(0xFFFF0000)
+
+#define OPTEE_SMC_FUNCID_SIP_SUNXI			0x8000
+#define OPTEE_SMC_OPTEE_FAST_CALL_SIP_SUNXI \
+	OPTEE_SMC_CALL_VAL(OPTEE_SMC_32, OPTEE_SMC_FAST_CALL, \
+			OPTEE_SMC_OWNER_SIP, \
+			OPTEE_SMC_FUNCID_SIP_SUNXI)
+
+
+/*
+ * Function specified by SMC Calling convention.
+ *
+ * SiP Service Call Count
+ *
+ * This call returns a 32-bit count of the available
+ * Service Calls. A return value of zero means no
+ * services are available.
+ */
+#define OPTEE_SMC_FUNCID_SIP_CALLS_COUNT	0xFF00
+#define OPTEE_SMC_SIP_SUNXI_CALLS_COUNT \
+	OPTEE_SMC_CALL_VAL(OPTEE_SMC_32, OPTEE_SMC_FAST_CALL, \
+			OPTEE_SMC_OWNER_SIP, \
+			OPTEE_SMC_FUNCID_CALLS_COUNT)
+
+/*
+ * Function specified by SMC Calling convention
+ *
+ * SiP Service Call UID
+ *
+ * Return the implementation of SiP Service Calls UID.
+ *
+ */
+#define OPTEE_SMC_SIP_SUNXI_UID_R0			0xa5d5c51b
+#define OPTEE_SMC_SIP_SUNXI_UID_R1			0x8d6c0002
+#define OPTEE_SMC_SIP_SUNXI_UID_R2			0x6f8611e4
+#define OPTEE_SMC_SIP_SUNXI_UID_R3			0x12b7e560
+#define OPTEE_SMC_FUNCID_SIP_SUNXI_CALLS_UID	0xFF01
+#define OPTEE_SMC_SIP_SUNXI_CALLS_UID \
+	OPTEE_SMC_CALL_VAL(OPTEE_SMC_32, OPTEE_SMC_FAST_CALL, \
+			OPTEE_SMC_OWNER_SIP, \
+			OPTEE_SMC_FUNCID_SIP_SUNXI_CALLS_UID)
+
+void platform_init(void);
+uint32_t platform_smc_handle(struct thread_smc_args *smc_args);
+
+#endif /*PLATFORM_H*/
diff --git a/core/arch/arm/plat-sunxi/platform_config.h b/core/arch/arm/plat-sunxi/platform_config.h
new file mode 100644
index 0000000..8060d9b
--- /dev/null
+++ b/core/arch/arm/plat-sunxi/platform_config.h
@@ -0,0 +1,152 @@
+/*
+ * Copyright (c) 2014, Allwinner Technology Co., Ltd.
+ * 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 PLATFORM_CONFIG_H
+#define PLATFORM_CONFIG_H
+
+#define STACK_ALIGNMENT		8
+
+#ifdef CFG_WITH_PAGER
+#error "Pager not supported for platform sunxi"
+#endif
+#ifdef CFG_WITH_LPAE
+#error "LPAE not supported for platform sunxi"
+#endif
+
+#define GIC_BASE		0x01c40000
+#define GICC_OFFSET		0x2000
+#define GICD_OFFSET		0x1000
+#define UART0_BASE		0x07000000
+#define UART1_BASE		0x07000400
+#define UART2_BASE		0x07000800
+#define UART3_BASE		0x07000c00
+#define CCI400_BASE             0x01c90000
+#define SMC_BASE                0x01c0b000
+#define PRCM_BASE               0x08001400
+
+/* CCI-400 register defines */
+#define CCI400_SECURE_ACCESS_REG  (0x8)
+
+/* PRCM register defines */
+#define PRCM_CPU_SOFT_ENTRY_REG   (0x164)
+
+/* console uart define */
+#define CONSOLE_UART_BASE       UART0_BASE
+
+#define DRAM0_BASE		0x20000000
+#define DRAM0_SIZE		0x80000000
+
+/* Location of trusted dram on sunxi */
+#define TZDRAM_BASE		0x9C000000
+#define TZDRAM_SIZE		0x04000000
+
+#define CFG_TEE_CORE_NB_CORE	8
+
+#define DDR_PHYS_START		DRAM0_BASE
+#define DDR_SIZE		DRAM0_SIZE
+
+#define CFG_DDR_START		DDR_PHYS_START
+#define CFG_DDR_SIZE		DDR_SIZE
+
+#define CFG_DDR_TEETZ_RESERVED_START	TZDRAM_BASE
+#define CFG_DDR_TEETZ_RESERVED_SIZE	TZDRAM_SIZE
+
+#define TEE_RAM_START		(TZDRAM_BASE)
+#define TEE_RAM_SIZE		(1 * 1024 * 1024)
+
+/*
+ * TEE/TZ RAM layout:
+ *
+ *  +-----------------------------------------+  <- CFG_DDR_TEETZ_RESERVED_START
+ *  | TEETZ private RAM  |  TEE_RAM           |   ^
+ *  |                    +--------------------+   |
+ *  |                    |  TA_RAM            |   |
+ *  +-----------------------------------------+   | CFG_DDR_TEETZ_RESERVED_SIZE
+ *  |                    |      teecore alloc |   |
+ *  |  TEE/TZ and NSec   |  PUB_RAM   --------|   |
+ *  |   shared memory    |         NSec alloc |   |
+ *  +-----------------------------------------+   v
+ *
+ *  TEE_RAM : 1MByte
+ *  PUB_RAM : 1MByte
+ *  TA_RAM  : all what is left (at least 2MByte !)
+ */
+
+/* define the several memory area sizes */
+#if (CFG_DDR_TEETZ_RESERVED_SIZE < (4 * 1024 * 1024))
+#error "Invalid CFG_DDR_TEETZ_RESERVED_SIZE: at least 4MB expected"
+#endif
+
+#define CFG_TEE_RAM_PH_SIZE		(1 * 1024 * 1024)
+#define CFG_TEE_RAM_SIZE		CFG_TEE_RAM_PH_SIZE
+#define CFG_TA_RAM_SIZE			(CFG_DDR_TEETZ_RESERVED_SIZE - \
+					CFG_TEE_RAM_SIZE - CFG_SHMEM_SIZE)
+
+/* define the secure/unsecure memory areas */
+#define CFG_DDR_ARMTZ_ONLY_START	(CFG_DDR_TEETZ_RESERVED_START)
+#define CFG_DDR_ARMTZ_ONLY_SIZE		(CFG_TEE_RAM_SIZE + CFG_TA_RAM_SIZE)
+
+#define CFG_DDR_ARM_ARMTZ_START		\
+			(CFG_DDR_ARMTZ_ONLY_START + CFG_DDR_ARMTZ_ONLY_SIZE)
+#define CFG_DDR_ARM_ARMTZ_SIZE		(CFG_PUB_RAM_SIZE)
+
+/* define the memory areas (TEE_RAM must start at reserved DDR start addr */
+#define CFG_TEE_RAM_START		(CFG_DDR_ARMTZ_ONLY_START)
+#define CFG_TA_RAM_START		(CFG_TEE_RAM_START + CFG_TEE_RAM_SIZE)
+#define CFG_PUB_RAM_START		(CFG_TA_RAM_START + CFG_TA_RAM_SIZE)
+
+/* Full GlobalPlatform test suite requires CFG_SHMEM_SIZE to be at least 2MB */
+#define CFG_SHMEM_START		(DDR_PHYS_START + 0x1000000)
+#define CFG_SHMEM_SIZE		0x100000
+
+#define CFG_TEE_LOAD_ADDR	TEE_RAM_START
+
+/* AHB0 devices */
+#define DEVICE0_PA_BASE		ROUNDDOWN(0x01400000, CORE_MMU_DEVICE_SIZE)
+#define DEVICE0_VA_BASE		DEVICE0_PA_BASE
+#define DEVICE0_SIZE		ROUNDUP(0x00900000, CORE_MMU_DEVICE_SIZE)
+#define DEVICE0_TYPE		MEM_AREA_IO_SEC
+
+/* AHB1 devices */
+#define DEVICE1_PA_BASE		ROUNDDOWN(0x00800000, CORE_MMU_DEVICE_SIZE)
+#define DEVICE1_VA_BASE		DEVICE1_PA_BASE
+#define DEVICE1_SIZE		ROUNDUP(0x00300000, CORE_MMU_DEVICE_SIZE)
+#define DEVICE1_TYPE		MEM_AREA_IO_SEC
+
+/* AHB2 devices */
+#define DEVICE2_PA_BASE		ROUNDDOWN(0x03000000, CORE_MMU_DEVICE_SIZE)
+#define DEVICE2_VA_BASE		DEVICE2_PA_BASE
+#define DEVICE2_SIZE		ROUNDUP(0x01000000, CORE_MMU_DEVICE_SIZE)
+#define DEVICE2_TYPE		MEM_AREA_IO_SEC
+
+/* AHBS devices */
+#define DEVICE3_PA_BASE		ROUNDDOWN(0x06000000, CORE_MMU_DEVICE_SIZE)
+#define DEVICE3_VA_BASE		DEVICE3_PA_BASE
+#define DEVICE3_SIZE		ROUNDUP(0x02200000, CORE_MMU_DEVICE_SIZE)
+#define DEVICE3_TYPE		MEM_AREA_IO_SEC
+
+#endif /*PLATFORM_CONFIG_H*/
diff --git a/core/arch/arm/plat-sunxi/rng_support.c b/core/arch/arm/plat-sunxi/rng_support.c
new file mode 100644
index 0000000..434b104
--- /dev/null
+++ b/core/arch/arm/plat-sunxi/rng_support.c
@@ -0,0 +1,43 @@
+/*
+ * Copyright (c) 2014, Allwinner Technology Co., Ltd.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+#include <stdlib.h>
+#include <rng_support.h>
+#include <trace.h>
+
+/* Bad software version */
+uint8_t hw_get_random_byte(void)
+{
+	static uint32_t lcg_state;
+	static uint32_t nb_soft = 9876543;
+#define MAX_SOFT_RNG 1024
+	static const uint32_t a = 1664525;
+	static const uint32_t c = 1013904223;
+
+	nb_soft = (nb_soft + 1) % MAX_SOFT_RNG;
+	lcg_state = (a * lcg_state + c);
+	return (uint8_t) (lcg_state >> 24);
+}
diff --git a/core/arch/arm/plat-sunxi/smp_boot.S b/core/arch/arm/plat-sunxi/smp_boot.S
new file mode 100644
index 0000000..9e79842
--- /dev/null
+++ b/core/arch/arm/plat-sunxi/smp_boot.S
@@ -0,0 +1,104 @@
+/*
+ * Copyright (c) 2014, Allwinner Technology Co., Ltd.
+ * 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 <asm.S>
+#include <arm.h>
+#include <arm32_macros.S>
+#include <sm/optee_smc.h>
+#include <sm/teesmc_opteed_macros.h>
+#include <sm/teesmc_opteed.h>
+#include <kernel/unwind.h>
+
+
+FUNC smp_init_vector , :
+	b	.       /* Reset */
+	b	.	/* Undef */
+	b	.	/* Syscall */
+	b	.	/* Prefetch abort */
+	b	.	/* Data abort */
+	b	.	/* Reserved */
+	b	.	/* IRQ */
+	b	.	/* FIQ */
+END_FUNC smp_init_vector
+
+FUNC sunxi_secondary_entry , :
+UNWIND(	.fnstart)
+UNWIND(	.cantunwind)
+	/* secondary CPUs internal initialization */
+	read_sctlr r0
+	orr	r0, r0, #SCTLR_A
+	write_sctlr r0
+	
+	/* install smp initialization vector */
+	ldr	r0, =smp_init_vector
+	write_vbar r0
+
+	/* Setup tmp stack */
+        bl      get_core_pos
+        add     r0, r0, #1
+        ldr     r2, =stack_tmp_stride
+        ldr     r1, [r2]
+        mul     r2, r0, r1
+        ldr     r1, =stack_tmp
+        ldr     sp, [r1, r2]
+
+        /* NSACR configuration */
+    	read_nsacr  r1
+	orr     r1, r1, #NSACR_CP10
+	orr     r1, r1, #NSACR_CP11
+	orr     r1, r1, #NSACR_NS_SMP
+	write_nsacr  r1
+    	mcr     p15, 0, r1, c1, c1, 2
+	
+	/* Enable SMP bit */
+    	read_actlr  r0
+	orr     r0, r0, #ACTLR_SMP
+    	write_actlr r0
+	
+	/* fixup some platform limits */
+	bl	sunxi_secondary_fixup
+	
+	/* initialize gic cpu interface */
+	ldr	r0, =gic_data
+    	bl      gic_cpu_init
+	
+	/* secure env initialization */
+	bl	core_init_mmu_regs
+	bl	cpu_mmu_enable
+	bl	cpu_mmu_enable_icache
+	bl	cpu_mmu_enable_dcache
+
+	/* Initialize thread handling and secure monitor */
+	ldr	r0, =sunxi_secondary_ns_entry
+	ldr     r0, [r0]
+	bl	main_init
+
+	mov	r0, #TEESMC_OPTEED_RETURN_ENTRY_DONE
+	smc	#0
+	b	.	/* SMC should not return */
+UNWIND(	.fnend)
+END_FUNC sunxi_secondary_entry
diff --git a/core/arch/arm/plat-sunxi/smp_fixup.S b/core/arch/arm/plat-sunxi/smp_fixup.S
new file mode 100644
index 0000000..bf533b4
--- /dev/null
+++ b/core/arch/arm/plat-sunxi/smp_fixup.S
@@ -0,0 +1,116 @@
+/*
+ * Copyright (c) 2014, Allwinner Technology Co., Ltd.
+ * 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 <asm.S>
+#include <kernel/unwind.h>
+
+#define SLAVE_SNOOPCTL_OFFSET	0
+#define SNOOPCTL_SNOOP_ENABLE	(1 << 0)
+#define SNOOPCTL_DVM_ENABLE	(1 << 1)
+
+#define CCI_STATUS_OFFSET	0xc
+#define STATUS_CHANGE_PENDING	(1 << 0)
+
+#define CCI_SLAVE_OFFSET(n)	(0x1000 + 0x1000 * (n))
+
+#define SUNXI_CCI_PHYS_BASE	0x01c90000
+#define SUNXI_CCI_SLAVE_A7	3
+#define SUNXI_CCI_SLAVE_A15	4
+#define SUNXI_CCI_A15_OFFSET	CCI_SLAVE_OFFSET(SUNXI_CCI_SLAVE_A15)
+#define SUNXI_CCI_A7_OFFSET	CCI_SLAVE_OFFSET(SUNXI_CCI_SLAVE_A7)
+
+#define SUNXI_CCU_PHYS_BASE     (0x06000000)
+#define SUNXI_CCU_C0_CFG_OFFSET (0x54)
+#define SUNXI_CCU_C1_CFG_OFFSET (0x58)
+
+FUNC sunxi_secondary_fixup , :
+UNWIND(	.fnstart)
+	mrc	p15, 0, r0, c0, c0, 5	/* MPIDR   */
+	ubfx	r0, r0, #8, #4		/* cluster */
+	
+	ldr	r3, =SUNXI_CCU_PHYS_BASE + SUNXI_CCU_C0_CFG_OFFSET
+	cmp	r0, #0		    /* A7 cluster? */
+	addne	r3, r3, #SUNXI_CCU_C1_CFG_OFFSET - SUNXI_CCU_C0_CFG_OFFSET
+	ldr	r1, [r3]
+	bic     r1, r1, #(0x3<<8)   /* a15 atb div                            */
+	orr     r1, r1, #(0x1<<8)   /* div = 2                                */
+	bic     r1, r1, #(0x7<<0)   /* a15 atb div                            */
+	orr     r1, r1, #(0x2<<0)   /* div = value + 1                        */
+	str	r1, [r3]	    /* set atb div to 2, axi div to 3         */
+	dsb                         /* Synchronise side-effects of axi config */
+	ldr	r1, [r3]
+	bic     r1, r1, #(0x3<<8)   /* a15 atb div                            */
+	orr     r1, r1, #(0x2<<8)   /* div = 4                                */
+	bic     r1, r1, #(0x7<<0)   /* a15 atb div                            */
+	orr     r1, r1, #(0x3<<0)   /* div = value + 1                        */
+	str	r1, [r3]	    /* set atb div to 4, axi div to 4         */
+	dsb                         /* Synchronise side-effects of axi config */
+	
+	/* Enable CCI snoops. */
+	ldr	r3, =SUNXI_CCI_PHYS_BASE + SUNXI_CCI_A7_OFFSET
+	cmp	r0, #0		/* A7 cluster? */
+	addne	r3, r3, #SUNXI_CCI_A15_OFFSET - SUNXI_CCI_A7_OFFSET
+
+	@ r3 now points to the correct CCI slave register block
+	ldr	r1, [r3, #SLAVE_SNOOPCTL_OFFSET]
+	orr	r1, r1, #SNOOPCTL_SNOOP_ENABLE
+	orr	r1, r1, #SNOOPCTL_DVM_ENABLE
+	str	r1, [r3, #SLAVE_SNOOPCTL_OFFSET]	/* enable CCI snoops */
+
+	/* Wait for snoop control change to complete */
+	ldr	r3, =SUNXI_CCI_PHYS_BASE
+1:
+        ldr	r1, [r3, #CCI_STATUS_OFFSET]
+	tst	r1, #STATUS_CHANGE_PENDING
+	bne	1b
+	dsb                             /* Synchronise side-effects of enabling CCI */
+
+	cmp	r0, #1                  /* A15 cluster ?  */
+	bne     2f
+	
+	/* a80 platform-specific Cortex-A15 setup */
+	mrc p15, 1, r1, c15, c0, 4      /* ACTLR2 */
+	orr r1, r1, #(0x1<<31)          /* Enable CPU regional clock gates */
+	mcr p15, 1, r1, c15, c0, 4
+	
+	mrc p15, 1, r1, c15, c0, 0      /* L2ACTLR */
+	orr r1, r1, #(0x1<<26)          /* Enables L2, GIC, and Timer regional clock gates */
+	mcr p15, 1, r1, c15, c0, 0
+	
+	mrc p15, 1, r1, c15, c0, 0      /* L2ACTLR */
+	orr r1, r1, #(0x1<<3)           /* Disables clean/evict from being pushed to external */
+	mcr p15, 1, r1, c15, c0, 0
+
+	mrc p15, 1, r1, c9, c0, 2
+	bic r1, r1, #(0x7<<0)           /* L2 data ram latency */
+	orr r1, r1, #(0x3<<0)
+	mcr p15, 1, r1, c9, c0, 2
+
+2:
+        /* a80 platform-specific operations porcess done. */
+	bx	lr
+UNWIND(	.fnend)
+END_FUNC sunxi_secondary_fixup
diff --git a/core/arch/arm/plat-sunxi/sub.mk b/core/arch/arm/plat-sunxi/sub.mk
new file mode 100644
index 0000000..7c98a65
--- /dev/null
+++ b/core/arch/arm/plat-sunxi/sub.mk
@@ -0,0 +1,9 @@
+global-incdirs-y += .
+srcs-y += entry.S
+srcs-y += main.c
+srcs-y += rng_support.c
+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/conf.mk b/core/arch/arm/plat-ti/conf.mk
new file mode 100644
index 0000000..64e499f
--- /dev/null
+++ b/core/arch/arm/plat-ti/conf.mk
@@ -0,0 +1,24 @@
+PLATFORM_FLAVOR ?= dra7xx
+
+CFG_WITH_STACK_CANARIES ?= y
+CFG_WITH_STATS ?= y
+CFG_WITH_SOFTWARE_PRNG ?= n
+
+$(call force,CFG_8250_UART,y)
+$(call force,CFG_ARM32_core,y)
+$(call force,CFG_GENERIC_BOOT,y)
+$(call force,CFG_HWSUPP_MEM_PERM_PXN,y)
+$(call force,CFG_PM_STUBS,y)
+$(call force,CFG_SECURE_TIME_SOURCE_CNTPCT,y)
+$(call force,CFG_GIC,y)
+ifneq ($(CFG_WITH_SOFTWARE_PRNG),y)
+$(call force,CFG_DRA7_RNG,y)
+endif
+
+# 32-bit flags
+arm32-platform-cpuarch		:= cortex-a15
+arm32-platform-cflags		+= -mcpu=$(arm32-platform-cpuarch)
+arm32-platform-aflags		+= -mcpu=$(arm32-platform-cpuarch)
+core_arm32-platform-aflags	+= -mfpu=neon
+
+ta-targets = ta_arm32
diff --git a/core/arch/arm/plat-ti/console.c b/core/arch/arm/plat-ti/console.c
new file mode 100644
index 0000000..48f0f65
--- /dev/null
+++ b/core/arch/arm/plat-ti/console.c
@@ -0,0 +1,67 @@
+/*
+ * Copyright (c) 2016, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <console.h>
+#include <drivers/serial8250_uart.h>
+#include <mm/core_memprot.h>
+#include <platform_config.h>
+
+register_phys_mem(MEM_AREA_IO_NSEC,
+		  CONSOLE_UART_BASE,
+		  SERIAL8250_UART_REG_SIZE);
+
+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)
+{
+	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());
+}
diff --git a/core/arch/arm/plat-ti/kern.ld.S b/core/arch/arm/plat-ti/kern.ld.S
new file mode 100644
index 0000000..8d794ee
--- /dev/null
+++ b/core/arch/arm/plat-ti/kern.ld.S
@@ -0,0 +1 @@
+#include "../kernel/kern.ld.S"
diff --git a/core/arch/arm/plat-ti/link.mk b/core/arch/arm/plat-ti/link.mk
new file mode 100644
index 0000000..448ab89
--- /dev/null
+++ b/core/arch/arm/plat-ti/link.mk
@@ -0,0 +1 @@
+include core/arch/arm/kernel/link.mk
diff --git a/core/arch/arm/plat-ti/main.c b/core/arch/arm/plat-ti/main.c
new file mode 100644
index 0000000..c811862
--- /dev/null
+++ b/core/arch/arm/plat-ti/main.c
@@ -0,0 +1,151 @@
+/*
+ * 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 <platform_config.h>
+
+#include <stdint.h>
+#include <string.h>
+#include <assert.h>
+#include <drivers/gic.h>
+#include <arm.h>
+#include <kernel/generic_boot.h>
+#include <kernel/panic.h>
+#include <kernel/pm_stubs.h>
+#include <trace.h>
+#include <kernel/misc.h>
+#include <kernel/mutex.h>
+#include <kernel/tee_time.h>
+#include <mm/core_mmu.h>
+#include <mm/core_memprot.h>
+#include <tee/entry_std.h>
+#include <tee/entry_fast.h>
+#include <console.h>
+#include <sm/sm.h>
+
+static struct gic_data gic_data;
+
+register_phys_mem(MEM_AREA_IO_SEC, GICC_BASE, GICC_SIZE);
+register_phys_mem(MEM_AREA_IO_SEC, GICD_BASE, GICD_SIZE);
+
+void main_init_gic(void)
+{
+	vaddr_t gicc_base;
+	vaddr_t gicd_base;
+
+	gicc_base = (vaddr_t)phys_to_virt(GICC_BASE, MEM_AREA_IO_SEC);
+	gicd_base = (vaddr_t)phys_to_virt(GICD_BASE, MEM_AREA_IO_SEC);
+
+	if (!gicc_base || !gicd_base)
+		panic();
+
+	gic_init(&gic_data, gicc_base, gicd_base);
+	itr_init(&gic_data.chip);
+}
+
+void main_secondary_init_gic(void)
+{
+	gic_cpu_init(&gic_data);
+}
+
+static void main_fiq(void)
+{
+	gic_it_handle(&gic_data);
+}
+
+static const struct thread_handlers handlers = {
+	.std_smc = tee_entry_std,
+	.fast_smc = tee_entry_fast,
+	.fiq = main_fiq,
+	.cpu_on = pm_panic,
+	.cpu_off = pm_panic,
+	.cpu_suspend = pm_panic,
+	.cpu_resume = pm_panic,
+	.system_off = pm_panic,
+	.system_reset = pm_panic,
+};
+
+const struct thread_handlers *generic_boot_get_handlers(void)
+{
+	return &handlers;
+}
+
+struct plat_nsec_ctx {
+	uint32_t usr_sp;
+	uint32_t usr_lr;
+	uint32_t svc_sp;
+	uint32_t svc_lr;
+	uint32_t svc_spsr;
+	uint32_t abt_sp;
+	uint32_t abt_lr;
+	uint32_t abt_spsr;
+	uint32_t und_sp;
+	uint32_t und_lr;
+	uint32_t und_spsr;
+	uint32_t irq_sp;
+	uint32_t irq_lr;
+	uint32_t irq_spsr;
+	uint32_t fiq_sp;
+	uint32_t fiq_lr;
+	uint32_t fiq_spsr;
+	uint32_t fiq_rx[5];
+	uint32_t mon_lr;
+	uint32_t mon_spsr;
+};
+
+void init_sec_mon(unsigned long nsec_entry)
+{
+	struct plat_nsec_ctx *plat_ctx;
+	struct sm_nsec_ctx *nsec_ctx;
+
+	plat_ctx = phys_to_virt(nsec_entry, MEM_AREA_IO_SEC);
+	if (!plat_ctx)
+		panic();
+
+	/* Invalidate cache to fetch data from external memory */
+	cache_maintenance_l1(DCACHE_AREA_INVALIDATE,
+			     plat_ctx, sizeof(*plat_ctx));
+
+	/* Initialize secure monitor */
+	nsec_ctx = sm_get_nsec_ctx();
+
+	nsec_ctx->mode_regs.usr_sp = plat_ctx->usr_sp;
+	nsec_ctx->mode_regs.usr_lr = plat_ctx->usr_lr;
+	nsec_ctx->mode_regs.irq_spsr = plat_ctx->irq_spsr;
+	nsec_ctx->mode_regs.irq_sp = plat_ctx->irq_sp;
+	nsec_ctx->mode_regs.irq_lr = plat_ctx->irq_lr;
+	nsec_ctx->mode_regs.svc_spsr = plat_ctx->svc_spsr;
+	nsec_ctx->mode_regs.svc_sp = plat_ctx->svc_sp;
+	nsec_ctx->mode_regs.svc_lr = plat_ctx->svc_lr;
+	nsec_ctx->mode_regs.abt_spsr = plat_ctx->abt_spsr;
+	nsec_ctx->mode_regs.abt_sp = plat_ctx->abt_sp;
+	nsec_ctx->mode_regs.abt_lr = plat_ctx->abt_lr;
+	nsec_ctx->mode_regs.und_spsr = plat_ctx->und_spsr;
+	nsec_ctx->mode_regs.und_sp = plat_ctx->und_sp;
+	nsec_ctx->mode_regs.und_lr = plat_ctx->und_lr;
+	nsec_ctx->mon_lr = plat_ctx->mon_lr;
+	nsec_ctx->mon_spsr = plat_ctx->mon_spsr;
+}
diff --git a/core/arch/arm/plat-ti/platform_config.h b/core/arch/arm/plat-ti/platform_config.h
new file mode 100644
index 0000000..c1aaee9
--- /dev/null
+++ b/core/arch/arm/plat-ti/platform_config.h
@@ -0,0 +1,109 @@
+/*
+ * 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.
+ */
+
+#ifndef PLATFORM_CONFIG_H
+#define PLATFORM_CONFIG_H
+
+#if defined(PLATFORM_FLAVOR_dra7xx)
+
+#define DRAM0_BASE		0xbe000000
+#define DRAM0_SIZE		0x02000000
+
+#ifdef CFG_WITH_PAGER
+#error Pager not supported on this platform
+#endif /*CFG_WITH_PAGER*/
+
+/* Location of protected DDR on the DRA7xx platform */
+#define TZDRAM_BASE		0xbe000000
+#define TZDRAM_SIZE		0x01c00000
+
+#define CFG_TEE_CORE_NB_CORE	2
+
+/* UART1 */
+#define CONSOLE_UART_BASE		0x4806A000
+#define CONSOLE_UART_CLK_IN_HZ	48000000
+#define UART_BAUDRATE			115200
+
+#define GIC_BASE        0x48210000
+#define GICC_OFFSET     0x2000
+#define GICC_SIZE       0x1000
+#define GICD_OFFSET     0x1000
+#define GICD_SIZE       0x1000
+#define GICC_BASE       (GIC_BASE + GICC_OFFSET)
+#define GICD_BASE       (GIC_BASE + GICD_OFFSET)
+
+#define SECRAM_BASE     0x40200000
+
+/* RNG */
+#define RNG_BASE        0x48090000
+
+#else
+#error "Unknown platform flavor"
+#endif
+
+/* Make stacks aligned to data cache line length */
+#define STACK_ALIGNMENT		64
+
+/* Full GlobalPlatform test suite requires CFG_SHMEM_SIZE to be at least 2MB */
+#define CFG_SHMEM_START		(DRAM0_BASE + TZDRAM_SIZE)
+#define CFG_SHMEM_SIZE		0x400000
+
+#define CFG_TEE_RAM_VA_SIZE	(1024 * 1024)
+
+#ifndef CFG_TEE_LOAD_ADDR
+#define CFG_TEE_LOAD_ADDR	(CFG_TEE_RAM_START + 0x100)
+#endif
+
+/*
+ * Assumes that either TZSRAM isn't large enough or TZSRAM doesn't exist,
+ * everything is in TZDRAM.
+ * +------------------+
+ * |        | TEE_RAM |
+ * + TZDRAM +---------+
+ * |        | TA_RAM  |
+ * +--------+---------+
+ */
+#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		ROUNDDOWN((TZDRAM_SIZE - CFG_TEE_RAM_VA_SIZE), \
+					  CORE_MMU_DEVICE_SIZE)
+
+#define DEVICE2_PA_BASE		ROUNDDOWN(SECRAM_BASE, CORE_MMU_DEVICE_SIZE)
+#define DEVICE2_VA_BASE		DEVICE2_PA_BASE
+#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/sub.mk b/core/arch/arm/plat-ti/sub.mk
new file mode 100644
index 0000000..3a8214b
--- /dev/null
+++ b/core/arch/arm/plat-ti/sub.mk
@@ -0,0 +1,3 @@
+global-incdirs-y += .
+srcs-y += main.c
+srcs-y += console.c
diff --git a/core/arch/arm/plat-vexpress/conf.mk b/core/arch/arm/plat-vexpress/conf.mk
new file mode 100644
index 0000000..5d7d8c1
--- /dev/null
+++ b/core/arch/arm/plat-vexpress/conf.mk
@@ -0,0 +1,71 @@
+PLATFORM_FLAVOR ?= qemu_virt
+
+# 32-bit flags
+arm32-platform-cpuarch		:= cortex-a15
+arm32-platform-cflags		+= -mcpu=$(arm32-platform-cpuarch)
+arm32-platform-aflags		+= -mcpu=$(arm32-platform-cpuarch)
+core_arm32-platform-aflags	+= -mfpu=neon
+
+ifeq ($(PLATFORM_FLAVOR),fvp)
+platform-flavor-armv8 := 1
+platform-debugger-arm := 1
+endif
+ifeq ($(PLATFORM_FLAVOR),juno)
+platform-flavor-armv8 := 1
+platform-debugger-arm := 1
+endif
+ifeq ($(PLATFORM_FLAVOR),qemu_armv8a)
+platform-flavor-armv8 := 1
+$(call force,CFG_DT,y)
+endif
+
+
+ifeq ($(platform-debugger-arm),1)
+# ARM debugger needs this
+platform-cflags-debug-info = -gdwarf-2
+platform-aflags-debug-info = -gdwarf-2
+endif
+
+ifeq ($(platform-flavor-armv8),1)
+$(call force,CFG_WITH_ARM_TRUSTED_FW,y)
+endif
+
+$(call force,CFG_GENERIC_BOOT,y)
+$(call force,CFG_GIC,y)
+$(call force,CFG_HWSUPP_MEM_PERM_PXN,y)
+$(call force,CFG_PL011,y)
+$(call force,CFG_PM_STUBS,y)
+$(call force,CFG_SECURE_TIME_SOURCE_CNTPCT,y)
+
+ta-targets = ta_arm32
+
+ifeq ($(CFG_ARM64_core),y)
+$(call force,CFG_WITH_LPAE,y)
+ta-targets += ta_arm64
+else
+$(call force,CFG_ARM32_core,y)
+endif
+
+CFG_TEE_FS_KEY_MANAGER_TEST ?= y
+CFG_WITH_STACK_CANARIES ?= y
+CFG_WITH_STATS ?= y
+
+ifeq ($(PLATFORM_FLAVOR),juno)
+CFG_CRYPTO_WITH_CE ?= y
+endif
+
+ifeq ($(PLATFORM_FLAVOR),qemu_virt)
+ifeq ($(CFG_CORE_SANITIZE_KADDRESS),y)
+# CFG_ASAN_SHADOW_OFFSET is calculated as:
+# (&__asan_shadow_start - (CFG_TEE_RAM_START / 8)
+# This is unfortunately currently not possible to do in make so we have to
+# calculate it offline, there's some asserts in
+# core/arch/arm/kernel/generic_boot.c to check that we got it right
+CFG_ASAN_SHADOW_OFFSET=0xc4e38e0
+endif
+$(call force,CFG_DT,y)
+# SE API is only supported by QEMU Virt platform
+CFG_SE_API ?= y
+CFG_SE_API_SELF_TEST ?= y
+CFG_PCSC_PASSTHRU_READER_DRV ?= y
+endif
diff --git a/core/arch/arm/plat-vexpress/juno_core_pos_a32.S b/core/arch/arm/plat-vexpress/juno_core_pos_a32.S
new file mode 100644
index 0000000..a75a65d
--- /dev/null
+++ b/core/arch/arm/plat-vexpress/juno_core_pos_a32.S
@@ -0,0 +1,45 @@
+/*
+ * 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 <asm.S>
+#include <arm.h>
+#include <arm32_macros.S>
+#include <kernel/unwind.h>
+
+/* For Juno number the two A57s as 4 to 5 and A53s as 0 to 3 */
+FUNC get_core_pos , :
+UNWIND(	.fnstart)
+	read_mpidr r0
+	/* Calculate CorePos = ((ClusterId ^ 1) * 4) + CoreId */
+	and	r1, r0, #MPIDR_CPU_MASK
+	and	r0, r0, #MPIDR_CLUSTER_MASK
+	eor	r0, r0, #(1 << MPIDR_CLUSTER_SHIFT)
+	add	r0, r1, r0, LSR #6
+	bx	lr
+UNWIND(	.fnend)
+END_FUNC get_core_pos
+
diff --git a/core/arch/arm/plat-vexpress/juno_core_pos_a64.S b/core/arch/arm/plat-vexpress/juno_core_pos_a64.S
new file mode 100644
index 0000000..47bb85d
--- /dev/null
+++ b/core/arch/arm/plat-vexpress/juno_core_pos_a64.S
@@ -0,0 +1,42 @@
+/*
+ * Copyright (c) 2014, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <asm.S>
+#include <arm.h>
+
+/* For Juno number the two A57s as 4 to 5 and A53s as 0 to 3 */
+FUNC get_core_pos , :
+	mrs	x0, mpidr_el1
+	/* Calculate CorePos = ((ClusterId ^ 1) * 4) + CoreId */
+	and	x1, x0, #MPIDR_CPU_MASK
+	and	x0, x0, #MPIDR_CLUSTER_MASK
+	eor	x0, x0, #(1 << MPIDR_CLUSTER_SHIFT)
+	add	x0, x1, x0, LSR #6
+	ret
+END_FUNC get_core_pos
+
+
diff --git a/core/arch/arm/plat-vexpress/kern.ld.S b/core/arch/arm/plat-vexpress/kern.ld.S
new file mode 100644
index 0000000..8d794ee
--- /dev/null
+++ b/core/arch/arm/plat-vexpress/kern.ld.S
@@ -0,0 +1 @@
+#include "../kernel/kern.ld.S"
diff --git a/core/arch/arm/plat-vexpress/link.mk b/core/arch/arm/plat-vexpress/link.mk
new file mode 100644
index 0000000..448ab89
--- /dev/null
+++ b/core/arch/arm/plat-vexpress/link.mk
@@ -0,0 +1 @@
+include core/arch/arm/kernel/link.mk
diff --git a/core/arch/arm/plat-vexpress/main.c b/core/arch/arm/plat-vexpress/main.c
new file mode 100644
index 0000000..85ed9ee
--- /dev/null
+++ b/core/arch/arm/plat-vexpress/main.c
@@ -0,0 +1,201 @@
+/*
+ * Copyright (c) 2016, Linaro Limited
+ * 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 <stdint.h>
+#include <string.h>
+
+#include <drivers/gic.h>
+#include <drivers/pl011.h>
+#include <drivers/tzc400.h>
+
+#include <arm.h>
+#include <kernel/generic_boot.h>
+#include <kernel/pm_stubs.h>
+#include <trace.h>
+#include <kernel/misc.h>
+#include <kernel/panic.h>
+#include <kernel/tee_time.h>
+#include <tee/entry_fast.h>
+#include <tee/entry_std.h>
+#include <mm/core_memprot.h>
+#include <mm/core_mmu.h>
+#include <console.h>
+#include <keep.h>
+#include <initcall.h>
+
+static void main_fiq(void);
+
+static const struct thread_handlers handlers = {
+	.std_smc = tee_entry_std,
+	.fast_smc = tee_entry_fast,
+	.fiq = main_fiq,
+#if defined(CFG_WITH_ARM_TRUSTED_FW)
+	.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,
+#else
+	.cpu_on = pm_panic,
+	.cpu_off = pm_panic,
+	.cpu_suspend = pm_panic,
+	.cpu_resume = pm_panic,
+	.system_off = pm_panic,
+	.system_reset = pm_panic,
+#endif
+};
+
+static struct gic_data gic_data;
+
+register_phys_mem(MEM_AREA_IO_SEC, CONSOLE_UART_BASE, PL011_REG_SIZE);
+
+const struct thread_handlers *generic_boot_get_handlers(void)
+{
+	return &handlers;
+}
+
+#ifdef GIC_BASE
+
+register_phys_mem(MEM_AREA_IO_SEC, GICD_BASE, GIC_DIST_REG_SIZE);
+register_phys_mem(MEM_AREA_IO_SEC, GICC_BASE, GIC_DIST_REG_SIZE);
+
+void main_init_gic(void)
+{
+	vaddr_t gicc_base;
+	vaddr_t gicd_base;
+
+	gicc_base = (vaddr_t)phys_to_virt(GIC_BASE + GICC_OFFSET,
+					  MEM_AREA_IO_SEC);
+	gicd_base = (vaddr_t)phys_to_virt(GIC_BASE + GICD_OFFSET,
+					  MEM_AREA_IO_SEC);
+	if (!gicc_base || !gicd_base)
+		panic();
+
+#if defined(PLATFORM_FLAVOR_fvp) || defined(PLATFORM_FLAVOR_juno) || \
+	defined(PLATFORM_FLAVOR_qemu_armv8a)
+	/* On ARMv8, GIC configuration is initialized in ARM-TF */
+	gic_init_base_addr(&gic_data, gicc_base, gicd_base);
+#else
+	/* Initialize GIC */
+	gic_init(&gic_data, gicc_base, gicd_base);
+#endif
+	itr_init(&gic_data.chip);
+}
+#endif
+
+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());
+}
+
+#ifdef IT_CONSOLE_UART
+static enum itr_return console_itr_cb(struct itr_handler *h __unused)
+{
+	paddr_t uart_base = console_base();
+
+	while (pl011_have_rx_data(uart_base)) {
+		int ch __maybe_unused = pl011_getchar(uart_base);
+
+		DMSG("cpu %zu: got 0x%x", get_core_pos(), ch);
+	}
+	return ITRR_HANDLED;
+}
+
+static struct itr_handler console_itr = {
+	.it = IT_CONSOLE_UART,
+	.flags = ITRF_TRIGGER_LEVEL,
+	.handler = console_itr_cb,
+};
+KEEP_PAGER(console_itr);
+
+static TEE_Result init_console_itr(void)
+{
+	itr_add(&console_itr);
+	itr_enable(IT_CONSOLE_UART);
+	return TEE_SUCCESS;
+}
+driver_init(init_console_itr);
+#endif
+
+#ifdef CFG_TZC400
+register_phys_mem(MEM_AREA_IO_SEC, TZC400_BASE, TZC400_REG_SIZE);
+
+static TEE_Result init_tzc400(void)
+{
+	void *va;
+
+	DMSG("Initializing TZC400");
+
+	va = phys_to_virt(TZC400_BASE, MEM_AREA_IO_SEC);
+	if (!va) {
+		EMSG("TZC400 not mapped");
+		panic();
+	}
+
+	tzc_init((vaddr_t)va);
+	tzc_dump_state();
+
+	return TEE_SUCCESS;
+}
+
+service_init(init_tzc400);
+#endif /*CFG_TZC400*/
diff --git a/core/arch/arm/plat-vexpress/platform_config.h b/core/arch/arm/plat-vexpress/platform_config.h
new file mode 100644
index 0000000..bd006ca
--- /dev/null
+++ b/core/arch/arm/plat-vexpress/platform_config.h
@@ -0,0 +1,298 @@
+/*
+ * Copyright (c) 2014, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef PLATFORM_CONFIG_H
+#define PLATFORM_CONFIG_H
+
+#include <stdint.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*/
+
+#if defined(PLATFORM_FLAVOR_fvp)
+
+#define GIC_BASE		0x2c000000
+#define UART0_BASE		0x1c090000
+#define UART1_BASE		0x1c0a0000
+#define UART2_BASE		0x1c0b0000
+#define UART3_BASE		0x1c0c0000
+#define TZC400_BASE		0x2a4a0000
+
+#define IT_UART1		38
+
+#define CONSOLE_UART_BASE	UART1_BASE
+#define IT_CONSOLE_UART		IT_UART1
+
+#elif defined(PLATFORM_FLAVOR_juno)
+
+#define GIC_BASE		0x2c010000
+
+/* FPGA UART0 */
+#define UART0_BASE		0x1c090000
+/* FPGA UART1 */
+#define UART1_BASE		0x1c0a0000
+/* SoC UART0 */
+#define UART2_BASE		0x7ff80000
+/* SoC UART1 */
+#define UART3_BASE		0x7ff70000
+
+
+#define UART0_CLK_IN_HZ		24000000
+#define UART1_CLK_IN_HZ		24000000
+#define UART2_CLK_IN_HZ		7273800
+#define UART3_CLK_IN_HZ		7273800
+
+
+#define IT_UART3		116
+
+#define CONSOLE_UART_BASE	UART3_BASE
+#define IT_CONSOLE_UART		IT_UART3
+#define CONSOLE_UART_CLK_IN_HZ	UART3_CLK_IN_HZ
+
+#elif defined(PLATFORM_FLAVOR_qemu_virt)
+
+#define GIC_BASE		0x08000000
+#define UART0_BASE		0x09000000
+#define UART1_BASE		0x09040000
+#define PCSC_BASE		0x09100000
+
+#define IT_UART1		40
+#define IT_PCSC			37
+
+#define CONSOLE_UART_BASE	UART1_BASE
+#define IT_CONSOLE_UART		IT_UART1
+
+#elif defined(PLATFORM_FLAVOR_qemu_armv8a)
+
+#define UART0_BASE		0x09000000
+#define UART1_BASE		0x09040000
+
+#define CONSOLE_UART_BASE	UART1_BASE
+
+#else
+#error "Unknown platform flavor"
+#endif
+
+#if defined(PLATFORM_FLAVOR_fvp)
+/*
+ * FVP specifics.
+ */
+
+#define DRAM0_BASE		0x80000000
+#define DRAM0_SIZE		0x80000000
+
+#ifdef CFG_WITH_PAGER
+
+/* Emulated SRAM */
+#define TZSRAM_BASE		(0x06000000)
+#define TZSRAM_SIZE		CFG_CORE_TZSRAM_EMUL_SIZE
+
+#define TZDRAM_BASE		(TZSRAM_BASE + CFG_TEE_RAM_VA_SIZE)
+#define TZDRAM_SIZE		(0x02000000 - CFG_TEE_RAM_VA_SIZE)
+
+#else /*CFG_WITH_PAGER*/
+
+/* Location of trusted dram on the base fvp */
+#define TZDRAM_BASE		0x06000000
+#define TZDRAM_SIZE		0x02000000
+
+#endif /*CFG_WITH_PAGER*/
+
+#define CFG_TEE_CORE_NB_CORE	8
+
+#define CFG_SHMEM_START		(DRAM0_BASE + 0x3000000)
+#define CFG_SHMEM_SIZE		0x200000
+
+#define GICC_OFFSET		0x0
+#define GICD_OFFSET		0x3000000
+
+#elif defined(PLATFORM_FLAVOR_juno)
+/*
+ * Juno specifics.
+ */
+
+#define DRAM0_BASE		0x80000000
+#define DRAM0_SIZE		0x7F000000
+
+#ifdef CFG_WITH_PAGER
+
+/* Emulated SRAM */
+#define TZSRAM_BASE		0xFF000000
+#define TZSRAM_SIZE		CFG_CORE_TZSRAM_EMUL_SIZE
+
+#define TZDRAM_BASE		(TZSRAM_BASE + CFG_TEE_RAM_VA_SIZE)
+#define TZDRAM_SIZE		(0x00E00000 - CFG_TEE_RAM_VA_SIZE)
+
+#else /*CFG_WITH_PAGER*/
+/*
+ * Last part of DRAM is reserved as secure dram, note that the last 2MiB
+ * of DRAM0 is used by SCP dor DDR retraining.
+ */
+#define TZDRAM_BASE		0xFF000000
+/*
+ * Should be
+ * #define TZDRAM_SIZE		0x00FF8000
+ * but is smaller due to SECTION_SIZE alignment, can be fixed once
+ * OP-TEE OS is mapped using small pages instead.
+ */
+#define TZDRAM_SIZE		0x00E00000
+#endif /*CFG_WITH_PAGER*/
+
+#define CFG_TEE_CORE_NB_CORE	6
+
+#define CFG_SHMEM_START		(DRAM0_BASE + DRAM0_SIZE - CFG_SHMEM_SIZE)
+#define CFG_SHMEM_SIZE		0x200000
+
+#define GICC_OFFSET		0x1f000
+#define GICD_OFFSET		0
+
+#elif defined(PLATFORM_FLAVOR_qemu_virt)
+/*
+ * QEMU virt specifics.
+ */
+
+#define DRAM0_BASE		UINTPTR_C(0x40000000)
+#define DRAM0_SIZE		(UINTPTR_C(0x42100000) - CFG_SHMEM_SIZE)
+
+#define DRAM0_TEERES_BASE	(DRAM0_BASE + DRAM0_SIZE)
+#define DRAM0_TEERES_SIZE	CFG_SHMEM_SIZE
+
+#ifdef CFG_WITH_PAGER
+
+/* Emulated SRAM */
+#define TZSRAM_BASE		0x0e000000
+#define TZSRAM_SIZE		CFG_CORE_TZSRAM_EMUL_SIZE
+
+#define TZDRAM_BASE		(TZSRAM_BASE + TZSRAM_SIZE)
+#define TZDRAM_SIZE		(0x01000000 - TZSRAM_SIZE)
+
+#else /* CFG_WITH_PAGER */
+
+#define TZDRAM_BASE		0x0e000000
+#define TZDRAM_SIZE		0x01000000
+
+#endif /* CFG_WITH_PAGER */
+
+
+#define CFG_TEE_CORE_NB_CORE	2
+
+#define CFG_SHMEM_START		(DRAM0_TEERES_BASE + \
+					(DRAM0_TEERES_SIZE - CFG_SHMEM_SIZE))
+#define CFG_SHMEM_SIZE		0x200000
+
+#define GICD_OFFSET		0
+#define GICC_OFFSET		0x10000
+
+
+#elif defined(PLATFORM_FLAVOR_qemu_armv8a)
+
+#ifdef CFG_WITH_PAGER
+#error "Pager not supported for platform vexpress-qemu_armv8a"
+#endif
+
+#define DRAM0_BASE		UINTPTR_C(0x40000000)
+#define DRAM0_SIZE		(UINTPTR_C(0x40000000) - CFG_SHMEM_SIZE)
+
+#define DRAM0_TEERES_BASE	(DRAM0_BASE + DRAM0_SIZE)
+#define DRAM0_TEERES_SIZE	CFG_SHMEM_SIZE
+
+#define TZDRAM_BASE		0x0e100000
+#define TZDRAM_SIZE		0x00f00000
+
+#define CFG_TEE_CORE_NB_CORE	2
+
+#define CFG_SHMEM_START		(DRAM0_TEERES_BASE + \
+					(DRAM0_TEERES_SIZE - CFG_SHMEM_SIZE))
+#define CFG_SHMEM_SIZE		0x200000
+
+#else
+#error "Unknown platform flavor"
+#endif
+
+#define CFG_TEE_RAM_VA_SIZE	(1024 * 1024)
+
+#ifndef CFG_TEE_LOAD_ADDR
+#define CFG_TEE_LOAD_ADDR	CFG_TEE_RAM_START
+#endif
+
+#ifdef CFG_WITH_PAGER
+/*
+ * Have TZSRAM either as real physical or emulated by reserving an area
+ * somewhere else.
+ *
+ * +------------------+
+ * | TZSRAM | TEE_RAM |
+ * +--------+---------+
+ * | TZDRAM | TA_RAM  |
+ * +--------+---------+
+ */
+#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 +---------+
+ * |        | TA_RAM  |
+ * +--------+---------+
+ */
+#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		ROUNDDOWN((TZDRAM_SIZE - CFG_TEE_RAM_VA_SIZE), \
+					  CORE_MMU_DEVICE_SIZE)
+#endif
+
+#ifdef GIC_BASE
+#define GICD_BASE		(GIC_BASE + GICD_OFFSET)
+#define GICC_BASE		(GIC_BASE + GICC_OFFSET)
+#endif
+
+#ifndef UART_BAUDRATE
+#define UART_BAUDRATE		115200
+#endif
+#ifndef CONSOLE_BAUDRATE
+#define CONSOLE_BAUDRATE	UART_BAUDRATE
+#endif
+
+/* For virtual platforms where there isn't a clock */
+#ifndef CONSOLE_UART_CLK_IN_HZ
+#define CONSOLE_UART_CLK_IN_HZ	1
+#endif
+
+#endif /*PLATFORM_CONFIG_H*/
diff --git a/core/arch/arm/plat-vexpress/sub.mk b/core/arch/arm/plat-vexpress/sub.mk
new file mode 100644
index 0000000..086f539
--- /dev/null
+++ b/core/arch/arm/plat-vexpress/sub.mk
@@ -0,0 +1,7 @@
+global-incdirs-y += .
+srcs-y += main.c
+ifeq ($(PLATFORM_FLAVOR_juno),y)
+srcs-$(CFG_ARM32_core) += juno_core_pos_a32.S
+srcs-$(CFG_ARM64_core) += juno_core_pos_a64.S
+endif
+srcs-y += vendor_props.c
diff --git a/core/arch/arm/plat-vexpress/vendor_props.c b/core/arch/arm/plat-vexpress/vendor_props.c
new file mode 100644
index 0000000..2f4d10d
--- /dev/null
+++ b/core/arch/arm/plat-vexpress/vendor_props.c
@@ -0,0 +1,91 @@
+/*
+ * Copyright (c) 2016, Linaro Limited.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+#include <tee/tee_svc.h>
+#include <user_ta_header.h>
+#include <util.h>
+#include <kernel/tee_ta_manager.h>
+#include <kernel/tee_common_otp.h>
+#include <tee/tee_cryp_utl.h>
+
+/*
+ * The data to hash is 48 bytes made up of:
+ * - 16 bytes: the UUID of the calling TA.
+ * - 32 bytes: the hardware device ID
+ * The resulting endorsement seed is 32 bytes.
+ *
+ * The output buffer is the "binary" struct defined in
+ * the "prop_value" union and therefore comprises:
+ * -  4 bytes: the size of the binary value data (32)
+ * - 32 bytes: the binary value data (endorsement seed)
+ *
+ * Note that this code assumes an endorsement seed
+ * size == device ID size for convenience.
+ */
+static TEE_Result get_prop_endorsement(struct tee_ta_session *sess,
+				       void *buf, size_t *blen)
+{
+	TEE_Result res;
+	uint32_t ta_endorsement_seed_size = 32;
+	uint8_t data[sizeof(TEE_UUID) + ta_endorsement_seed_size];
+	uint32_t bin[1 + ta_endorsement_seed_size / sizeof(uint32_t)];
+	uint32_t *bin_len = (uint32_t *)bin;
+	uint8_t *bin_val = (uint8_t *)(&bin[1]);
+
+	if (*blen < sizeof(bin)) {
+		*blen = sizeof(bin);
+		return TEE_ERROR_SHORT_BUFFER;
+	}
+	*blen = sizeof(bin);
+
+	memcpy(data, &sess->ctx->uuid, sizeof(TEE_UUID));
+
+	if (tee_otp_get_die_id(&data[sizeof(TEE_UUID)],
+			       ta_endorsement_seed_size))
+		return TEE_ERROR_BAD_STATE;
+
+	res = tee_hash_createdigest(TEE_ALG_SHA256, data, sizeof(data),
+				    bin_val, ta_endorsement_seed_size);
+	if (res != TEE_SUCCESS)
+		return TEE_ERROR_BAD_STATE;
+
+	*bin_len = ta_endorsement_seed_size;
+
+	return tee_svc_copy_to_user((void *)buf, bin, sizeof(bin));
+}
+
+static const struct tee_props vendor_propset_array_tee[] = {
+	{
+		.name = "com.microsoft.ta.endorsementSeed",
+		.prop_type = USER_TA_PROP_TYPE_BINARY_BLOCK,
+		.get_prop_func = get_prop_endorsement
+	},
+};
+
+const struct tee_vendor_props vendor_props_tee = {
+	.props = vendor_propset_array_tee,
+	.len = ARRAY_SIZE(vendor_propset_array_tee),
+};
diff --git a/core/arch/arm/plat-zynq7k/conf.mk b/core/arch/arm/plat-zynq7k/conf.mk
new file mode 100644
index 0000000..f457d8d
--- /dev/null
+++ b/core/arch/arm/plat-zynq7k/conf.mk
@@ -0,0 +1,24 @@
+PLATFORM_FLAVOR ?= zc702
+
+arm32-platform-cpuarch		:= cortex-a9
+
+arm32-platform-cflags		+= -mcpu=$(arm32-platform-cpuarch)
+arm32-platform-aflags		+= -mcpu=$(arm32-platform-cpuarch)
+core_arm32-platform-aflags	+= -mfpu=neon
+
+$(call force,CFG_ARM32_core,y)
+$(call force,CFG_GENERIC_BOOT,y)
+$(call force,CFG_GIC,y)
+$(call force,CFG_CDNS_UART,y)
+$(call force,CFG_PM_STUBS,y)
+$(call force,CFG_WITH_SOFTWARE_PRNG,y)
+$(call force,CFG_PL310,y)
+$(call force,CFG_PL310_LOCKED,y)
+$(call force,CFG_SECURE_TIME_SOURCE_REE,y)
+
+ta-targets = ta_arm32
+
+CFG_BOOT_SYNC_CPU ?= y
+CFG_BOOT_SECONDARY_REQUEST ?= y
+CFG_CRYPTO_SIZE_OPTIMIZATION ?= n
+CFG_WITH_STACK_CANARIES ?= y
diff --git a/core/arch/arm/plat-zynq7k/kern.ld.S b/core/arch/arm/plat-zynq7k/kern.ld.S
new file mode 100644
index 0000000..8d794ee
--- /dev/null
+++ b/core/arch/arm/plat-zynq7k/kern.ld.S
@@ -0,0 +1 @@
+#include "../kernel/kern.ld.S"
diff --git a/core/arch/arm/plat-zynq7k/link.mk b/core/arch/arm/plat-zynq7k/link.mk
new file mode 100644
index 0000000..448ab89
--- /dev/null
+++ b/core/arch/arm/plat-zynq7k/link.mk
@@ -0,0 +1 @@
+include core/arch/arm/kernel/link.mk
diff --git a/core/arch/arm/plat-zynq7k/main.c b/core/arch/arm/plat-zynq7k/main.c
new file mode 100644
index 0000000..74bc1ce
--- /dev/null
+++ b/core/arch/arm/plat-zynq7k/main.c
@@ -0,0 +1,276 @@
+/*
+ * Copyright (C) 2015 Freescale Semiconductor, Inc.
+ * All rights reserved.
+ * Copyright (c) 2016, Wind River Systems.
+ * 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 <console.h>
+#include <drivers/cdns_uart.h>
+#include <drivers/gic.h>
+#include <io.h>
+#include <kernel/generic_boot.h>
+#include <kernel/misc.h>
+#include <kernel/panic.h>
+#include <kernel/pm_stubs.h>
+#include <kernel/tz_ssvce_pl310.h>
+#include <mm/core_mmu.h>
+#include <mm/core_memprot.h>
+#include <platform_config.h>
+#include <platform_smc.h>
+#include <stdint.h>
+#include <tee/entry_fast.h>
+#include <tee/entry_std.h>
+
+static void main_fiq(void);
+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,
+	.cpu_on = pm_panic,
+	.cpu_off = pm_panic,
+	.cpu_suspend = pm_panic,
+	.cpu_resume = pm_panic,
+	.system_off = pm_panic,
+	.system_reset = pm_panic,
+};
+
+static struct gic_data gic_data;
+
+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);
+register_phys_mem(MEM_AREA_IO_SEC, PL310_BASE, CORE_MMU_DEVICE_SIZE);
+register_phys_mem(MEM_AREA_IO_SEC, SLCR_BASE, CORE_MMU_DEVICE_SIZE);
+
+const struct thread_handlers *generic_boot_get_handlers(void)
+{
+	return &handlers;
+}
+
+static void main_fiq(void)
+{
+	panic();
+}
+
+void plat_cpu_reset_late(void)
+{
+	if (!get_core_pos()) {
+		/* primary core */
+#if defined(CFG_BOOT_SECONDARY_REQUEST)
+		/* set secondary entry address and release core */
+		write32(CFG_TEE_LOAD_ADDR, SECONDARY_ENTRY_DROP);
+		dsb();
+		sev();
+#endif
+
+		/* SCU config */
+		write32(SCU_INV_CTRL_INIT, SCU_BASE + SCU_INV_SEC);
+		write32(SCU_SAC_CTRL_INIT, SCU_BASE + SCU_SAC);
+		write32(SCU_NSAC_CTRL_INIT, SCU_BASE + SCU_NSAC);
+
+		/* SCU enable */
+		write32(read32(SCU_BASE + SCU_CTRL) | 0x1,
+			SCU_BASE + SCU_CTRL);
+
+		/* NS Access control */
+		write32(ACCESS_BITS_ALL, SECURITY2_SDIO0);
+		write32(ACCESS_BITS_ALL, SECURITY3_SDIO1);
+		write32(ACCESS_BITS_ALL, SECURITY4_QSPI);
+		write32(ACCESS_BITS_ALL, SECURITY6_APB_SLAVES);
+
+		write32(SLCR_UNLOCK_MAGIC, SLCR_UNLOCK);
+
+		write32(ACCESS_BITS_ALL, SLCR_TZ_DDR_RAM);
+		write32(ACCESS_BITS_ALL, SLCR_TZ_DMA_NS);
+		write32(ACCESS_BITS_ALL, SLCR_TZ_DMA_IRQ_NS);
+		write32(ACCESS_BITS_ALL, SLCR_TZ_DMA_PERIPH_NS);
+		write32(ACCESS_BITS_ALL, SLCR_TZ_GEM);
+		write32(ACCESS_BITS_ALL, SLCR_TZ_SDIO);
+		write32(ACCESS_BITS_ALL, SLCR_TZ_USB);
+
+		write32(SLCR_LOCK_MAGIC, SLCR_LOCK);
+	}
+}
+
+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());
+}
+
+vaddr_t pl310_base(void)
+{
+	static void *va __early_bss;
+
+	if (cpu_mmu_enabled()) {
+		if (!va)
+			va = phys_to_virt(PL310_BASE, MEM_AREA_IO_SEC);
+		return (vaddr_t)va;
+	}
+	return PL310_BASE;
+}
+
+void arm_cl2_config(vaddr_t pl310_base)
+{
+	/* Disable PL310 */
+	write32(0, pl310_base + PL310_CTRL);
+
+	/*
+	 * Xilinx AR#54190 recommends setting L2C RAM in SLCR
+	 * to 0x00020202 for proper cache operations.
+	 */
+	write32(SLCR_L2C_RAM_VALUE, SLCR_L2C_RAM);
+
+	write32(PL310_TAG_RAM_CTRL_INIT, pl310_base + PL310_TAG_RAM_CTRL);
+	write32(PL310_DATA_RAM_CTRL_INIT, pl310_base + PL310_DATA_RAM_CTRL);
+	write32(PL310_AUX_CTRL_INIT, pl310_base + PL310_AUX_CTRL);
+	write32(PL310_PREFETCH_CTRL_INIT, pl310_base + PL310_PREFETCH_CTRL);
+	write32(PL310_POWER_CTRL_INIT, pl310_base + PL310_POWER_CTRL);
+
+	/* invalidate all cache ways */
+	arm_cl2_invbyway(pl310_base);
+}
+
+void arm_cl2_enable(vaddr_t pl310_base)
+{
+	uint32_t val;
+
+	/* Enable PL310 ctrl -> only set lsb bit */
+	write32(1, pl310_base + PL310_CTRL);
+
+	/* if L2 FLZW enable, enable in L1 */
+	val = read32(pl310_base + PL310_AUX_CTRL);
+	if (val & 1)
+		write_actlr(read_actlr() | (1 << 3));
+}
+
+void main_init_gic(void)
+{
+	vaddr_t gicc_base;
+	vaddr_t gicd_base;
+
+	gicc_base = (vaddr_t)phys_to_virt(GIC_BASE + GICC_OFFSET,
+					  MEM_AREA_IO_SEC);
+	gicd_base = (vaddr_t)phys_to_virt(GIC_BASE + GICD_OFFSET,
+					  MEM_AREA_IO_SEC);
+
+	if (!gicc_base || !gicd_base)
+		panic();
+
+	/* Initialize GIC */
+	gic_init(&gic_data, gicc_base, gicd_base);
+	itr_init(&gic_data.chip);
+}
+
+void main_secondary_init_gic(void)
+{
+	gic_cpu_init(&gic_data);
+}
+
+static vaddr_t slcr_access_range[] = {
+	0x004, 0x008,	/* lock, unlock */
+	0x100, 0x1FF,	/* PLL */
+	0x200, 0x2FF,	/* Reset */
+	0xA00, 0xAFF	/* L2C */
+};
+
+static uint32_t write_slcr(uint32_t addr, uint32_t val)
+{
+	uint32_t i;
+
+	for (i = 0; i < ARRAY_SIZE(slcr_access_range); i += 2) {
+		if (addr >= slcr_access_range[i] &&
+		    addr <= slcr_access_range[i+1]) {
+			static vaddr_t va __early_bss;
+
+			if (!va)
+				va = (vaddr_t)phys_to_virt(SLCR_BASE,
+							   MEM_AREA_IO_SEC);
+			write32(val, va + addr);
+			return OPTEE_SMC_RETURN_OK;
+		}
+	}
+	return OPTEE_SMC_RETURN_EBADADDR;
+}
+
+static uint32_t read_slcr(uint32_t addr, uint32_t *val)
+{
+	uint32_t i;
+
+	for (i = 0; i < ARRAY_SIZE(slcr_access_range); i += 2) {
+		if (addr >= slcr_access_range[i] &&
+		    addr <= slcr_access_range[i+1]) {
+			static vaddr_t va __early_bss;
+
+			if (!va)
+				va = (vaddr_t)phys_to_virt(SLCR_BASE,
+							   MEM_AREA_IO_SEC);
+			*val = read32(va + addr);
+			return OPTEE_SMC_RETURN_OK;
+		}
+	}
+	return OPTEE_SMC_RETURN_EBADADDR;
+}
+
+static void platform_tee_entry_fast(struct thread_smc_args *args)
+{
+	switch (args->a0) {
+	case ZYNQ7K_SMC_SLCR_WRITE:
+		args->a0 = write_slcr(args->a1, args->a2);
+		break;
+	case ZYNQ7K_SMC_SLCR_READ:
+		args->a0 = read_slcr(args->a1, &args->a2);
+		break;
+	default:
+		tee_entry_fast(args);
+		break;
+	}
+}
diff --git a/core/arch/arm/plat-zynq7k/plat_init.S b/core/arch/arm/plat-zynq7k/plat_init.S
new file mode 100644
index 0000000..6d99a30
--- /dev/null
+++ b/core/arch/arm/plat-zynq7k/plat_init.S
@@ -0,0 +1,111 @@
+/*
+ * Copyright (c) 2014, STMicroelectronics International N.V.
+ * All rights reserved.
+ * Copyright (c) 2016, Wind River Systems.
+ * 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.
+ */
+
+/*
+ * Entry points for the A9 inits, A9 revision specific or not.
+ * It is assume no stack is available when these routines are called.
+ * It is assume each routine is called with return address in LR
+ * and with ARM registers R0, R1, R2, R3 being scratchable.
+ */
+
+#include <arm32.h>
+#include <arm32_macros.S>
+#include <arm32_macros_cortex_a9.S>
+#include <asm.S>
+#include <kernel/tz_ssvce_def.h>
+#include <kernel/unwind.h>
+#include <platform_config.h>
+
+#define ZYNQ_SLCR_L2C_RAM	0xF8000A1C
+
+.section .text
+.balign 4
+.code 32
+
+/*
+ * Cortex A9 early configuration
+ *
+ * Use registers R0-R3.
+ * No stack usage.
+ * LR store return address.
+ * Trap CPU in case of error.
+ */
+FUNC plat_cpu_reset_early , :
+UNWIND(	.fnstart)
+
+	/*
+	 * Disallow NSec to mask FIQ [bit4: FW=0]
+	 * Allow NSec to manage Imprecise Abort [bit5: AW=1]
+	 * Imprecise Abort trapped to Abort Mode [bit3: EA=0]
+	 * In Sec world, FIQ trapped to FIQ Mode [bit2: FIQ=0]
+	 * IRQ always trapped to IRQ Mode [bit1: IRQ=0]
+	 * Secure World [bit0: NS=0]
+	 */
+	mov r0, #SCR_AW
+	write_scr r0		/* write Secure Configuration Register */
+
+	/*
+	 * Mandated HW config loaded
+	 *
+	 * SCTLR = 0x00004000
+	 * - Round-Robin replac. for icache, btac, i/duTLB (bit14: RoundRobin)
+	 *
+	 * ACTRL = 0x00000041
+	 * - core always in full SMP (FW bit0=1, SMP bit6=1)
+	 * - L2 write full line of zero disabled (bit3=0)
+	 *   (keep WFLZ low. Will be set once outer L2 is ready)
+	 *
+	 * NSACR = 0x00020C00
+	 * - NSec cannot change ACTRL.SMP (NS_SMP bit18=0)
+	 * - Nsec can lockdown TLB (TL bit17=1)
+	 * - NSec cannot access PLE (PLE bit16=0)
+	 * - NSec can use SIMD/VFP (CP10/CP11) (bit15:14=2b00, bit11:10=2b11)
+	 *
+	 * PCR = 0x00000001
+	 * - no change latency, enable clk gating
+	 */
+	movw r0, #0x4000
+	movt r0, #0x0000
+	write_sctlr r0
+
+	movw r0, #0x0041
+	movt r0, #0x0000
+	write_actlr r0
+
+	movw r0, #0x0FFF
+	movt r0, #0x0002
+	write_nsacr r0
+
+	movw r0, #0x0000
+	movt r0, #0x0001
+	write_pcr r0
+
+	mov pc, lr
+UNWIND(	.fnend)
+END_FUNC plat_cpu_reset_early
diff --git a/core/arch/arm/plat-zynq7k/platform_config.h b/core/arch/arm/plat-zynq7k/platform_config.h
new file mode 100644
index 0000000..c970048
--- /dev/null
+++ b/core/arch/arm/plat-zynq7k/platform_config.h
@@ -0,0 +1,276 @@
+/*
+ * Copyright (c) 2016, Wind River Systems.
+ * 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 PLATFORM_CONFIG_H
+#define PLATFORM_CONFIG_H
+
+#define STACK_ALIGNMENT			64
+
+/* For Zynq7000 board */
+
+#define SCU_BASE			0xF8F00000
+#define PL310_BASE			0xF8F02000
+#define GIC_BASE			0xF8F00000
+#define GICC_OFFSET			0x100
+#define GICD_OFFSET			0x1000
+#define GIC_CPU_BASE			(GIC_BASE + GICC_OFFSET)
+#define GIC_DIST_BASE			(GIC_BASE + GICD_OFFSET)
+
+#define SLCR_BASE			0xF8000000
+#define SLCR_LOCK			0xF8000004
+#define SLCR_UNLOCK			0xF8000008
+#define SLCR_TZ_DDR_RAM			0xF8000430
+#define SLCR_TZ_DMA_NS			0xF8000440
+#define SLCR_TZ_DMA_IRQ_NS		0xF8000444
+#define SLCR_TZ_DMA_PERIPH_NS		0xF8000448
+#define SLCR_TZ_GEM			0xF8000450
+#define SLCR_TZ_SDIO			0xF8000454
+#define SLCR_TZ_USB			0xF8000458
+#define SLCR_L2C_RAM			0xF8000A1C
+
+#define SLCR_LOCK_MAGIC			0x0000767B
+#define SLCR_UNLOCK_MAGIC		0x0000DF0D
+
+#define SECURITY2_SDIO0			0xE0200008
+#define SECURITY3_SDIO1			0xE020000C
+#define SECURITY4_QSPI			0xE0200010
+#define SECURITY6_APB_SLAVES		0xE0200018
+
+#define UART0_BASE			0xE0000000
+#define UART1_BASE			0xE0001000
+
+#define CONSOLE_UART_BASE		UART1_BASE
+
+#define DRAM0_BASE			0x00100000
+#define DRAM0_SIZE			0x3FF00000
+
+#define CFG_TEE_RAM_VA_SIZE		(1024 * 1024)
+
+#define CFG_TEE_CORE_NB_CORE		2
+
+#define DDR_PHYS_START			DRAM0_BASE
+#define DDR_SIZE			DRAM0_SIZE
+
+#define CFG_DDR_START			DDR_PHYS_START
+#define CFG_DDR_SIZE			DDR_SIZE
+
+/*
+ * PL310 TAG RAM Control Register
+ *
+ * bit[10:8]:1 - 2 cycle of write accesses latency
+ * bit[6:4]:1 - 2 cycle of read accesses latency
+ * bit[2:0]:1 - 2 cycle of setup latency
+ */
+#ifndef PL310_TAG_RAM_CTRL_INIT
+#define PL310_TAG_RAM_CTRL_INIT		0x00000111
+#endif
+
+/*
+ * PL310 DATA RAM Control Register
+ *
+ * bit[10:8]:2 - 3 cycle of write accesses latency
+ * bit[6:4]:2 - 3 cycle of read accesses latency
+ * bit[2:0]:2 - 3 cycle of setup latency
+ */
+#ifndef PL310_DATA_RAM_CTRL_INIT
+#define PL310_DATA_RAM_CTRL_INIT	0x00000222
+#endif
+
+/*
+ * PL310 Auxiliary Control Register
+ *
+ * I/Dcache prefetch enabled (bit29:28=2b11)
+ * NS can access interrupts (bit27=1)
+ * NS can lockown cache lines (bit26=1)
+ * Pseudo-random replacement policy (bit25=0)
+ * Force write allocated (default)
+ * Shared attribute internally ignored (bit22=1, bit13=0)
+ * Parity disabled (bit21=0)
+ * Event monitor disabled (bit20=0)
+ * Platform fmavor specific way config:
+ * - 64kb way size (bit19:17=3b011)
+ * - 8-way associciativity (bit16=0)
+ * Store buffer device limitation enabled (bit11=1)
+ * Cacheable accesses have high prio (bit10=0)
+ * Full Line Zero (FLZ) disabled (bit0=0)
+ */
+#ifndef PL310_AUX_CTRL_INIT
+#define PL310_AUX_CTRL_INIT		0x3C460800
+#endif
+
+/*
+ * PL310 Prefetch Control Register
+ *
+ * Double linefill disabled (bit30=0)
+ * I/D prefetch enabled (bit29:28=2b11)
+ * Prefetch drop enabled (bit24=1)
+ * Incr double linefill disable (bit23=0)
+ * Prefetch offset = 7 (bit4:0)
+ */
+#define PL310_PREFETCH_CTRL_INIT	0x31000007
+
+/*
+ * PL310 Power Register
+ *
+ * Dynamic clock gating enabled
+ * Standby mode enabled
+ */
+#define PL310_POWER_CTRL_INIT		0x00000003
+
+/*
+ * SCU Invalidate Register
+ *
+ * Invalidate all registers
+ */
+#define SCU_INV_CTRL_INIT		0xFFFFFFFF
+
+/*
+ * SCU Access Register
+ * - both secure CPU access SCU
+ */
+#define SCU_SAC_CTRL_INIT		0x0000000F
+
+/*
+ * SCU NonSecure Access Register
+ * - both nonsec cpu access SCU, private and global timer
+ */
+#define SCU_NSAC_CTRL_INIT		0x00000FFF
+
+/* all bit enabled in access control register */
+#define ACCESS_BITS_ALL			0xFFFFFFFF
+
+/* recommended value for setting the L2C_RAM register */
+#define SLCR_L2C_RAM_VALUE		0x00020202
+
+/* place in OCRAM to write secondary entry to */
+#define SECONDARY_ENTRY_DROP		0xFFFFFFF0
+
+/* define the memory areas */
+
+#ifdef CFG_WITH_PAGER
+
+/*
+ * TEE/TZ RAM layout:
+ *
+ *  +---------------------------------------+  <- CFG_CORE_TZSRAM_EMUL_START
+ *  | TEE private highly | TEE_RAM          |   ^
+ *  |   secure memory    |                  |   | CFG_CORE_TZSRAM_EMUL_SIZE
+ *  +---------------------------------------+   v
+ *
+ *  +---------------------------------------+  <- CFG_DDR_TEETZ_RESERVED_START
+ *  | TEE private secure |  TA_RAM          |   ^
+ *  |   external memory  |                  |   |
+ *  +---------------------------------------+   | CFG_DDR_TEETZ_RESERVED_SIZE
+ *  |     Non secure     |  SHM             |   |
+ *  |   shared memory    |                  |   |
+ *  +---------------------------------------+   v
+ *
+ *  TEE_RAM : default 256kByte
+ *  TA_RAM  : all what is left in DDR TEE reserved area
+ *  PUB_RAM : default 2MByte
+ */
+
+/* emulated SRAM, 256K at start of secure DDR */
+
+#define CFG_CORE_TZSRAM_EMUL_START	0x3E000000
+
+#define TZSRAM_BASE			CFG_CORE_TZSRAM_EMUL_START
+#define TZSRAM_SIZE			CFG_CORE_TZSRAM_EMUL_SIZE
+
+/* Location of trusted dram */
+
+#define CFG_DDR_TEETZ_RESERVED_START	0x3E100000
+#define CFG_DDR_TEETZ_RESERVED_SIZE	0x01F00000
+
+#define CFG_PUB_RAM_SIZE		(1 * 1024 * 1024)
+#define CFG_TEE_RAM_PH_SIZE		TZSRAM_SIZE
+
+#define TZDRAM_BASE			(CFG_DDR_TEETZ_RESERVED_START)
+#define TZDRAM_SIZE				(CFG_DDR_TEETZ_RESERVED_SIZE - \
+						CFG_PUB_RAM_SIZE)
+
+#define CFG_SHMEM_START			(CFG_DDR_TEETZ_RESERVED_START + \
+						TZDRAM_SIZE)
+#define CFG_SHMEM_SIZE			CFG_PUB_RAM_SIZE
+
+#define CFG_TA_RAM_START		TZDRAM_BASE
+#define CFG_TA_RAM_SIZE			TZDRAM_SIZE
+
+#define CFG_TEE_RAM_START		TZSRAM_BASE
+
+#ifndef CFG_TEE_LOAD_ADDR
+#define CFG_TEE_LOAD_ADDR		TZSRAM_BASE
+#endif
+
+#else /* CFG_WITH_PAGER */
+
+/*
+ * TEE/TZ RAM layout:
+ *
+ *  +---------------------------------------+  <- CFG_DDR_TEETZ_RESERVED_START
+ *  | TEE private secure |  TEE_RAM         |   ^
+ *  |   external memory  +------------------+   |
+ *  |                    |  TA_RAM          |   |
+ *  +---------------------------------------+   | CFG_DDR_TEETZ_RESERVED_SIZE
+ *  |     Non secure     |  SHM             |   |
+ *  |   shared memory    |                  |   |
+ *  +---------------------------------------+   v
+ *
+ *  TEE_RAM : 1MByte
+ *  PUB_RAM : 1MByte
+ *  TA_RAM  : all what is left (at least 2MByte !)
+ */
+
+#define CFG_DDR_TEETZ_RESERVED_START	0x3E000000
+#define CFG_DDR_TEETZ_RESERVED_SIZE	0x02000000
+
+#define CFG_PUB_RAM_SIZE		(1 * 1024 * 1024)
+#define CFG_TEE_RAM_PH_SIZE		(1 * 1024 * 1024)
+
+#define TZDRAM_BASE			(CFG_DDR_TEETZ_RESERVED_START)
+#define TZDRAM_SIZE			(CFG_DDR_TEETZ_RESERVED_SIZE - \
+						CFG_PUB_RAM_SIZE)
+
+#define CFG_TA_RAM_START		(CFG_DDR_TEETZ_RESERVED_START + \
+						CFG_TEE_RAM_PH_SIZE)
+#define CFG_TA_RAM_SIZE			(CFG_DDR_TEETZ_RESERVED_SIZE - \
+						CFG_TEE_RAM_PH_SIZE - \
+						CFG_PUB_RAM_SIZE)
+
+#define CFG_SHMEM_START			(CFG_DDR_TEETZ_RESERVED_START + \
+						TZDRAM_SIZE)
+#define CFG_SHMEM_SIZE			CFG_PUB_RAM_SIZE
+
+#define CFG_TEE_RAM_START		TZDRAM_BASE
+
+#ifndef CFG_TEE_LOAD_ADDR
+#define CFG_TEE_LOAD_ADDR		TZDRAM_BASE
+#endif
+
+#endif /* CFG_WITH_PAGER */
+
+#endif /*PLATFORM_CONFIG_H*/
diff --git a/core/arch/arm/plat-zynq7k/platform_smc.h b/core/arch/arm/plat-zynq7k/platform_smc.h
new file mode 100644
index 0000000..ac6bc33
--- /dev/null
+++ b/core/arch/arm/plat-zynq7k/platform_smc.h
@@ -0,0 +1,81 @@
+/*
+ * Copyright (c) 2016, Wind River System
+ * 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 PLATFORM_SMC_H
+#define PLATFORM_SMC_H
+
+#include <sm/optee_smc.h>
+
+/*
+ * Read SLCR (System Level Control Register)
+ *
+ * Call register usage:
+ * a0	SMC Function ID, ZYNQ7K_SMC_SLCR_READ
+ * a1	Register offset
+ * a2-7	Not used
+ *
+ * Normal return register usage:
+ * a0	OPTEE_SMC_RETURN_OK
+ * a1	Value read back
+ * a2-3	Not used
+ * a4-7	Preserved
+ *
+ * OPTEE_SMC_RETURN_EBADCMD on Invalid input offset:
+ * a0	OPTEE_SMC_RETURN_EBADCMD
+ * a1	Undefined value
+ * a2-3	Not used
+ * a4-7	Preserved
+ */
+#define ZYNQ7K_SMC_FUNCID_SLCR_READ  0x100
+#define ZYNQ7K_SMC_SLCR_READ \
+	OPTEE_SMC_CALL_VAL(OPTEE_SMC_32, OPTEE_SMC_FAST_CALL, \
+			   OPTEE_SMC_OWNER_OEM, ZYNQ7K_SMC_FUNCID_SLCR_READ)
+
+/*
+ * Write SLCR (System Level Control Register)
+ *
+ * Call register usage:
+ * a0	SMC Function ID, ZYNQ7K_SMC_SLCR_READ
+ * a1	Register offset
+ * a2	Value to write
+ * a3-7	Not used
+ *
+ * Normal return register usage:
+ * a0	OPTEE_SMC_RETURN_OK
+ * a1-3	Not used
+ * a4-7	Preserved
+ *
+ * OPTEE_SMC_RETURN_EBADCMD on Invalid input offset:
+ * a0	OPTEE_SMC_RETURN_EBADCMD
+ * a1-3	Not used
+ * a4-7	Preserved
+ */
+#define ZYNQ7K_SMC_FUNCID_SLCR_WRITE  0x101
+#define ZYNQ7K_SMC_SLCR_WRITE \
+	OPTEE_SMC_CALL_VAL(OPTEE_SMC_32, OPTEE_SMC_FAST_CALL, \
+			   OPTEE_SMC_OWNER_OEM, ZYNQ7K_SMC_FUNCID_SLCR_WRITE)
+
+#endif /* PLATFORM_SMC_H */
diff --git a/core/arch/arm/plat-zynq7k/sub.mk b/core/arch/arm/plat-zynq7k/sub.mk
new file mode 100644
index 0000000..652c084
--- /dev/null
+++ b/core/arch/arm/plat-zynq7k/sub.mk
@@ -0,0 +1,3 @@
+global-incdirs-y += .
+srcs-y += main.c
+srcs-y += plat_init.S
diff --git a/core/arch/arm/plat-zynqmp/conf.mk b/core/arch/arm/plat-zynqmp/conf.mk
new file mode 100644
index 0000000..67570bc
--- /dev/null
+++ b/core/arch/arm/plat-zynqmp/conf.mk
@@ -0,0 +1,29 @@
+PLATFORM_FLAVOR ?= zcu102
+
+# 32-bit flags
+arm32-platform-cpuarch		:= cortex-a53
+arm32-platform-cflags		+= -mcpu=$(arm32-platform-cpuarch)
+arm32-platform-aflags		+= -mcpu=$(arm32-platform-cpuarch)
+arm32-platform-aflags		+= -mfpu=neon
+
+$(call force,CFG_CDNS_UART,y)
+$(call force,CFG_GENERIC_BOOT,y)
+$(call force,CFG_GIC,y)
+$(call force,CFG_HWSUPP_MEM_PERM_PXN,y)
+$(call force,CFG_PM_STUBS,y)
+$(call force,CFG_SECURE_TIME_SOURCE_CNTPCT,y)
+$(call force,CFG_WITH_ARM_TRUSTED_FW,y)
+
+ta-targets = ta_arm32
+
+ifeq ($(CFG_ARM64_core),y)
+$(call force,CFG_WITH_LPAE,y)
+ta-targets += ta_arm64
+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/kern.ld.S b/core/arch/arm/plat-zynqmp/kern.ld.S
new file mode 100644
index 0000000..8d794ee
--- /dev/null
+++ b/core/arch/arm/plat-zynqmp/kern.ld.S
@@ -0,0 +1 @@
+#include "../kernel/kern.ld.S"
diff --git a/core/arch/arm/plat-zynqmp/link.mk b/core/arch/arm/plat-zynqmp/link.mk
new file mode 100644
index 0000000..448ab89
--- /dev/null
+++ b/core/arch/arm/plat-zynqmp/link.mk
@@ -0,0 +1 @@
+include core/arch/arm/kernel/link.mk
diff --git a/core/arch/arm/plat-zynqmp/main.c b/core/arch/arm/plat-zynqmp/main.c
new file mode 100644
index 0000000..31b8475
--- /dev/null
+++ b/core/arch/arm/plat-zynqmp/main.c
@@ -0,0 +1,122 @@
+/*
+ * Copyright (c) 2016, Xilinx Inc.
+ * 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 <stdint.h>
+#include <string.h>
+
+#include <drivers/gic.h>
+#include <drivers/cdns_uart.h>
+
+#include <arm.h>
+#include <console.h>
+#include <kernel/generic_boot.h>
+#include <kernel/pm_stubs.h>
+#include <kernel/misc.h>
+#include <kernel/tee_time.h>
+#include <mm/core_memprot.h>
+#include <tee/entry_fast.h>
+#include <tee/entry_std.h>
+#include <trace.h>
+
+static void main_fiq(void);
+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,
+#if defined(CFG_WITH_ARM_TRUSTED_FW)
+	.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,
+#else
+	.cpu_on = pm_panic,
+	.cpu_off = pm_panic,
+	.cpu_suspend = pm_panic,
+	.cpu_resume = pm_panic,
+	.system_off = pm_panic,
+	.system_reset = pm_panic,
+#endif
+};
+
+const struct thread_handlers *generic_boot_get_handlers(void)
+{
+	return &handlers;
+}
+
+void main_init_gic(void)
+{
+	vaddr_t gicc_base, gicd_base;
+
+	gicc_base = (vaddr_t)phys_to_virt(GIC_BASE + GICC_OFFSET,
+					  MEM_AREA_IO_SEC);
+	gicd_base = (vaddr_t)phys_to_virt(GIC_BASE + GICD_OFFSET,
+					  MEM_AREA_IO_SEC);
+	/* On ARMv8, GIC configuration is initialized in ARM-TF */
+	gic_init_base_addr(&gic_data, gicc_base, gicd_base);
+}
+
+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());
+}
diff --git a/core/arch/arm/plat-zynqmp/platform_config.h b/core/arch/arm/plat-zynqmp/platform_config.h
new file mode 100644
index 0000000..91c0f82
--- /dev/null
+++ b/core/arch/arm/plat-zynqmp/platform_config.h
@@ -0,0 +1,126 @@
+/*
+ * Copyright (c) 2016, Xilinx Inc.
+ * 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 PLATFORM_CONFIG_H
+#define PLATFORM_CONFIG_H
+
+/* Make stacks aligned to data cache line length */
+#define STACK_ALIGNMENT		64
+
+#ifdef CFG_WITH_PAGER
+#error "Pager not supported for zynqmp"
+#endif
+
+#if defined(PLATFORM_FLAVOR_zc1751_dc1) || \
+	defined(PLATFORM_FLAVOR_zc1751_dc2) || \
+	defined(PLATFORM_FLAVOR_zcu102)
+
+#define GIC_BASE		0xF9010000
+#define UART0_BASE		0xFF000000
+#define UART1_BASE		0xFF001000
+
+#define IT_UART0		53
+#define IT_UART1		54
+
+#define UART0_CLK_IN_HZ		100000000
+#define UART1_CLK_IN_HZ		100000000
+#define CONSOLE_UART_BASE	UART0_BASE
+#define IT_CONSOLE_UART		IT_UART0
+#define CONSOLE_UART_CLK_IN_HZ	UART0_CLK_IN_HZ
+
+#define DRAM0_BASE		0
+#define DRAM0_SIZE		0x80000000
+
+/* Location of trusted dram */
+#define TZDRAM_BASE		0x60000000
+#define TZDRAM_SIZE		0x10000000
+
+#define CFG_SHMEM_START		0x70000000
+#define CFG_SHMEM_SIZE		0x10000000
+
+#define GICD_OFFSET		0
+#define GICC_OFFSET		0x20000
+
+#else
+#error "Unknown platform flavor"
+#endif
+
+#define CFG_TEE_CORE_NB_CORE	4
+
+#define CFG_TEE_RAM_VA_SIZE	(1024 * 1024)
+
+#ifndef CFG_TEE_LOAD_ADDR
+#define CFG_TEE_LOAD_ADDR	CFG_TEE_RAM_START
+#endif
+
+/*
+ * Assumes that either TZSRAM isn't large enough or TZSRAM doesn't exist,
+ * everything is in TZDRAM.
+ * +------------------+
+ * |        | TEE_RAM |
+ * + TZDRAM +---------+
+ * |        | TA_RAM  |
+ * +--------+---------+
+ */
+#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		ROUNDDOWN((TZDRAM_SIZE - CFG_TEE_RAM_VA_SIZE), \
+					  CORE_MMU_DEVICE_SIZE)
+
+
+#define DEVICE0_PA_BASE		ROUNDDOWN(CONSOLE_UART_BASE, \
+					  CORE_MMU_DEVICE_SIZE)
+#define DEVICE0_VA_BASE		DEVICE0_PA_BASE
+#define DEVICE0_SIZE		CORE_MMU_DEVICE_SIZE
+#define DEVICE0_TYPE		MEM_AREA_IO_SEC
+
+#define DEVICE1_PA_BASE		ROUNDDOWN(GIC_BASE, CORE_MMU_DEVICE_SIZE)
+#define DEVICE1_VA_BASE		DEVICE1_PA_BASE
+#define DEVICE1_SIZE		CORE_MMU_DEVICE_SIZE
+#define DEVICE1_TYPE		MEM_AREA_IO_SEC
+
+#define DEVICE2_PA_BASE		ROUNDDOWN(GIC_BASE + GICD_OFFSET, \
+					  CORE_MMU_DEVICE_SIZE)
+#define DEVICE2_VA_BASE		DEVICE2_PA_BASE
+#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
+
+/* For virtual platforms where there isn't a clock */
+#ifndef CONSOLE_UART_CLK_IN_HZ
+#define CONSOLE_UART_CLK_IN_HZ	1
+#endif
+
+#endif /*PLATFORM_CONFIG_H*/
diff --git a/core/arch/arm/plat-zynqmp/sub.mk b/core/arch/arm/plat-zynqmp/sub.mk
new file mode 100644
index 0000000..8ddc2fd
--- /dev/null
+++ b/core/arch/arm/plat-zynqmp/sub.mk
@@ -0,0 +1,2 @@
+global-incdirs-y += .
+srcs-y += main.c
diff --git a/core/arch/arm/pta/core_self_tests.c b/core/arch/arm/pta/core_self_tests.c
new file mode 100644
index 0000000..639b8a6
--- /dev/null
+++ b/core/arch/arm/pta/core_self_tests.c
@@ -0,0 +1,252 @@
+/*
+ * 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 <malloc.h>
+#include <stdbool.h>
+#include <trace.h>
+#include "core_self_tests.h"
+
+/*
+ * Enable expect LOG macro to enable/disable self tests traces.
+ *
+ * #define LOG     DMSG_RAW
+ * #define LOG(...)
+ */
+#define LOG(...)
+
+static int self_test_division(void);
+static int self_test_malloc(void);
+
+/* exported entry points for some basic test */
+TEE_Result core_self_tests(uint32_t nParamTypes __unused,
+		TEE_Param pParams[TEE_NUM_PARAMS] __unused)
+{
+	if (self_test_division() || self_test_malloc()) {
+		EMSG("some self_test_xxx failed! you should enable local LOG");
+		return TEE_ERROR_GENERIC;
+	}
+	return TEE_SUCCESS;
+}
+
+/* test division support. resulting trace shall be manually checked */
+static int self_test_division(void)
+{
+	signed a, b, c, d;
+	bool r;
+	int ret = 0;
+
+	LOG("");
+	LOG("division tests (division and modulo):");
+	/* get some unpredicted values to prevent compilation optimizations: */
+	/* => use the stack address */
+
+	LOG("- test with unsigned small integers:");
+	a = (signed)((unsigned)(vaddr_t)&a & 0xFFFFF);
+	b = (signed)((unsigned)(vaddr_t)&b & 0x00FFF) + 1;
+	c = a / b;
+	d = a % b;
+	r = ((b * c + d) == a);
+	if (!r)
+		ret = -1;
+	LOG("  0x%08x / 0x%08x = %u / %u = %u = 0x%x)",
+	    (unsigned)a, (unsigned)b, (unsigned)a, (unsigned)b, (unsigned)c,
+	    (unsigned)c);
+	LOG("  0x%08x %% 0x%08x = %u %% %u = %u = 0x%x)", (unsigned)a,
+	    (unsigned)b, (unsigned)a, (unsigned)b, (unsigned)d, (unsigned)d);
+	LOG("  check results => %s", r ? "ok" : "FAILED !!!");
+	LOG("");
+
+	LOG("- test with signed small integers, negative numerator:");
+	a = (signed)(vaddr_t)&a;
+	b = (signed)((unsigned)(vaddr_t)&b & 0x00FFF) - 1;
+	c = a / b;
+	d = a % b;
+	r = ((b * c + d) == a);
+	if (!r)
+		ret = -1;
+	LOG("  0x%08x / 0x%08x = %d / %d = %d = 0x%x)",
+	    (unsigned)a, (unsigned)b, (signed)a, (signed)b, (signed)c,
+	    (unsigned)c);
+	LOG("  0x%08x %% 0x%08x = %d %% %d = %d = 0x%x)", (unsigned)a,
+	    (unsigned)b, (signed)a, (signed)b, (signed)d, (unsigned)d);
+	LOG("  check results => %s", r ? "ok" : "FAILED !!!");
+	LOG("");
+
+	LOG("- test with signed small integers, negative denominator:");
+	a = (signed)((unsigned)(vaddr_t)&a & 0xFFFFF);
+	b = -(signed)((unsigned)(vaddr_t)&b & 0x00FFF) + 1;
+	c = a / b;
+	d = a % b;
+
+	LOG("- test with unsigned integers, big numerator (> 0x80000000):");
+	a = (signed)(vaddr_t)&a;
+	b = (signed)((unsigned)(vaddr_t)&b & 0x00FFF) + 1;
+	c = (signed)((unsigned)a / (unsigned)b);
+	d = (signed)((unsigned)a % (unsigned)b);
+	r = (((unsigned)b * (unsigned)c + (unsigned)d) == (unsigned)a);
+	if (!r)
+		ret = -1;
+	LOG("  0x%08x / 0x%08x = %u / %u = %u = 0x%x)",
+	    (unsigned)a, (unsigned)b, (unsigned)a, (unsigned)b, (unsigned)c,
+	    (unsigned)c);
+	LOG("  0x%08x %% 0x%08x = %u %% %u = %u = 0x%x)", (unsigned)a,
+	    (unsigned)b, (unsigned)a, (unsigned)b, (unsigned)d, (unsigned)d);
+	LOG("  check results => %s", r ? "ok" : "FAILED !!!");
+	LOG("");
+
+	LOG("- test with unsigned integers, big num. & denom. (> 0x80000000):");
+	a = (signed)(vaddr_t)&a;
+	b = (signed)((unsigned)(vaddr_t)&a - 1);
+	c = (signed)((unsigned)a / (unsigned)b);
+	d = (signed)((unsigned)a % (unsigned)b);
+	r = (((unsigned)b * (unsigned)c + (unsigned)d) == (unsigned)a);
+	if (!r)
+		ret = -1;
+	LOG("  0x%08x / 0x%08x = %u / %u = %u = 0x%x)",
+	    (unsigned)a, (unsigned)b, (unsigned)a, (unsigned)b, (unsigned)c,
+	    (unsigned)c);
+	LOG("  0x%08x %% 0x%08x = %u %% %u = %u = 0x%x)", (unsigned)a,
+	    (unsigned)b, (unsigned)a, (unsigned)b, (unsigned)d, (unsigned)d);
+	LOG("  check results => %s", r ? "ok" : "FAILED !!!");
+	LOG("");
+
+	return ret;
+}
+
+/* test malloc support. resulting trace shall be manually checked */
+static int self_test_malloc(void)
+{
+	char *p1 = NULL, *p2 = NULL;
+	int *p3 = NULL, *p4 = NULL;
+	bool r;
+	int ret = 0;
+
+	LOG("malloc tests (malloc, free, calloc, realloc, memalign):");
+	LOG("  p1=%p  p2=%p  p3=%p  p4=%p",
+	    (void *)p1, (void *)p2, (void *)p3, (void *)p4);
+	/* test malloc */
+	p1 = malloc(1024);
+	LOG("- p1 = malloc(1024)");
+	p2 = malloc(1024);
+	LOG("- p2 = malloc(1024)");
+	LOG("  p1=%p  p2=%p  p3=%p  p4=%p",
+	    (void *)p1, (void *)p2, (void *)p3, (void *)p4);
+	r = (p1 && p2 && malloc_buffer_is_within_alloced(p1, 1024) &&
+		!malloc_buffer_is_within_alloced(p1 + 25, 1000) &&
+		!malloc_buffer_is_within_alloced(p1 - 25, 500) &&
+		malloc_buffer_overlaps_heap(p1 - 25, 500));
+	if (!r)
+		ret = -1;
+	LOG("  => test %s", r ? "ok" : "FAILED");
+	LOG("");
+
+	/* test realloc */
+	p1 = realloc(p1, 3 * 1024);
+	LOG("- p1 = realloc(p1, 3*1024)");
+	LOG("- free p2");
+	free(p2);
+	p2 = malloc(1024);
+	LOG("- p2 = malloc(1024)");
+	LOG("  p1=%p  p2=%p  p3=%p  p4=%p",
+	    (void *)p1, (void *)p2, (void *)p3, (void *)p4);
+	r = (p1 && p2);
+	if (!r)
+		ret = -1;
+	LOG("  => test %s", r ? "ok" : "FAILED");
+	LOG("");
+	LOG("- free p1, p2");
+	free(p1);
+	free(p2);
+	p1 = NULL;
+	p2 = NULL;
+
+	/* test calloc */
+	p3 = calloc(4, 1024);
+	p4 = calloc(0x100, 1024 * 1024);
+	LOG("- p3 = calloc(4, 1024)");
+	LOG("- p4 = calloc(0x100, 1024*1024)   too big: should fail!");
+	LOG("  p1=%p  p2=%p  p3=%p  p4=%p",
+	    (void *)p1, (void *)p2, (void *)p3, (void *)p4);
+	r = (p3 && !p4);
+	if (!r)
+		ret = -1;
+	LOG("  => test %s", r ? "ok" : "FAILED");
+	LOG("");
+	LOG("- free p3, p4");
+	free(p3);
+	free(p4);
+	p3 = NULL;
+	p4 = NULL;
+
+	/* test memalign */
+	p3 = memalign(0x1000, 1024);
+	LOG("- p3 = memalign(%d, 1024)", 0x1000);
+	p1 = malloc(1024);
+	LOG("- p1 = malloc(1024)");
+	p4 = memalign(0x100, 512);
+	LOG("- p4 = memalign(%d, 512)", 0x100);
+	LOG("  p1=%p  p2=%p  p3=%p  p4=%p",
+	    (void *)p1, (void *)p2, (void *)p3, (void *)p4);
+	r = (p1 && p3 && p4 &&
+	    !((vaddr_t)p3 % 0x1000) && !((vaddr_t)p4 % 0x100));
+	if (!r)
+		ret = -1;
+	LOG("  => test %s", r ? "ok" : "FAILED");
+	LOG("");
+	LOG("- free p1, p3, p4");
+	free(p1);
+	free(p3);
+	free(p4);
+	p1 = NULL;
+	p3 = NULL;
+	p4 = NULL;
+
+	/* test memalign with invalid alignments */
+	p3 = memalign(100, 1024);
+	LOG("- p3 = memalign(%d, 1024)", 100);
+	p4 = memalign(0, 1024);
+	LOG("- p4 = memalign(%d, 1024)", 0);
+	LOG("  p1=%p  p2=%p  p3=%p  p4=%p",
+	    (void *)p1, (void *)p2, (void *)p3, (void *)p4);
+	r = (!p3 && !p4);
+	if (!r)
+		ret = -1;
+	LOG("  => test %s", r ? "ok" : "FAILED");
+	LOG("");
+	LOG("- free p3, p4");
+	free(p3);
+	free(p4);
+	p3 = NULL;
+	p4 = NULL;
+
+	/* test free(NULL) */
+	LOG("- free NULL");
+	free(NULL);
+	LOG("");
+	LOG("malloc test done");
+
+	return ret;
+}
diff --git a/core/arch/arm/pta/core_self_tests.h b/core/arch/arm/pta/core_self_tests.h
new file mode 100644
index 0000000..ed98669
--- /dev/null
+++ b/core/arch/arm/pta/core_self_tests.h
@@ -0,0 +1,37 @@
+/*
+ * 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.
+ */
+#ifndef CORE_SELF_TESTS_H
+#define CORE_SELF_TESTS_H
+
+#include <tee_api_types.h>
+#include <tee_api_defines.h>
+
+/* basic run-time tests */
+TEE_Result core_self_tests(uint32_t nParamTypes,
+		TEE_Param pParams[TEE_NUM_PARAMS]);
+
+#endif /*CORE_SELF_TESTS_H*/
diff --git a/core/arch/arm/pta/gprof.c b/core/arch/arm/pta/gprof.c
new file mode 100644
index 0000000..20ff366
--- /dev/null
+++ b/core/arch/arm/pta/gprof.c
@@ -0,0 +1,221 @@
+/*
+ * Copyright (c) 2016, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <arm.h>
+#include <kernel/misc.h>
+#include <kernel/pseudo_ta.h>
+#include <kernel/user_ta.h>
+#include <kernel/thread.h>
+#include <mm/core_memprot.h>
+#include <mm/tee_mmu.h>
+#include <optee_msg_supplicant.h>
+#include <pta_gprof.h>
+#include <string.h>
+
+static TEE_Result gprof_send_rpc(TEE_UUID *uuid, void *buf, size_t len,
+				 uint32_t *id)
+{
+	struct optee_msg_param params[3];
+	TEE_Result res = TEE_ERROR_GENERIC;
+	uint64_t c = 0;
+	paddr_t pa;
+	char *va;
+
+	thread_rpc_alloc_payload(sizeof(*uuid) + len, &pa, &c);
+	if (!pa)
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	va = phys_to_virt(pa, MEM_AREA_NSEC_SHM);
+	if (!va)
+		goto exit;
+
+	memcpy(va, uuid, sizeof(*uuid));
+	memcpy(va + sizeof(*uuid), buf, len);
+
+	memset(params, 0, sizeof(params));
+	params[0].attr = OPTEE_MSG_ATTR_TYPE_VALUE_INOUT;
+	params[0].u.value.a = *id;
+
+	params[1].attr = OPTEE_MSG_ATTR_TYPE_TMEM_INPUT;
+	params[1].u.tmem.buf_ptr = pa;
+	params[1].u.tmem.size = sizeof(*uuid);
+	params[1].u.tmem.shm_ref = c;
+
+	params[2].attr = OPTEE_MSG_ATTR_TYPE_TMEM_INPUT;
+	params[2].u.tmem.buf_ptr = pa + sizeof(*uuid);
+	params[2].u.tmem.size = len;
+	params[2].u.tmem.shm_ref = c;
+
+	res = thread_rpc_cmd(OPTEE_MSG_RPC_CMD_GPROF, 3, params);
+	if (res != TEE_SUCCESS)
+		goto exit;
+
+	*id = (uint32_t)params[0].u.value.a;
+exit:
+	thread_rpc_free_payload(c);
+	return res;
+}
+
+static TEE_Result gprof_send(struct tee_ta_session *s,
+			     uint32_t param_types,
+			     TEE_Param params[TEE_NUM_PARAMS])
+{
+	uint32_t exp_pt = TEE_PARAM_TYPES(TEE_PARAM_TYPE_VALUE_INOUT,
+					  TEE_PARAM_TYPE_MEMREF_INPUT,
+					  TEE_PARAM_TYPE_NONE,
+					  TEE_PARAM_TYPE_NONE);
+
+	if (exp_pt != param_types)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	return gprof_send_rpc(&s->ctx->uuid, params[1].memref.buffer,
+			      params[1].memref.size, &params[0].value.a);
+}
+
+static TEE_Result gprof_start_pc_sampling(struct tee_ta_session *s,
+					  uint32_t param_types,
+					  TEE_Param params[TEE_NUM_PARAMS])
+{
+	uint32_t exp_pt = TEE_PARAM_TYPES(TEE_PARAM_TYPE_MEMREF_INOUT,
+					  TEE_PARAM_TYPE_VALUE_INPUT,
+					  TEE_PARAM_TYPE_NONE,
+					  TEE_PARAM_TYPE_NONE);
+	struct sample_buf *sbuf;
+	uint32_t offset;
+	uint32_t scale;
+	TEE_Result res;
+	uint32_t len;
+	uaddr_t buf;
+
+	if (exp_pt != param_types)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	buf = (uaddr_t)params[0].memref.buffer;
+	len = params[0].memref.size;
+	offset = params[1].value.a;
+	scale = params[1].value.b;
+
+	res = tee_mmu_check_access_rights(to_user_ta_ctx(s->ctx),
+					  TEE_MEMORY_ACCESS_WRITE |
+					  TEE_MEMORY_ACCESS_ANY_OWNER,
+					  buf, len);
+	if (res != TEE_SUCCESS)
+		return res;
+	sbuf = calloc(1, sizeof(*sbuf));
+	if (!sbuf)
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	sbuf->samples = (uint16_t *)buf;
+	sbuf->nsamples = len / sizeof(*sbuf->samples);
+	sbuf->offset = offset;
+	sbuf->scale = scale;
+	sbuf->freq = read_cntfrq();
+	sbuf->enabled = true;
+	s->sbuf = sbuf;
+
+	return TEE_SUCCESS;
+}
+
+static TEE_Result gprof_stop_pc_sampling(struct tee_ta_session *s,
+					 uint32_t param_types,
+					 TEE_Param params[TEE_NUM_PARAMS])
+{
+	uint32_t exp_pt = TEE_PARAM_TYPES(TEE_PARAM_TYPE_VALUE_OUTPUT,
+					  TEE_PARAM_TYPE_NONE,
+					  TEE_PARAM_TYPE_NONE,
+					  TEE_PARAM_TYPE_NONE);
+	struct sample_buf *sbuf;
+	uint32_t rate;
+
+	if (exp_pt != param_types)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	sbuf = s->sbuf;
+	if (!sbuf)
+		return TEE_ERROR_BAD_STATE;
+	assert(sbuf->samples);
+
+	/* Stop sampling */
+	if (sbuf->enabled)
+		sbuf->enabled = false;
+
+	rate = ((uint64_t)sbuf->count * sbuf->freq) / sbuf->usr;
+	params[0].value.a = rate;
+
+	DMSG("TA sampling stats: sample count=%" PRIu32 " user time=%" PRIu64
+	     " cntfrq=%" PRIu32 " rate=%" PRIu32, sbuf->count, sbuf->usr,
+	     sbuf->freq, rate);
+
+	free(sbuf);
+	s->sbuf = NULL;
+
+	return TEE_SUCCESS;
+}
+
+/*
+ * Trusted Application Entry Points
+ */
+
+static TEE_Result open_session(uint32_t param_types __unused,
+			       TEE_Param params[TEE_NUM_PARAMS] __unused,
+			       void **sess_ctx __unused)
+{
+	struct tee_ta_session *s;
+
+	/* Check that we're called from a user TA */
+	s = tee_ta_get_calling_session();
+	if (!s)
+		return TEE_ERROR_ACCESS_DENIED;
+	if (is_pseudo_ta_ctx(s->ctx))
+		return TEE_ERROR_ACCESS_DENIED;
+
+	return TEE_SUCCESS;
+}
+
+static TEE_Result invoke_command(void *sess_ctx __unused, uint32_t cmd_id,
+				 uint32_t param_types,
+				 TEE_Param params[TEE_NUM_PARAMS])
+{
+	struct tee_ta_session *s = tee_ta_get_calling_session();
+
+	switch (cmd_id) {
+	case PTA_GPROF_SEND:
+		return gprof_send(s, param_types, params);
+	case PTA_GPROF_START_PC_SAMPLING:
+		return gprof_start_pc_sampling(s, param_types, params);
+	case PTA_GPROF_STOP_PC_SAMPLING:
+		return gprof_stop_pc_sampling(s, param_types, params);
+	default:
+		break;
+	}
+	return TEE_ERROR_NOT_IMPLEMENTED;
+}
+
+pseudo_ta_register(.uuid = PTA_GPROF_UUID, .name = "gprof",
+		   .flags = PTA_DEFAULT_FLAGS,
+		   .open_session_entry_point = open_session,
+		   .invoke_command_entry_point = invoke_command);
diff --git a/core/arch/arm/pta/interrupt_tests.c b/core/arch/arm/pta/interrupt_tests.c
new file mode 100644
index 0000000..bc307a8
--- /dev/null
+++ b/core/arch/arm/pta/interrupt_tests.c
@@ -0,0 +1,239 @@
+/*
+ * Copyright (c) 2016, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+#include <keep.h>
+#include <kernel/interrupt.h>
+#include <kernel/misc.h>
+#include <kernel/pseudo_ta.h>
+#include <kernel/tee_time.h>
+#include <kernel/thread.h>
+#include <platform_config.h>
+#include <string.h>
+#include <tee/tee_cryp_provider.h>
+#include <trace.h>
+
+#define TA_NAME		"interrupt_tests.ta"
+
+#define INTERRUPT_TESTS_UUID \
+		{ 0x48d58475, 0x3d5e, 0x4202, \
+		{ 0xa7, 0x75, 0x97, 0x85, 0xd2, 0x0f, 0x78, 0xae } }
+
+#define CMD_INTERRUPT_TESTS	0
+
+#define SGI_NUM		16
+#define PPI_NUM		32
+
+#ifndef TEST_SGI_ID
+#define TEST_SGI_ID	11
+#endif
+#ifndef TEST_PPI_ID
+#define TEST_PPI_ID	29
+#endif
+#ifndef TEST_SPI_ID
+#define TEST_SPI_ID	61
+#endif
+#ifndef TEST_TIMES
+#define TEST_TIMES	3
+#endif
+
+/*
+ * Trusted Application Entry Points
+ */
+
+static size_t test_sgi_value[CFG_TEE_CORE_NB_CORE];
+static size_t test_spi_value[CFG_TEE_CORE_NB_CORE];
+static size_t test_ppi_value[CFG_TEE_CORE_NB_CORE];
+static size_t expect_sgi_value[CFG_TEE_CORE_NB_CORE];
+static size_t expect_spi_value[CFG_TEE_CORE_NB_CORE];
+static size_t expect_ppi_value[CFG_TEE_CORE_NB_CORE];
+
+static enum itr_return __maybe_unused ihandler_ok(struct itr_handler *handler)
+{
+	size_t core_num = get_core_pos();
+
+	assert(core_num < CFG_TEE_CORE_NB_CORE);
+
+	if (handler->it < SGI_NUM)
+		test_sgi_value[core_num]++;
+	else if (handler->it < PPI_NUM)
+		test_ppi_value[core_num]++;
+	else
+		test_spi_value[core_num]++;
+
+	return ITRR_HANDLED;
+}
+KEEP_PAGER(ihandler_ok);
+
+struct itr_handler sgi_handler = {
+	.it = TEST_SGI_ID,
+	.handler = ihandler_ok,
+};
+
+struct itr_handler spi_handler = {
+	.it = TEST_SPI_ID,
+	.handler = ihandler_ok,
+};
+
+struct itr_handler ppi_handler = {
+	.it = TEST_PPI_ID,
+	.handler = ihandler_ok,
+};
+
+static TEE_Result test_sgi(void)
+{
+	TEE_Result res;
+	uint8_t i;
+	uint8_t j;
+	uint8_t num;
+	uint8_t cpu_mask;
+
+	itr_add(&sgi_handler);
+	itr_enable(TEST_SGI_ID);
+
+	for (i = 0; i < CFG_TEE_CORE_NB_CORE; i++)
+		expect_sgi_value[i]++;
+	itr_raise_sgi(TEST_SGI_ID,
+		     (uint8_t)(SHIFT_U32(1, CFG_TEE_CORE_NB_CORE) - 1));
+	tee_time_wait(200);
+	if (memcmp(test_sgi_value, expect_sgi_value, sizeof(test_sgi_value)))
+		return TEE_ERROR_GENERIC;
+
+	for (i = 0; i < TEST_TIMES; i++) {
+		res = crypto_ops.prng.read(&num, 1);
+		if (res != TEE_SUCCESS)
+			return TEE_ERROR_GENERIC;
+		num = num % CFG_TEE_CORE_NB_CORE;
+		cpu_mask = 0x0;
+		for (j = 0; j < num; j++) {
+			expect_sgi_value[j]++;
+			cpu_mask |= (0x1 << j);
+		}
+		itr_raise_sgi(TEST_SGI_ID, cpu_mask);
+		tee_time_wait(200);
+		if (memcmp(test_sgi_value, expect_sgi_value,
+		    sizeof(test_sgi_value)))
+			return TEE_ERROR_GENERIC;
+	}
+
+	return TEE_SUCCESS;
+}
+
+static TEE_Result test_spi(void)
+{
+	TEE_Result res;
+	uint8_t i;
+	uint8_t num;
+
+	itr_add(&spi_handler);
+	itr_enable(TEST_SPI_ID);
+
+	for (i = 0; i < TEST_TIMES; i++) {
+		res = crypto_ops.prng.read(&num, 1);
+		if (res != TEE_SUCCESS)
+			return TEE_ERROR_GENERIC;
+		num = num % CFG_TEE_CORE_NB_CORE;
+		expect_spi_value[num]++;
+		itr_set_affinity(TEST_SPI_ID, 0x1 << num);
+		itr_raise_pi(TEST_SPI_ID);
+		tee_time_wait(200);
+		if (memcmp(test_spi_value, expect_spi_value,
+		    sizeof(test_spi_value)))
+			return TEE_ERROR_GENERIC;
+	}
+
+	return TEE_SUCCESS;
+}
+
+static TEE_Result test_ppi(void)
+{
+	uint32_t exceptions;
+
+	itr_add(&ppi_handler);
+	itr_enable(TEST_PPI_ID);
+
+	exceptions = thread_mask_exceptions(THREAD_EXCP_IRQ);
+	expect_ppi_value[get_core_pos()]++;
+	itr_raise_pi(TEST_PPI_ID);
+	thread_unmask_exceptions(exceptions);
+	tee_time_wait(200);
+	if (memcmp(test_ppi_value, expect_ppi_value, sizeof(test_ppi_value)))
+		return TEE_ERROR_GENERIC;
+
+	return TEE_SUCCESS;
+}
+
+static TEE_Result interrupt_tests(uint32_t nParamTypes __unused,
+			TEE_Param pParams[TEE_NUM_PARAMS]__unused)
+{
+	TEE_Result res;
+
+	assert(crypto_ops.prng.read);
+
+	res = test_sgi();
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = test_spi();
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = test_ppi();
+	if (res != TEE_SUCCESS)
+		return res;
+
+	return TEE_SUCCESS;
+}
+
+static TEE_Result invoke_command(void *psess __unused,
+				 uint32_t cmd, uint32_t ptypes,
+				 TEE_Param params[4])
+{
+	TEE_Result res;
+	uint8_t i;
+
+	switch (cmd) {
+	case CMD_INTERRUPT_TESTS:
+		res = interrupt_tests(ptypes, params);
+		DMSG("test value: sgi spi ppi");
+		for (i = 0; i < CFG_TEE_CORE_NB_CORE; i++)
+			DMSG("------------[%zu] [%zu] [%zu]",
+			    test_sgi_value[i], test_spi_value[i],
+			    test_ppi_value[i]);
+		DMSG("expc value: sgi spi ppi");
+		for (i = 0; i < CFG_TEE_CORE_NB_CORE; i++)
+			DMSG("------------[%zu] [%zu] [%zu]",
+			    expect_sgi_value[i], expect_spi_value[i],
+			    expect_ppi_value[i]);
+		return res;
+	default:
+		break;
+	}
+	return TEE_ERROR_BAD_PARAMETERS;
+}
+
+pseudo_ta_register(.uuid = INTERRUPT_TESTS_UUID, .name = TA_NAME,
+		   .flags = PTA_DEFAULT_FLAGS,
+		   .invoke_command_entry_point = invoke_command);
diff --git a/core/arch/arm/pta/pta_self_tests.c b/core/arch/arm/pta/pta_self_tests.c
new file mode 100644
index 0000000..6472356
--- /dev/null
+++ b/core/arch/arm/pta/pta_self_tests.c
@@ -0,0 +1,251 @@
+/*
+ * 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 <compiler.h>
+#include <types_ext.h>
+#include <kernel/pseudo_ta.h>
+#include <trace.h>
+#include <tee_api_types.h>
+#include <tee_api_defines.h>
+#include "core_self_tests.h"
+
+#define TA_NAME		"sta_self_tests.ta"
+
+#define STA_SELF_TEST_UUID \
+		{ 0xd96a5b40, 0xc3e5, 0x21e3, \
+			{ 0x87, 0x94, 0x10, 0x02, 0xa5, 0xd5, 0xc6, 0x1b } }
+
+#define CMD_TRACE	0
+#define CMD_PARAMS	1
+#define CMD_SELF_TESTS	2
+
+static TEE_Result test_trace(uint32_t param_types __unused,
+			TEE_Param params[TEE_NUM_PARAMS] __unused)
+{
+	IMSG("pseudo TA \"%s\" says \"Hello world !\"", TA_NAME);
+
+	return TEE_SUCCESS;
+}
+
+/*
+ * Supported tests on parameters
+ * (I, J, K, L refer to param index)
+ *
+ * Case 1: command parameters type are: 1 in/out value, 3 empty.
+ *         => process outI.a = inI.a + inI.b
+ * Case 2: command parameters type are: 3 input value, 1 output value
+ *         => process = outI.a = inJ.a + inK.a + inL.a
+ * Case 3: command parameters type are: 1 in/out memref, 3 empty.
+ *         => process = outI[0] = sum(inI[0..len-1])
+ */
+static TEE_Result test_entry_params(uint32_t type, TEE_Param p[TEE_NUM_PARAMS])
+{
+	size_t i;
+	uint8_t d8, *in;
+
+	/* case 1a: 1 input/output value argument */
+	if ((TEE_PARAM_TYPE_GET(type, 0) == TEE_PARAM_TYPE_VALUE_INOUT) &&
+		(TEE_PARAM_TYPE_GET(type, 1) == TEE_PARAM_TYPE_NONE) &&
+		(TEE_PARAM_TYPE_GET(type, 2) == TEE_PARAM_TYPE_NONE) &&
+		(TEE_PARAM_TYPE_GET(type, 3) == TEE_PARAM_TYPE_NONE)) {
+		p[0].value.a = p[0].value.a + p[0].value.b;
+		return TEE_SUCCESS;
+	}
+	/* case 1b: 1 input/output value argument */
+	if ((TEE_PARAM_TYPE_GET(type, 0) == TEE_PARAM_TYPE_NONE) &&
+		(TEE_PARAM_TYPE_GET(type, 1) == TEE_PARAM_TYPE_VALUE_INOUT) &&
+		(TEE_PARAM_TYPE_GET(type, 2) == TEE_PARAM_TYPE_NONE) &&
+		(TEE_PARAM_TYPE_GET(type, 3) == TEE_PARAM_TYPE_NONE)) {
+		p[1].value.a = p[1].value.a + p[1].value.b;
+		return TEE_SUCCESS;
+	}
+	/* case 1c: 1 input/output value argument */
+	if ((TEE_PARAM_TYPE_GET(type, 0) == TEE_PARAM_TYPE_NONE) &&
+		(TEE_PARAM_TYPE_GET(type, 1) == TEE_PARAM_TYPE_NONE) &&
+		(TEE_PARAM_TYPE_GET(type, 2) == TEE_PARAM_TYPE_VALUE_INOUT) &&
+		(TEE_PARAM_TYPE_GET(type, 3) == TEE_PARAM_TYPE_NONE)) {
+		p[2].value.a = p[2].value.a + p[2].value.b;
+		return TEE_SUCCESS;
+	}
+	/* case 1d: 1 input/output value argument */
+	if ((TEE_PARAM_TYPE_GET(type, 0) == TEE_PARAM_TYPE_NONE) &&
+		(TEE_PARAM_TYPE_GET(type, 1) == TEE_PARAM_TYPE_NONE) &&
+		(TEE_PARAM_TYPE_GET(type, 2) == TEE_PARAM_TYPE_NONE) &&
+		(TEE_PARAM_TYPE_GET(type, 3) == TEE_PARAM_TYPE_VALUE_INOUT)) {
+		p[3].value.a = p[3].value.a + p[3].value.b;
+		return TEE_SUCCESS;
+	}
+
+	/* case 2a: 3 input value arguments, 1 output value argument */
+	if ((TEE_PARAM_TYPE_GET(type, 0) == TEE_PARAM_TYPE_VALUE_OUTPUT) &&
+		(TEE_PARAM_TYPE_GET(type, 1) == TEE_PARAM_TYPE_VALUE_INPUT) &&
+		(TEE_PARAM_TYPE_GET(type, 2) == TEE_PARAM_TYPE_VALUE_INPUT) &&
+		(TEE_PARAM_TYPE_GET(type, 3) == TEE_PARAM_TYPE_VALUE_INPUT)) {
+		p[0].value.a = p[1].value.a + p[2].value.a + p[3].value.a;
+		p[0].value.b = p[1].value.b + p[2].value.b + p[3].value.b;
+		return TEE_SUCCESS;
+	}
+	/* case 2a: 3 input value arguments, 1 output value argument */
+	if ((TEE_PARAM_TYPE_GET(type, 0) == TEE_PARAM_TYPE_VALUE_INPUT) &&
+		(TEE_PARAM_TYPE_GET(type, 1) == TEE_PARAM_TYPE_VALUE_OUTPUT) &&
+		(TEE_PARAM_TYPE_GET(type, 2) == TEE_PARAM_TYPE_VALUE_INPUT) &&
+		(TEE_PARAM_TYPE_GET(type, 3) == TEE_PARAM_TYPE_VALUE_INPUT)) {
+		p[1].value.a = p[0].value.a + p[2].value.a + p[3].value.a;
+		p[1].value.b = p[0].value.b + p[2].value.b + p[3].value.b;
+		return TEE_SUCCESS;
+	}
+	/* case 2a: 3 input value arguments, 1 output value argument */
+	if ((TEE_PARAM_TYPE_GET(type, 0) == TEE_PARAM_TYPE_VALUE_INPUT) &&
+		(TEE_PARAM_TYPE_GET(type, 1) == TEE_PARAM_TYPE_VALUE_INPUT) &&
+		(TEE_PARAM_TYPE_GET(type, 2) == TEE_PARAM_TYPE_VALUE_OUTPUT) &&
+		(TEE_PARAM_TYPE_GET(type, 3) == TEE_PARAM_TYPE_VALUE_INPUT)) {
+		p[2].value.a = p[0].value.a + p[1].value.a + p[3].value.a;
+		p[2].value.b = p[0].value.b + p[1].value.b + p[3].value.b;
+		return TEE_SUCCESS;
+	}
+	/* case 2a: 3 input value arguments, 1 output value argument */
+	if ((TEE_PARAM_TYPE_GET(type, 0) == TEE_PARAM_TYPE_VALUE_INPUT) &&
+		(TEE_PARAM_TYPE_GET(type, 1) == TEE_PARAM_TYPE_VALUE_INPUT) &&
+		(TEE_PARAM_TYPE_GET(type, 2) == TEE_PARAM_TYPE_VALUE_INPUT) &&
+		(TEE_PARAM_TYPE_GET(type, 3) == TEE_PARAM_TYPE_VALUE_OUTPUT)) {
+		p[3].value.a = p[0].value.a + p[1].value.a + p[2].value.a;
+		p[3].value.b = p[0].value.b + p[1].value.b + p[2].value.b;
+		return TEE_SUCCESS;
+	}
+
+	DMSG("expect memref params: %p/%" PRIu32 " - %p/%" PRIu32 "zu - %p/%" PRIu32 "zu - %p/%" PRIu32 "zu",
+			p[0].memref.buffer, p[0].memref.size,
+			p[1].memref.buffer, p[1].memref.size,
+			p[2].memref.buffer, p[2].memref.size,
+			p[3].memref.buffer, p[3].memref.size);
+
+	/* case 3a: 1 in/out memref argument */
+	if ((TEE_PARAM_TYPE_GET(type, 0) == TEE_PARAM_TYPE_MEMREF_INOUT) &&
+		(TEE_PARAM_TYPE_GET(type, 1) == TEE_PARAM_TYPE_NONE) &&
+		(TEE_PARAM_TYPE_GET(type, 2) == TEE_PARAM_TYPE_NONE) &&
+		(TEE_PARAM_TYPE_GET(type, 3) == TEE_PARAM_TYPE_NONE)) {
+		in = (uint8_t *)p[0].memref.buffer;
+		d8 = 0;
+		for (i = 0; i < p[0].memref.size; i++)
+			d8 += in[i];
+		*(uint8_t *)p[0].memref.buffer = d8;
+		return TEE_SUCCESS;
+	}
+	/* case 3b: 1 in/out memref argument */
+	if ((TEE_PARAM_TYPE_GET(type, 0) == TEE_PARAM_TYPE_NONE) &&
+		(TEE_PARAM_TYPE_GET(type, 1) == TEE_PARAM_TYPE_MEMREF_INOUT) &&
+		(TEE_PARAM_TYPE_GET(type, 2) == TEE_PARAM_TYPE_NONE) &&
+		(TEE_PARAM_TYPE_GET(type, 3) == TEE_PARAM_TYPE_NONE)) {
+		in = (uint8_t *)p[1].memref.buffer;
+		d8 = 0;
+		for (i = 0; i < p[1].memref.size; i++)
+			d8 += in[i];
+		*(uint8_t *)p[1].memref.buffer = d8;
+		return TEE_SUCCESS;
+	}
+	/* case 3c: 1 in/out memref argument */
+	if ((TEE_PARAM_TYPE_GET(type, 0) == TEE_PARAM_TYPE_NONE) &&
+		(TEE_PARAM_TYPE_GET(type, 1) == TEE_PARAM_TYPE_NONE) &&
+		(TEE_PARAM_TYPE_GET(type, 2) == TEE_PARAM_TYPE_MEMREF_INOUT) &&
+		(TEE_PARAM_TYPE_GET(type, 3) == TEE_PARAM_TYPE_NONE)) {
+		in = (uint8_t *)p[2].memref.buffer;
+		d8 = 0;
+		for (i = 0; i < p[2].memref.size; i++)
+			d8 += in[i];
+		*(uint8_t *)p[2].memref.buffer = d8;
+		return TEE_SUCCESS;
+	}
+	/* case 3d: 1 in/out memref argument */
+	if ((TEE_PARAM_TYPE_GET(type, 0) == TEE_PARAM_TYPE_NONE) &&
+		(TEE_PARAM_TYPE_GET(type, 1) == TEE_PARAM_TYPE_NONE) &&
+		(TEE_PARAM_TYPE_GET(type, 2) == TEE_PARAM_TYPE_NONE) &&
+		(TEE_PARAM_TYPE_GET(type, 3) == TEE_PARAM_TYPE_MEMREF_INOUT)) {
+		in = (uint8_t *)p[3].memref.buffer;
+		d8 = 0;
+		for (i = 0; i < p[3].memref.size; i++)
+			d8 += in[i];
+		*(uint8_t *)p[3].memref.buffer = d8;
+		return TEE_SUCCESS;
+	}
+
+	EMSG("unexpected parameters");
+	return TEE_ERROR_BAD_PARAMETERS;
+}
+
+/*
+ * Trusted Application Entry Points
+ */
+
+static TEE_Result create_ta(void)
+{
+	DMSG("create entry point for pseudo TA \"%s\"", TA_NAME);
+	return TEE_SUCCESS;
+}
+
+static void destroy_ta(void)
+{
+	DMSG("destroy entry point for pseudo ta \"%s\"", TA_NAME);
+}
+
+static TEE_Result open_session(uint32_t nParamTypes __unused,
+		TEE_Param pParams[TEE_NUM_PARAMS] __unused,
+		void **ppSessionContext __unused)
+{
+	DMSG("open entry point for pseudo ta \"%s\"", TA_NAME);
+	return TEE_SUCCESS;
+}
+
+static void close_session(void *pSessionContext __unused)
+{
+	DMSG("close entry point for pseudo ta \"%s\"", TA_NAME);
+}
+
+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 pseudo ta \"%s\"", TA_NAME);
+
+	switch (nCommandID) {
+	case CMD_TRACE:
+		return test_trace(nParamTypes, pParams);
+	case CMD_PARAMS:
+		return test_entry_params(nParamTypes, pParams);
+	case CMD_SELF_TESTS:
+		return core_self_tests(nParamTypes, pParams);
+	default:
+		break;
+	}
+	return TEE_ERROR_BAD_PARAMETERS;
+}
+
+pseudo_ta_register(.uuid = STA_SELF_TEST_UUID, .name = TA_NAME,
+		   .flags = PTA_DEFAULT_FLAGS,
+		   .create_entry_point = create_ta,
+		   .destroy_entry_point = destroy_ta,
+		   .open_session_entry_point = open_session,
+		   .close_session_entry_point = close_session,
+		   .invoke_command_entry_point = invoke_command);
diff --git a/core/arch/arm/pta/se_api_self_tests.c b/core/arch/arm/pta/se_api_self_tests.c
new file mode 100644
index 0000000..f6f5ec4
--- /dev/null
+++ b/core/arch/arm/pta/se_api_self_tests.c
@@ -0,0 +1,498 @@
+/*
+ * Copyright (c) 2014, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <compiler.h>
+#include <kernel/pseudo_ta.h>
+#include <tee_api_types.h>
+#include <tee_api_defines.h>
+#include <trace.h>
+
+#include <tee/se/manager.h>
+#include <tee/se/reader.h>
+#include <tee/se/session.h>
+#include <tee/se/iso7816.h>
+#include <tee/se/aid.h>
+#include <tee/se/apdu.h>
+#include <tee/se/channel.h>
+#include <tee/se/util.h>
+
+#include <stdlib.h>
+#include <string.h>
+
+#include "aid_priv.h"
+#include "apdu_priv.h"
+#include "reader_priv.h"
+
+
+#define TA_NAME		"se_api_self_tests.ta"
+
+#define MAX_READERS	10
+
+#define CMD_SELF_TESTS	0
+
+#define SE_API_SELF_TEST_UUID \
+		{ 0xAEB79790, 0x6F03, 0x11E4,  \
+			{ 0x98, 0x03, 0x08, 0x00, 0x20, 0x0C, 0x9A, 0x66 } }
+
+#define ASSERT(expr) \
+	do { \
+		if (!(expr)) { \
+			EMSG("assertion '%s' failed at %s:%d (func '%s')", \
+				#expr, __FILE__, __LINE__, __func__); \
+			return TEE_ERROR_GENERIC; \
+		} \
+	} while (0)
+
+#define CHECK(ret) \
+	do { \
+		if (ret != TEE_SUCCESS) \
+			return ret; \
+	} while (0)
+
+/*
+ * Trusted Application Entry Points
+ */
+
+static TEE_Result test_reader(struct tee_se_reader_proxy **handle)
+{
+	TEE_Result ret;
+	uint8_t cmd[] = { ISO7816_CLA, MANAGE_CHANNEL_CMD,
+		OPEN_CHANNEL, OPEN_NEXT_AVAILABLE };
+	uint8_t resp[3];
+	size_t resp_size = sizeof(resp);
+	const int expected_channel_id = 1;
+
+	DMSG("entry");
+	/* transmit should fail since no one attached to the reader */
+	ret = tee_se_reader_transmit(handle[0], cmd, sizeof(cmd),
+			resp, &resp_size);
+	ASSERT(ret == TEE_ERROR_BAD_STATE);
+
+	ret = tee_se_reader_attach(handle[0]);
+	ASSERT(ret == TEE_SUCCESS);
+
+	ret = tee_se_reader_attach(handle[0]);
+	ASSERT(ret == TEE_SUCCESS);
+
+	/* referenced by 2 owners */
+	ASSERT(2 == tee_se_reader_get_refcnt(handle[0]));
+
+	ret = tee_se_reader_transmit(handle[0], cmd, sizeof(cmd),
+				resp, &resp_size);
+	ASSERT(ret == TEE_SUCCESS);
+	ASSERT(resp[0] == expected_channel_id &&
+		resp[1] == CMD_OK_SW1 && resp[2] == CMD_OK_SW2);
+
+	tee_se_reader_detach(handle[0]);
+
+	ASSERT(1 == tee_se_reader_get_refcnt(handle[0]));
+
+	tee_se_reader_detach(handle[0]);
+	DMSG("exit");
+
+	return TEE_SUCCESS;
+}
+
+static TEE_Result test_aid(struct tee_se_reader_proxy **proxies)
+{
+	struct tee_se_session *s = NULL;
+	struct tee_se_channel *b = NULL, *l = NULL;
+	struct tee_se_aid *aid = NULL;
+	TEE_Result ret;
+
+	DMSG("entry");
+	ret = tee_se_aid_create("D0000CAFE00001", &aid);
+	ASSERT(ret == TEE_SUCCESS);
+
+	ret = tee_se_reader_open_session(proxies[0], &s);
+	ASSERT(ret == TEE_SUCCESS);
+
+	ret = tee_se_session_open_basic_channel(s, aid, &b);
+	ASSERT(ret == TEE_SUCCESS);
+
+	ret = tee_se_session_open_logical_channel(s, aid, &l);
+	ASSERT(ret == TEE_SUCCESS);
+
+	ASSERT(tee_se_aid_get_refcnt(aid) == 3);
+
+	tee_se_session_close_channel(s, b);
+	tee_se_session_close_channel(s, l);
+
+	ASSERT(tee_se_aid_get_refcnt(aid) == 1);
+
+	tee_se_session_close(s);
+	tee_se_aid_release(aid);
+	DMSG("exit");
+
+	return TEE_SUCCESS;
+}
+
+static TEE_Result test_session(struct tee_se_reader_proxy **proxies)
+{
+	struct tee_se_channel *c1 = NULL, *c2 = NULL;
+	struct tee_se_session *s1 = NULL, *s2 = NULL;
+	TEE_Result ret;
+
+	DMSG("entry");
+	ret = tee_se_reader_open_session(proxies[0], &s1);
+	ASSERT(ret == TEE_SUCCESS);
+
+	/* should success, multiple sessions open by different user */
+	ret = tee_se_reader_open_session(proxies[0], &s2);
+	ASSERT(ret == TEE_SUCCESS);
+
+	/* open basic channel on s1 (should success) */
+	ret = tee_se_session_open_basic_channel(s1, NULL, &c1);
+	ASSERT(ret == TEE_SUCCESS);
+
+	/* open basic channel on s2
+	 * (should fail, basic channel is locked by s1)
+	 */
+	ret = tee_se_session_open_basic_channel(s2, NULL, &c2);
+	ASSERT(ret == TEE_ERROR_NOT_SUPPORTED);
+	ASSERT(c2 == NULL);
+
+	/* close basic channel on s1 */
+	tee_se_session_close_channel(s1, c1);
+	c1 = NULL;
+
+	/* open basic channel on s2 (this time should success) */
+	ret = tee_se_session_open_basic_channel(s1, NULL, &c2);
+	ASSERT(ret == TEE_SUCCESS);
+
+	/* close basic channel on s2 */
+	tee_se_session_close_channel(s2, c2);
+	c2 = NULL;
+
+	/* open logical channel on s1 and s2 (both should success) */
+	ret = tee_se_session_open_logical_channel(s1, NULL, &c1);
+	ASSERT(ret == TEE_SUCCESS);
+	ret = tee_se_session_open_logical_channel(s2, NULL, &c2);
+	ASSERT(ret == TEE_SUCCESS);
+
+	/* clean up */
+	tee_se_session_close_channel(s1, c1);
+	tee_se_session_close_channel(s2, c2);
+
+	tee_se_session_close(s1);
+	tee_se_session_close(s2);
+	DMSG("exit");
+
+	return TEE_SUCCESS;
+}
+
+static TEE_Result test_select_resp(struct tee_se_reader_proxy **proxies)
+{
+	struct tee_se_aid *aid = NULL;
+	struct tee_se_session *s = NULL;
+	struct tee_se_channel *c = NULL;
+	struct resp_apdu *resp;
+	TEE_Result ret;
+
+	DMSG("entry");
+	ret = tee_se_aid_create("D0000CAFE00001", &aid);
+	ASSERT(ret == TEE_SUCCESS);
+
+	ret = tee_se_reader_open_session(proxies[0], &s);
+	ASSERT(ret == TEE_SUCCESS);
+
+	ret = tee_se_session_open_logical_channel(s, aid, &c);
+	ASSERT(ret == TEE_SUCCESS);
+
+	ret = tee_se_channel_get_select_response(c, &resp);
+	ASSERT(ret == TEE_SUCCESS);
+
+	ASSERT((resp_apdu_get_sw1(resp) == CMD_OK_SW1) &&
+			(resp_apdu_get_sw2(resp) == CMD_OK_SW2));
+
+	/*
+	 * the ownership of resp apdu should be the channel
+	 * and it should be the only owner
+	 */
+	ASSERT(apdu_get_refcnt(to_apdu_base(resp)) == 1);
+
+	/* increase the reference counter of resp apdu */
+	apdu_acquire(to_apdu_base(resp));
+
+	/* clean up */
+	tee_se_session_close_channel(s, c);
+
+	/* channel should release resp apdu when closed */
+	ASSERT(apdu_get_refcnt(to_apdu_base(resp)) == 1);
+	apdu_release(to_apdu_base(resp));
+
+	tee_se_session_close(s);
+	tee_se_aid_release(aid);
+	DMSG("exit");
+
+	return TEE_SUCCESS;
+}
+
+/*
+ * The JAVA Card Simulator (jcardsim.jar) built-in applet(s):
+ *
+ * AID					|Type
+ * -------------------------------------+----------------------
+ * D0000CAFE00001			| MultiSelectable
+ * (default selected on basic channel)	|
+ * -------------------------------------+----------------------
+ * D0000CAFE00002			| Non-MultiSelectable
+ * -------------------------------------+----------------------
+ *
+ */
+static TEE_Result test_logical_channel(struct tee_se_reader_proxy **proxies)
+{
+	struct tee_se_channel *channel[MAX_LOGICAL_CHANNEL] = { NULL };
+	struct tee_se_aid *aid = NULL;
+	struct tee_se_session *s = NULL;
+	TEE_Result ret;
+	int i;
+
+	DMSG("entry");
+	ret = tee_se_reader_open_session(proxies[0], &s);
+	ASSERT(ret == TEE_SUCCESS);
+
+	/*
+	 * test open logical channels based on AID selected on basic channel
+	 * (D0000CAFE00001 is default selected on basic channel,
+	 * this call should success since D0000CAFE00001 is MultiSelectable,
+	 * upon open, each logical channel should select D0000CAFE00001)
+	 */
+	for (i = 1; i < MAX_LOGICAL_CHANNEL; i ++) {
+		ret = tee_se_session_open_logical_channel(s, NULL, &channel[i]);
+		ASSERT(ret == TEE_SUCCESS);
+	}
+
+	/*
+	 * should fail on next open
+	 * (exceeds maximum logical channel number)
+	 */
+	ret = tee_se_session_open_logical_channel(s, NULL, &channel[0]);
+	ASSERT(ret == TEE_ERROR_NOT_SUPPORTED);
+
+	/* close 3 channels */
+	for (i = 1; i < 4; i++) {
+		tee_se_session_close_channel(s, channel[i]);
+		channel[i] = NULL;
+	}
+
+	/* re-open 3 channels (should success) */
+	for (i = 1; i < 4; i++) {
+		ret = tee_se_session_open_logical_channel(s, NULL, &channel[i]);
+		ASSERT(ret == TEE_SUCCESS);
+	}
+
+	/* logical channel 1 select D0000CAFE00002 (should success) */
+	tee_se_aid_create("D0000CAFE00002", &aid);
+	ret = tee_se_channel_select(channel[1], aid);
+	ASSERT(ret == TEE_SUCCESS);
+
+	/* logical channel 2 select D0000CAFE00002
+	 * (should fail since D0000CAFE00002 is not MultiSelectable)
+	 */
+	ret = tee_se_channel_select(channel[2], aid);
+	ASSERT(ret == TEE_ERROR_NOT_SUPPORTED);
+
+	/* clean up */
+	for (i = 1; i < MAX_LOGICAL_CHANNEL; i++)
+		tee_se_session_close_channel(s, channel[i]);
+	tee_se_session_close(s);
+	tee_se_aid_release(aid);
+	DMSG("exit");
+
+	return TEE_SUCCESS;
+}
+
+static TEE_Result verify_result(struct resp_apdu *apdu, const char *data)
+{
+	size_t str_length = strlen(data);
+	size_t byte_length = strlen(data) / 2;
+	uint8_t *resp_data = resp_apdu_get_data(apdu);
+	size_t resp_len = resp_apdu_get_data_len(apdu);
+	uint8_t bytes[byte_length];
+	size_t i = 0;
+
+	ASSERT(resp_len == byte_length);
+
+	hex_decode(data, str_length, bytes);
+	while (i < resp_len) {
+		ASSERT(bytes[i] == resp_data[i]);
+		i++;
+	}
+	return TEE_SUCCESS;
+}
+
+static TEE_Result test_transmit(struct tee_se_reader_proxy **proxies)
+{
+	struct tee_se_channel *c1 = NULL, *c2 = NULL;
+	struct tee_se_session *s1 = NULL, *s2 = NULL;
+	struct tee_se_aid *full_aid = NULL, *partial_aid = NULL;
+	struct cmd_apdu *cmd;
+	struct resp_apdu *resp;
+	size_t tx_buf_len = 0, rx_buf_len = 7;
+	TEE_Result ret;
+
+	DMSG("entry");
+	ret = tee_se_aid_create("D0000CAFE00001", &full_aid);
+	ASSERT(ret == TEE_SUCCESS);
+
+	ret = tee_se_aid_create("D0000CAFE0000", &partial_aid);
+	ASSERT(ret == TEE_SUCCESS);
+
+	cmd = alloc_cmd_apdu(ISO7816_CLA, 0xFF, 0x0, 0x0,
+			tx_buf_len, rx_buf_len, NULL);
+	ASSERT(cmd);
+	resp = alloc_resp_apdu(rx_buf_len);
+	ASSERT(resp);
+
+	ret = tee_se_reader_open_session(proxies[0], &s1);
+	ASSERT(ret == TEE_SUCCESS);
+
+	ret = tee_se_reader_open_session(proxies[0], &s2);
+	ASSERT(ret == TEE_SUCCESS);
+
+	/* open logical channel on s1 (given full aid) */
+	ret = tee_se_session_open_logical_channel(s1, full_aid, &c1);
+	ASSERT(ret == TEE_SUCCESS);
+
+	/* should route to D0000CAFE00001 */
+	ret = tee_se_channel_transmit(c1, cmd, resp);
+	ASSERT(ret == TEE_SUCCESS);
+
+	/* select next should fail (full aid given) */
+	ret = tee_se_channel_select_next(c1);
+	ASSERT(ret == TEE_ERROR_ITEM_NOT_FOUND);
+
+	/* open logical channel on s2 (given partial aid) */
+	ret = tee_se_session_open_logical_channel(s2, partial_aid, &c2);
+	ASSERT(ret == TEE_SUCCESS);
+
+	/* should route to D0000CAFE00001 */
+	ret = tee_se_channel_transmit(c2, cmd, resp);
+	ASSERT(ret == TEE_SUCCESS);
+	ret = verify_result(resp, "D0000CAFE00001");
+	ASSERT(ret == TEE_SUCCESS);
+
+	/* select next should success (select D0000CAFE00002) */
+	ret = tee_se_channel_select_next(c2);
+	ASSERT(ret == TEE_SUCCESS);
+
+	/* should route to D0000CAFE00002 */
+	ret = tee_se_channel_transmit(c2, cmd, resp);
+	ASSERT(ret == TEE_SUCCESS);
+	ret = verify_result(resp, "D0000CAFE00002");
+	ASSERT(ret == TEE_SUCCESS);
+
+	/* select next should success (select D0000CAFE00001) */
+	ret = tee_se_channel_select_next(c2);
+	ASSERT(ret == TEE_SUCCESS);
+
+	/* should route to D0000CAFE00001 */
+	ret = tee_se_channel_transmit(c2, cmd, resp);
+	ASSERT(ret == TEE_SUCCESS);
+	ret = verify_result(resp, "D0000CAFE00001");
+	ASSERT(ret == TEE_SUCCESS);
+
+	/*
+	 * test route to the same applet in a row from different channel
+	 * (both should success)
+	 */
+	ret = tee_se_channel_transmit(c1, cmd, resp);
+	ASSERT(ret == TEE_SUCCESS);
+	ret = verify_result(resp, "D0000CAFE00001");
+	ASSERT(ret == TEE_SUCCESS);
+
+	ret = tee_se_channel_transmit(c2, cmd, resp);
+	ASSERT(ret == TEE_SUCCESS);
+	ret = verify_result(resp, "D0000CAFE00001");
+	ASSERT(ret == TEE_SUCCESS);
+
+	/* clean up */
+	tee_se_session_close_channel(s1, c1);
+	tee_se_session_close_channel(s2, c2);
+
+	tee_se_session_close(s1);
+	tee_se_session_close(s2);
+
+	tee_se_aid_release(full_aid);
+	tee_se_aid_release(partial_aid);
+	DMSG("exit");
+
+	return TEE_SUCCESS;
+}
+
+static TEE_Result se_api_self_tests(uint32_t nParamTypes __unused,
+				    TEE_Param pParams[TEE_NUM_PARAMS] __unused)
+{
+	size_t size = MAX_READERS;
+	TEE_Result ret;
+	struct tee_se_reader_proxy **proxies =
+		malloc(sizeof(void *) * MAX_READERS);
+
+	tee_se_manager_get_readers(proxies, &size);
+
+	ret = test_aid(proxies);
+	CHECK(ret);
+
+	ret = test_select_resp(proxies);
+	CHECK(ret);
+
+	ret = test_session(proxies);
+	CHECK(ret);
+
+	ret = test_logical_channel(proxies);
+	CHECK(ret);
+
+	ret = test_transmit(proxies);
+	CHECK(ret);
+
+	ret = test_reader(proxies);
+	CHECK(ret);
+
+	free(proxies);
+
+	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 se_api_self_tests(nParamTypes, pParams);
+	default:
+		break;
+	}
+	return TEE_ERROR_BAD_PARAMETERS;
+}
+
+pseudo_ta_register(.uuid = SE_API_SELF_TEST_UUID, .name = TA_NAME,
+		   .flags = PTA_DEFAULT_FLAGS,
+		   .invoke_command_entry_point = invoke_command);
diff --git a/core/arch/arm/pta/stats.c b/core/arch/arm/pta/stats.c
new file mode 100644
index 0000000..408c284
--- /dev/null
+++ b/core/arch/arm/pta/stats.c
@@ -0,0 +1,161 @@
+/*
+ * 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 <compiler.h>
+#include <stdio.h>
+#include <trace.h>
+#include <kernel/pseudo_ta.h>
+#include <mm/tee_pager.h>
+#include <mm/tee_mm.h>
+#include <string.h>
+#include <string_ext.h>
+#include <malloc.h>
+
+#define TA_NAME		"stats.ta"
+
+#define STATS_UUID \
+		{ 0xd96a5b40, 0xe2c7, 0xb1af, \
+			{ 0x87, 0x94, 0x10, 0x02, 0xa5, 0xd5, 0xc6, 0x1b } }
+
+#define STATS_CMD_PAGER_STATS		0
+#define STATS_CMD_ALLOC_STATS		1
+
+#define STATS_NB_POOLS			3
+
+static TEE_Result get_alloc_stats(uint32_t type, TEE_Param p[TEE_NUM_PARAMS])
+{
+	struct malloc_stats *stats;
+	uint32_t size_to_retrieve;
+	uint32_t pool_id;
+	uint32_t i;
+
+	/*
+	 * p[0].value.a = pool id (from 0 to n)
+	 *   - 0 means all the pools to be retrieved
+	 *   - 1..n means pool id
+	 * p[0].value.b = 0 if no reset of the stats
+	 * p[1].memref.buffer = output buffer to struct malloc_stats
+	 */
+	if (TEE_PARAM_TYPES(TEE_PARAM_TYPE_VALUE_INPUT,
+			    TEE_PARAM_TYPE_MEMREF_OUTPUT,
+			    TEE_PARAM_TYPE_NONE,
+			    TEE_PARAM_TYPE_NONE) != type) {
+		return TEE_ERROR_BAD_PARAMETERS;
+	}
+
+	pool_id = p[0].value.a;
+	if (pool_id > STATS_NB_POOLS)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	size_to_retrieve = sizeof(struct malloc_stats);
+	if (!pool_id)
+		size_to_retrieve *= STATS_NB_POOLS;
+
+	if (p[1].memref.size < size_to_retrieve) {
+		p[1].memref.size = size_to_retrieve;
+		return TEE_ERROR_SHORT_BUFFER;
+	}
+	p[1].memref.size = size_to_retrieve;
+	stats = p[1].memref.buffer;
+
+	for (i = 1; i <= STATS_NB_POOLS; i++) {
+		if ((pool_id) && (i != pool_id))
+			continue;
+
+		switch (i) {
+		case 1:
+			malloc_get_stats(stats);
+			strlcpy(stats->desc, "Heap", sizeof(stats->desc));
+			if (p[0].value.b)
+				malloc_reset_stats();
+			break;
+
+		case 2:
+			EMSG("public DDR not managed by secure side anymore");
+			break;
+
+		case 3:
+			tee_mm_get_pool_stats(&tee_mm_sec_ddr, stats,
+					      !!p[0].value.b);
+			strlcpy(stats->desc, "Secure DDR", sizeof(stats->desc));
+			break;
+
+		default:
+			EMSG("Wrong pool id");
+			break;
+		}
+
+		stats++;
+	}
+
+	return TEE_SUCCESS;
+}
+
+static TEE_Result get_pager_stats(uint32_t type, TEE_Param p[TEE_NUM_PARAMS])
+{
+	struct tee_pager_stats stats;
+
+	if (TEE_PARAM_TYPES(TEE_PARAM_TYPE_VALUE_OUTPUT,
+			    TEE_PARAM_TYPE_VALUE_OUTPUT,
+			    TEE_PARAM_TYPE_VALUE_OUTPUT,
+			    TEE_PARAM_TYPE_NONE) != type) {
+		EMSG("expect 3 output values as argument");
+		return TEE_ERROR_BAD_PARAMETERS;
+	}
+
+	tee_pager_get_stats(&stats);
+	p[0].value.a = stats.npages;
+	p[0].value.b = stats.npages_all;
+	p[1].value.a = stats.ro_hits;
+	p[1].value.b = stats.rw_hits;
+	p[2].value.a = stats.hidden_hits;
+	p[2].value.b = stats.zi_released;
+
+	return TEE_SUCCESS;
+}
+
+/*
+ * Trusted Application Entry Points
+ */
+
+static TEE_Result invoke_command(void *psess __unused,
+				 uint32_t cmd, uint32_t ptypes,
+				 TEE_Param params[TEE_NUM_PARAMS])
+{
+	switch (cmd) {
+	case STATS_CMD_PAGER_STATS:
+		return get_pager_stats(ptypes, params);
+	case STATS_CMD_ALLOC_STATS:
+		return get_alloc_stats(ptypes, params);
+	default:
+		break;
+	}
+	return TEE_ERROR_BAD_PARAMETERS;
+}
+
+pseudo_ta_register(.uuid = STATS_UUID, .name = TA_NAME,
+		   .flags = PTA_DEFAULT_FLAGS,
+		   .invoke_command_entry_point = invoke_command);
diff --git a/core/arch/arm/pta/sub.mk b/core/arch/arm/pta/sub.mk
new file mode 100644
index 0000000..3d961d4
--- /dev/null
+++ b/core/arch/arm/pta/sub.mk
@@ -0,0 +1,14 @@
+srcs-$(CFG_TEE_CORE_EMBED_INTERNAL_TESTS) += pta_self_tests.c
+srcs-$(CFG_TEE_CORE_EMBED_INTERNAL_TESTS) += core_self_tests.c
+srcs-$(CFG_TEE_CORE_EMBED_INTERNAL_TESTS) += interrupt_tests.c
+srcs-$(CFG_WITH_STATS) += stats.c
+srcs-$(CFG_TA_GPROF_SUPPORT) += gprof.c
+
+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
new file mode 100644
index 0000000..f9dc714
--- /dev/null
+++ b/core/arch/arm/pta/tee_fs_key_manager_tests.c
@@ -0,0 +1,375 @@
+/*
+ * 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
new file mode 100644
index 0000000..b2bd645
--- /dev/null
+++ b/core/arch/arm/sm/psci.c
@@ -0,0 +1,166 @@
+/*
+ * Copyright (C) 2016 Freescale Semiconductor, Inc.
+ * All rights reserved.
+ *
+ * Peng Fan <peng.fan@nxp.com>
+ *
+ * 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 <console.h>
+#include <kernel/generic_boot.h>
+#include <kernel/thread.h>
+#include <stdint.h>
+#include <sm/optee_smc.h>
+#include <sm/psci.h>
+#include <sm/sm.h>
+#include <trace.h>
+
+__weak uint32_t psci_version(void)
+{
+	return PSCI_VERSION_0_2;
+}
+
+__weak int psci_cpu_suspend(uint32_t power_state __unused,
+			    uintptr_t entry __unused,
+			    uint32_t context_id __unused)
+{
+	return PSCI_RET_NOT_SUPPORTED;
+}
+
+__weak int psci_cpu_off(void)
+{
+	return PSCI_RET_NOT_SUPPORTED;
+}
+
+__weak int psci_cpu_on(uint32_t cpu_id __unused, uint32_t entry __unused,
+		       uint32_t context_id __unused)
+{
+	return PSCI_RET_NOT_SUPPORTED;
+}
+
+__weak int psci_affinity_info(uint32_t affinity __unused,
+			      uint32_t lowest_affnity_level __unused)
+{
+	return PSCI_RET_NOT_SUPPORTED;
+}
+
+__weak int psci_migrate(uint32_t cpu_id __unused)
+{
+	return PSCI_RET_NOT_SUPPORTED;
+}
+
+__weak int psci_migrate_info_type(void)
+{
+	return PSCI_RET_NOT_SUPPORTED;
+}
+
+__weak int psci_migrate_info_up_cpu(void)
+{
+	return PSCI_RET_NOT_SUPPORTED;
+}
+
+__weak void psci_system_off(void)
+{
+}
+
+__weak void psci_system_reset(void)
+{
+}
+
+__weak int psci_features(uint32_t psci_fid __unused)
+{
+	return PSCI_RET_NOT_SUPPORTED;
+}
+
+__weak int psci_node_hw_state(uint32_t cpu_id __unused,
+			      uint32_t power_level __unused)
+{
+	return PSCI_RET_NOT_SUPPORTED;
+}
+
+__weak int psci_stat_residency(uint32_t cpu_id __unused,
+			       uint32_t power_state __unused)
+{
+	return PSCI_RET_NOT_SUPPORTED;
+}
+
+__weak int psci_stat_count(uint32_t cpu_id __unused,
+			   uint32_t power_state __unused)
+{
+	return PSCI_RET_NOT_SUPPORTED;
+}
+
+void tee_psci_handler(struct thread_smc_args *args)
+{
+	uint32_t smc_fid = args->a0;
+	uint32_t a1 = args->a1;
+	uint32_t a2 = args->a2;
+	uint32_t a3 = args->a3;
+
+	switch (smc_fid) {
+	case PSCI_VERSION:
+		args->a0 = psci_version();
+		break;
+	case PSCI_CPU_SUSPEND:
+		args->a0 = psci_cpu_suspend(a1, a2, a3);
+		break;
+	case PSCI_CPU_OFF:
+		args->a0 = psci_cpu_off();
+		break;
+	case PSCI_CPU_ON:
+		args->a0 = psci_cpu_on(a1, a2, a3);
+		break;
+	case PSCI_AFFINITY_INFO:
+		args->a0 = psci_affinity_info(a1, a2);
+		break;
+	case PSCI_MIGRATE:
+		args->a0 = psci_migrate(a1);
+		break;
+	case PSCI_MIGRATE_INFO_TYPE:
+		args->a0 = psci_migrate_info_type();
+		break;
+	case PSCI_MIGRATE_INFO_UP_CPU:
+		args->a0 = psci_migrate_info_up_cpu();
+		break;
+	case PSCI_SYSTEM_OFF:
+		psci_system_off();
+		while (1)
+			;
+		break;
+	case PSCI_SYSTEM_RESET:
+		psci_system_off();
+		while (1)
+			;
+		break;
+	case PSCI_PSCI_FEATURES:
+		args->a0 = psci_features(a1);
+		break;
+	case PSCI_NODE_HW_STATE:
+		args->a0 = psci_node_hw_state(a1, a2);
+		break;
+	default:
+		args->a0 = OPTEE_SMC_RETURN_UNKNOWN_FUNCTION;
+		break;
+	}
+}
diff --git a/core/arch/arm/sm/sm.c b/core/arch/arm/sm/sm.c
new file mode 100644
index 0000000..4a0c0f6
--- /dev/null
+++ b/core/arch/arm/sm/sm.c
@@ -0,0 +1,58 @@
+/*
+ * Copyright (c) 2016, Linaro Limited
+ * 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 <arm.h>
+#include <compiler.h>
+#include <kernel/misc.h>
+#include <platform_config.h>
+#include <sm/optee_smc.h>
+#include <sm/sm.h>
+#include <sm/std_smc.h>
+#include <string.h>
+#include "sm_private.h"
+
+bool sm_from_nsec(struct sm_ctx *ctx)
+{
+	uint32_t *nsec_r0 = (uint32_t *)(&ctx->nsec.r0);
+
+#ifdef CFG_PSCI_ARM32
+	if (OPTEE_SMC_OWNER_NUM(*nsec_r0) == OPTEE_SMC_OWNER_STANDARD) {
+		smc_std_handler((struct thread_smc_args *)nsec_r0);
+		return false;	/* Return to non secure state */
+	}
+#endif
+
+	sm_save_modes_regs(&ctx->nsec.mode_regs);
+	sm_restore_modes_regs(&ctx->sec.mode_regs);
+
+	memcpy(&ctx->sec.r0, nsec_r0, sizeof(uint32_t) * 8);
+	if (OPTEE_SMC_IS_FAST_CALL(ctx->sec.r0))
+		ctx->sec.mon_lr = (uint32_t)&thread_vector_table.fast_smc_entry;
+	else
+		ctx->sec.mon_lr = (uint32_t)&thread_vector_table.std_smc_entry;
+	return true;	/* return into secure state */
+}
diff --git a/core/arch/arm/sm/sm_a32.S b/core/arch/arm/sm/sm_a32.S
new file mode 100644
index 0000000..9c6becd
--- /dev/null
+++ b/core/arch/arm/sm/sm_a32.S
@@ -0,0 +1,291 @@
+/*
+ * Copyright (c) 2016, Linaro Limited
+ * 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 <asm.S>
+#include <arm.h>
+#include <arm32_macros.S>
+#include <kernel/unwind.h>
+#include <sm/optee_smc.h>
+#include <sm/teesmc_opteed.h>
+#include <sm/teesmc_opteed_macros.h>
+#include <asm-defines.h>
+
+	.section .text.sm_asm
+
+FUNC sm_save_modes_regs , :
+UNWIND(	.fnstart)
+UNWIND(	.cantunwind)
+	/* User mode registers has to be saved from system mode */
+	cps	#CPSR_MODE_SYS
+	stm	r0!, {sp, lr}
+
+	cps	#CPSR_MODE_IRQ
+	mrs	r2, spsr
+	stm	r0!, {r2, sp, lr}
+
+	cps	#CPSR_MODE_FIQ
+	mrs	r2, spsr
+	stm	r0!, {r2, sp, lr}
+
+	cps	#CPSR_MODE_SVC
+	mrs	r2, spsr
+	stm	r0!, {r2, sp, lr}
+
+	cps	#CPSR_MODE_ABT
+	mrs	r2, spsr
+	stm	r0!, {r2, sp, lr}
+
+	cps	#CPSR_MODE_UND
+	mrs	r2, spsr
+	stm	r0!, {r2, sp, lr}
+
+	cps	#CPSR_MODE_MON
+	bx	lr
+UNWIND(	.fnend)
+END_FUNC sm_save_modes_regs
+
+/* Restores the mode specific registers */
+FUNC sm_restore_modes_regs , :
+UNWIND(	.fnstart)
+UNWIND(	.cantunwind)
+	/* User mode registers has to be saved from system mode */
+	cps	#CPSR_MODE_SYS
+	ldm	r0!, {sp, lr}
+
+	cps	#CPSR_MODE_IRQ
+	ldm	r0!, {r2, sp, lr}
+	msr	spsr_fsxc, r2
+
+	cps	#CPSR_MODE_FIQ
+	ldm	r0!, {r2, sp, lr}
+	msr	spsr_fsxc, r2
+
+	cps	#CPSR_MODE_SVC
+	ldm	r0!, {r2, sp, lr}
+	msr	spsr_fsxc, r2
+
+	cps	#CPSR_MODE_ABT
+	ldm	r0!, {r2, sp, lr}
+	msr	spsr_fsxc, r2
+
+	cps	#CPSR_MODE_UND
+	ldm	r0!, {r2, sp, lr}
+	msr	spsr_fsxc, r2
+
+	cps	#CPSR_MODE_MON
+	bx	lr
+UNWIND(	.fnend)
+END_FUNC sm_restore_modes_regs
+
+/*
+ * stack_tmp is used as stack, the top of the stack is reserved to hold
+ * struct sm_ctx, everything below is for normal stack usage. As several
+ * different CPU modes are using the same stack it's important that switch
+ * of CPU mode isn't done until one mode is done. This means FIQ, IRQ and
+ * Async abort has to be masked while using stack_tmp.
+ */
+LOCAL_FUNC sm_smc_entry , :
+UNWIND(	.fnstart)
+UNWIND(	.cantunwind)
+	srsdb	sp!, #CPSR_MODE_MON
+	push	{r0-r7}
+
+	clrex		/* Clear the exclusive monitor */
+
+	/* Find out if we're doing an secure or non-secure entry */
+	read_scr r1
+	tst	r1, #SCR_NS
+	bne	.smc_from_nsec
+
+	/*
+	 * As we're coming from secure world (NS bit cleared) the stack
+	 * pointer points to sm_ctx.sec.r0 at this stage. After the
+	 * instruction below the stack pointer points to sm_ctx.
+	 */
+	sub	sp, sp, #(SM_CTX_SEC + SM_SEC_CTX_R0)
+
+	/* Save secure context */
+	add	r0, sp, #SM_CTX_SEC
+	bl	sm_save_modes_regs
+
+	/*
+	 * On FIQ exit we're restoring the non-secure context unchanged, on
+	 * all other exits we're shifting r1-r4 from secure context into
+	 * r0-r3 in non-secure context.
+	 */
+	add	r8, sp, #(SM_CTX_SEC + SM_SEC_CTX_R0)
+	ldm	r8, {r0-r4}
+	mov_imm	r9, TEESMC_OPTEED_RETURN_FIQ_DONE
+	cmp	r0, r9
+	addne	r8, sp, #(SM_CTX_NSEC + SM_NSEC_CTX_R0)
+	stmne	r8, {r1-r4}
+
+	/* Restore non-secure context */
+	add	r0, sp, #SM_CTX_NSEC
+	bl	sm_restore_modes_regs
+
+.sm_ret_to_nsec:
+	/*
+	 * Return to non-secure world
+	 */
+	add     r0, sp, #(SM_CTX_NSEC + SM_NSEC_CTX_R8)
+	ldm	r0, {r8-r12}
+
+	/* Update SCR */
+	read_scr r0
+	orr	r0, r0, #(SCR_NS | SCR_FIQ) /* Set NS and FIQ bit in SCR */
+	write_scr r0
+
+	add	sp, sp, #(SM_CTX_NSEC + SM_NSEC_CTX_R0)
+	b	.sm_exit
+
+.smc_from_nsec:
+	/*
+	 * As we're coming from non-secure world (NS bit set) the stack
+	 * pointer points to sm_ctx.nsec.r0 at this stage. After the
+	 * instruction below the stack pointer points to sm_ctx.
+	 */
+	sub	sp, sp, #(SM_CTX_NSEC + SM_NSEC_CTX_R0)
+
+	bic	r1, r1, #(SCR_NS | SCR_FIQ) /* Clear NS and FIQ bit in SCR */
+	write_scr r1
+
+	add	r0, sp, #(SM_CTX_NSEC + SM_NSEC_CTX_R8)
+	stm	r0, {r8-r12}
+
+	mov	r0, sp
+	bl	sm_from_nsec
+	cmp	r0, #0
+	beq	.sm_ret_to_nsec
+
+	/*
+	 * Continue into secure world
+	 */
+	add	sp, sp, #(SM_CTX_SEC + SM_SEC_CTX_R0)
+
+.sm_exit:
+	pop	{r0-r7}
+	rfefd	sp!
+UNWIND(	.fnend)
+END_FUNC sm_smc_entry
+
+/*
+ * FIQ handling
+ *
+ * Saves CPU context in the same way as sm_smc_entry() above. The CPU
+ * context will later be restored by sm_smc_entry() when handling a return
+ * from FIQ.
+ */
+LOCAL_FUNC sm_fiq_entry , :
+UNWIND(	.fnstart)
+UNWIND(	.cantunwind)
+	/* FIQ has a +4 offset for lr compared to preferred return address */
+	sub	lr, lr, #4
+	/* sp points just past struct sm_sec_ctx */
+	srsdb	sp!, #CPSR_MODE_MON
+	push	{r0-r7}
+
+	clrex		/* Clear the exclusive monitor */
+
+	/*
+	 * As we're coming from non-secure world the stack pointer points
+	 * to sm_ctx.nsec.r0 at this stage. After the instruction below the
+	 * stack pointer points to sm_ctx.
+	 */
+	sub	sp, sp, #(SM_CTX_NSEC + SM_NSEC_CTX_R0)
+
+	/* Update SCR */
+	read_scr r1
+	bic	r1, r1, #(SCR_NS | SCR_FIQ) /* Clear NS and FIQ bit in SCR */
+	write_scr r1
+
+	/* Save non-secure context */
+	add	r0, sp, #SM_CTX_NSEC
+	bl	sm_save_modes_regs
+	stm	r0!, {r8-r12}
+
+	/* Set FIQ entry */
+	ldr	r0, =(thread_vector_table + THREAD_VECTOR_TABLE_FIQ_ENTRY)
+	str	r0, [sp, #(SM_CTX_SEC + SM_SEC_CTX_MON_LR)]
+
+	/* Restore secure context */
+	add	r0, sp, #SM_CTX_SEC
+	bl	sm_restore_modes_regs
+
+	add	sp, sp, #(SM_CTX_SEC + SM_SEC_CTX_MON_LR)
+
+	rfefd	sp!
+UNWIND(	.fnend)
+END_FUNC sm_fiq_entry
+
+        .align	5
+LOCAL_FUNC sm_vect_table , :
+UNWIND(	.fnstart)
+UNWIND(	.cantunwind)
+	b	.		/* Reset			*/
+	b	.		/* Undefined instruction	*/
+	b	sm_smc_entry	/* Secure monitor call		*/
+	b	.		/* Prefetch abort		*/
+	b	.		/* Data abort			*/
+	b	.		/* Reserved			*/
+	b	.		/* IRQ				*/
+	b	sm_fiq_entry	/* FIQ				*/
+UNWIND(	.fnend)
+END_FUNC sm_vect_table
+
+/* void sm_init(vaddr_t stack_pointer); */
+FUNC sm_init , :
+UNWIND(	.fnstart)
+	/* Set monitor stack */
+	mrs	r1, cpsr
+	cps	#CPSR_MODE_MON
+	/* Point just beyond sm_ctx.sec */
+	sub	sp, r0, #(SM_CTX_SIZE - SM_CTX_NSEC)
+	msr	cpsr, r1
+
+	/* Set monitor vector (MVBAR) */
+	ldr	r0, =sm_vect_table
+	write_mvbar r0
+
+	bx	lr
+END_FUNC sm_init
+
+
+/* struct sm_nsec_ctx *sm_get_nsec_ctx(void); */
+FUNC sm_get_nsec_ctx , :
+	mrs	r1, cpsr
+	cps	#CPSR_MODE_MON
+	mov	r0, sp
+	msr	cpsr, r1
+
+	/*
+	 * As we're in secure mode mon_sp points just beyond sm_ctx.sec
+	 * which is sm_ctx.nsec
+	 */
+	bx	lr
+END_FUNC sm_get_nsec_ctx
diff --git a/core/arch/arm/sm/sm_private.h b/core/arch/arm/sm/sm_private.h
new file mode 100644
index 0000000..0b41bec
--- /dev/null
+++ b/core/arch/arm/sm/sm_private.h
@@ -0,0 +1,38 @@
+/*
+ * Copyright (c) 2016, Linaro Limited
+ * 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.
+ */
+#ifndef SM_PRIVATE_H
+#define SM_PRIVATE_H
+
+/* Returns true if returning to sec, false if returning to nsec */
+bool sm_from_nsec(struct sm_ctx *ctx);
+
+void sm_save_modes_regs(struct sm_mode_regs *regs);
+void sm_restore_modes_regs(struct sm_mode_regs *regs);
+
+#endif /*SM_PRIVATE_H*/
+
diff --git a/core/arch/arm/sm/std_smc.c b/core/arch/arm/sm/std_smc.c
new file mode 100644
index 0000000..5e5bb81
--- /dev/null
+++ b/core/arch/arm/sm/std_smc.c
@@ -0,0 +1,77 @@
+/*
+ * Copyright (C) 2016 Freescale Semiconductor, Inc.
+ * All rights reserved.
+ *
+ * Peng Fan <peng.fan@nxp.com>
+ *
+ * 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 <stdint.h>
+#include <sm/optee_smc.h>
+#include <sm/psci.h>
+#include <sm/sm.h>
+#include <sm/std_smc.h>
+#include <tee/uuid.h>
+#include <trace.h>
+
+static const TEE_UUID uuid = {
+	0x5f8b97df, 0x2d0d, 0x4ad2,
+	{0x98, 0xd2, 0x74, 0xf4, 0x38, 0x27, 0x98, 0xbb},
+};
+
+void smc_std_handler(struct thread_smc_args *args)
+{
+	uint32_t smc_fid = args->a0;
+
+	if (is_psci_fid(smc_fid)) {
+		tee_psci_handler(args);
+		return;
+	}
+
+	switch (smc_fid) {
+	case ARM_STD_SVC_CALL_COUNT:
+		/* PSCI is the only STD service implemented */
+		args->a0 = PSCI_NUM_CALLS;
+		break;
+	case ARM_STD_SVC_UID:
+		args->a0 = uuid.timeLow;
+		args->a1 = (uuid.timeHiAndVersion << 16) | uuid.timeMid;
+		args->a2 = (uuid.clockSeqAndNode[3] << 24) |
+			(uuid.clockSeqAndNode[2] << 16) |
+			(uuid.clockSeqAndNode[1] << 8) |
+			uuid.clockSeqAndNode[0];
+		args->a3 = (uuid.clockSeqAndNode[7] << 24) |
+			(uuid.clockSeqAndNode[6] << 16) |
+			(uuid.clockSeqAndNode[5] << 8) |
+			uuid.clockSeqAndNode[4];
+		break;
+	case ARM_STD_SVC_VERSION:
+		args->a0 = STD_SVC_VERSION_MAJOR;
+		args->a1 = STD_SVC_VERSION_MINOR;
+		break;
+	default:
+		args->a0 = OPTEE_SMC_RETURN_UNKNOWN_FUNCTION;
+		break;
+	}
+}
diff --git a/core/arch/arm/sm/sub.mk b/core/arch/arm/sm/sub.mk
new file mode 100644
index 0000000..4e28e29
--- /dev/null
+++ b/core/arch/arm/sm/sub.mk
@@ -0,0 +1,3 @@
+srcs-y += sm_a32.S
+srcs-y += sm.c
+srcs-$(CFG_PSCI_ARM32) += std_smc.c psci.c
diff --git a/core/arch/arm/tee/arch_svc.c b/core/arch/arm/tee/arch_svc.c
new file mode 100644
index 0000000..8a89ce9
--- /dev/null
+++ b/core/arch/arm/tee/arch_svc.c
@@ -0,0 +1,269 @@
+/*
+ * Copyright (c) 2014, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <arm.h>
+#include <assert.h>
+#include <kernel/misc.h>
+#include <kernel/thread.h>
+#include <kernel/trace_ta.h>
+#include <tee/tee_svc.h>
+#include <tee/arch_svc.h>
+#include <tee/tee_svc_cryp.h>
+#include <tee/tee_svc_storage.h>
+#include <tee/se/svc.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
+#endif
+
+struct syscall_entry {
+	syscall_t fn;
+#ifdef TRACE_SYSCALLS
+	const char *name;
+#endif
+};
+
+#ifdef TRACE_SYSCALLS
+#define SYSCALL_ENTRY(_fn) { .fn = (syscall_t)_fn, .name = #_fn }
+#else
+#define SYSCALL_ENTRY(_fn) { .fn = (syscall_t)_fn }
+#endif
+
+/*
+ * This array is ordered according to the SYSCALL ids TEE_SCN_xxx
+ */
+static const struct syscall_entry tee_svc_syscall_table[] = {
+	SYSCALL_ENTRY(syscall_sys_return),
+	SYSCALL_ENTRY(syscall_log),
+	SYSCALL_ENTRY(syscall_panic),
+	SYSCALL_ENTRY(syscall_get_property),
+	SYSCALL_ENTRY(syscall_get_property_name_to_index),
+	SYSCALL_ENTRY(syscall_open_ta_session),
+	SYSCALL_ENTRY(syscall_close_ta_session),
+	SYSCALL_ENTRY(syscall_invoke_ta_command),
+	SYSCALL_ENTRY(syscall_check_access_rights),
+	SYSCALL_ENTRY(syscall_get_cancellation_flag),
+	SYSCALL_ENTRY(syscall_unmask_cancellation),
+	SYSCALL_ENTRY(syscall_mask_cancellation),
+	SYSCALL_ENTRY(syscall_wait),
+	SYSCALL_ENTRY(syscall_get_time),
+	SYSCALL_ENTRY(syscall_set_ta_time),
+	SYSCALL_ENTRY(syscall_cryp_state_alloc),
+	SYSCALL_ENTRY(syscall_cryp_state_copy),
+	SYSCALL_ENTRY(syscall_cryp_state_free),
+	SYSCALL_ENTRY(syscall_hash_init),
+	SYSCALL_ENTRY(syscall_hash_update),
+	SYSCALL_ENTRY(syscall_hash_final),
+	SYSCALL_ENTRY(syscall_cipher_init),
+	SYSCALL_ENTRY(syscall_cipher_update),
+	SYSCALL_ENTRY(syscall_cipher_final),
+	SYSCALL_ENTRY(syscall_cryp_obj_get_info),
+	SYSCALL_ENTRY(syscall_cryp_obj_restrict_usage),
+	SYSCALL_ENTRY(syscall_cryp_obj_get_attr),
+	SYSCALL_ENTRY(syscall_cryp_obj_alloc),
+	SYSCALL_ENTRY(syscall_cryp_obj_close),
+	SYSCALL_ENTRY(syscall_cryp_obj_reset),
+	SYSCALL_ENTRY(syscall_cryp_obj_populate),
+	SYSCALL_ENTRY(syscall_cryp_obj_copy),
+	SYSCALL_ENTRY(syscall_cryp_derive_key),
+	SYSCALL_ENTRY(syscall_cryp_random_number_generate),
+	SYSCALL_ENTRY(syscall_authenc_init),
+	SYSCALL_ENTRY(syscall_authenc_update_aad),
+	SYSCALL_ENTRY(syscall_authenc_update_payload),
+	SYSCALL_ENTRY(syscall_authenc_enc_final),
+	SYSCALL_ENTRY(syscall_authenc_dec_final),
+	SYSCALL_ENTRY(syscall_asymm_operate),
+	SYSCALL_ENTRY(syscall_asymm_verify),
+	SYSCALL_ENTRY(syscall_storage_obj_open),
+	SYSCALL_ENTRY(syscall_storage_obj_create),
+	SYSCALL_ENTRY(syscall_storage_obj_del),
+	SYSCALL_ENTRY(syscall_storage_obj_rename),
+	SYSCALL_ENTRY(syscall_storage_alloc_enum),
+	SYSCALL_ENTRY(syscall_storage_free_enum),
+	SYSCALL_ENTRY(syscall_storage_reset_enum),
+	SYSCALL_ENTRY(syscall_storage_start_enum),
+	SYSCALL_ENTRY(syscall_storage_next_enum),
+	SYSCALL_ENTRY(syscall_storage_obj_read),
+	SYSCALL_ENTRY(syscall_storage_obj_write),
+	SYSCALL_ENTRY(syscall_storage_obj_trunc),
+	SYSCALL_ENTRY(syscall_storage_obj_seek),
+	SYSCALL_ENTRY(syscall_obj_generate_key),
+	SYSCALL_ENTRY(syscall_se_service_open),
+	SYSCALL_ENTRY(syscall_se_service_close),
+	SYSCALL_ENTRY(syscall_se_service_get_readers),
+	SYSCALL_ENTRY(syscall_se_reader_get_prop),
+	SYSCALL_ENTRY(syscall_se_reader_get_name),
+	SYSCALL_ENTRY(syscall_se_reader_open_session),
+	SYSCALL_ENTRY(syscall_se_reader_close_sessions),
+	SYSCALL_ENTRY(syscall_se_session_is_closed),
+	SYSCALL_ENTRY(syscall_se_session_get_atr),
+	SYSCALL_ENTRY(syscall_se_session_open_channel),
+	SYSCALL_ENTRY(syscall_se_session_close),
+	SYSCALL_ENTRY(syscall_se_channel_select_next),
+	SYSCALL_ENTRY(syscall_se_channel_get_select_resp),
+	SYSCALL_ENTRY(syscall_se_channel_transmit),
+	SYSCALL_ENTRY(syscall_se_channel_close),
+	SYSCALL_ENTRY(syscall_cache_operation),
+};
+
+#ifdef TRACE_SYSCALLS
+static void trace_syscall(size_t num)
+{
+	if (num == TEE_SCN_RETURN || num > TEE_SCN_MAX)
+		return;
+	FMSG("syscall #%zu (%s)", num, tee_svc_syscall_table[num].name);
+}
+#else
+static void trace_syscall(size_t num __unused)
+{
+}
+#endif
+
+#ifdef ARM32
+static void get_scn_max_args(struct thread_svc_regs *regs, size_t *scn,
+		size_t *max_args)
+{
+	*scn = regs->r7;
+	*max_args = regs->r6;
+}
+#endif /*ARM32*/
+
+#ifdef ARM64
+static void get_scn_max_args(struct thread_svc_regs *regs, size_t *scn,
+		size_t *max_args)
+{
+	if (((regs->spsr >> SPSR_MODE_RW_SHIFT) & SPSR_MODE_RW_MASK) ==
+	     SPSR_MODE_RW_32) {
+		*scn = regs->x7;
+		*max_args = regs->x6;
+	} else {
+		*scn = regs->x8;
+		*max_args = 0;
+	}
+}
+#endif /*ARM64*/
+
+#ifdef ARM32
+static void set_svc_retval(struct thread_svc_regs *regs, uint32_t ret_val)
+{
+	regs->r0 = ret_val;
+}
+#endif /*ARM32*/
+
+#ifdef ARM64
+static void set_svc_retval(struct thread_svc_regs *regs, uint64_t ret_val)
+{
+	regs->x0 = ret_val;
+}
+#endif /*ARM64*/
+
+void tee_svc_handler(struct thread_svc_regs *regs)
+{
+	size_t scn;
+	size_t max_args;
+	syscall_t scf;
+
+	COMPILE_TIME_ASSERT(ARRAY_SIZE(tee_svc_syscall_table) ==
+				(TEE_SCN_MAX + 1));
+
+	thread_user_save_vfp();
+
+	/* TA has just entered kernel mode */
+	tee_ta_update_session_utime_suspend();
+
+	/* Restore IRQ which are disabled on exception entry */
+	thread_restore_irq();
+
+	get_scn_max_args(regs, &scn, &max_args);
+
+	trace_syscall(scn);
+
+	if (max_args > TEE_SVC_MAX_ARGS) {
+		DMSG("Too many arguments for SCN %zu (%zu)", scn, max_args);
+		set_svc_retval(regs, TEE_ERROR_GENERIC);
+		return;
+	}
+
+	if (scn > TEE_SCN_MAX)
+		scf = syscall_not_supported;
+	else
+		scf = tee_svc_syscall_table[scn].fn;
+
+	set_svc_retval(regs, tee_svc_do_call(regs, scf));
+
+	if (scn != TEE_SCN_RETURN) {
+		/* We're about to switch back to user mode */
+		tee_ta_update_session_utime_resume();
+	}
+}
+
+#ifdef ARM32
+uint32_t tee_svc_sys_return_helper(uint32_t ret, bool panic,
+			uint32_t panic_code, struct thread_svc_regs *regs)
+{
+	if (panic) {
+		TAMSG("TA panicked with code 0x%x usr_sp 0x%x usr_lr 0x%x",
+		      panic_code, read_mode_sp(CPSR_MODE_SYS),
+		      read_mode_lr(CPSR_MODE_SYS));
+	}
+	regs->r1 = panic;
+	regs->r2 = panic_code;
+	regs->lr = (uintptr_t)thread_unwind_user_mode;
+	regs->spsr = read_cpsr();
+	return ret;
+}
+#endif /*ARM32*/
+#ifdef ARM64
+uint32_t tee_svc_sys_return_helper(uint32_t ret, bool panic,
+			uint32_t panic_code, struct thread_svc_regs *regs)
+{
+	if (panic) {
+		TAMSG("TA panicked with code 0x%x usr_sp 0x%" PRIx64 " usr_lr 0x%" PRIx64,
+			panic_code, regs->x13, regs->x14);
+	}
+	regs->x1 = panic;
+	regs->x2 = panic_code;
+	regs->elr = (uintptr_t)thread_unwind_user_mode;
+	regs->spsr = SPSR_64(SPSR_64_MODE_EL1, SPSR_64_MODE_SP_EL0, 0);
+	regs->spsr |= read_daif();
+	/*
+	 * Regs is the value of stack pointer before calling the SVC
+	 * handler.  By the addition matches for the reserved space at the
+	 * beginning of el0_sync_svc(). This prepares the stack when
+	 * returning to thread_unwind_user_mode instead of a normal
+	 * exception return.
+	 */
+	regs->sp_el0 = (uint64_t)(regs + 1);
+	return ret;
+}
+#endif /*ARM64*/
diff --git a/core/arch/arm/tee/arch_svc_a32.S b/core/arch/arm/tee/arch_svc_a32.S
new file mode 100644
index 0000000..d9c725c
--- /dev/null
+++ b/core/arch/arm/tee/arch_svc_a32.S
@@ -0,0 +1,122 @@
+/*
+ * Copyright (c) 2014, STMicroelectronics International N.V.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+#include "tee_syscall_numbers.h"
+#include "trace_levels.h"
+#include <asm.S>
+#include <arm.h>
+#include <tee_api_defines.h>
+#include <kernel/thread.h>
+#include <kernel/unwind.h>
+#include <asm-defines.h>
+
+	.section .text.arch_svc_asm
+
+/*
+ * uint32_t tee_svc_do_call(struct thread_svc_regs *regs, tee_svc_func func);
+ *
+ * Called from tee_svc_handler()
+ */
+FUNC tee_svc_do_call , :
+UNWIND(	.fnstart)
+UNWIND(	.cantunwind)
+	push	{r5-r9, lr}
+	mov	r7, sp
+	mov	r8, r0
+	mov	r9, r1
+	ldr	r5, [r8, #THREAD_SVC_REG_R5]
+	ldr	r6, [r8, #THREAD_SVC_REG_R6]
+
+	/*
+	 * Copy eventual arguments passed on the user stack.
+	 *
+	 * r5 holds the address of the first word
+	 * r6 holds the number of words
+	 *
+	 * tee_svc_handler() who calls this function has already checked
+	 * that we don't copy too much data.
+	 */
+	cmp     r6, #0
+	beq     .Lno_args
+	sub     sp, sp, r6, lsl #2
+	bic	sp, sp, #7	/* make sure it's a multiple of 8 */
+	mov     r0, sp
+	mov     r1, r5
+	mov     r2, r6, lsl #2
+	ldr     lr, =tee_svc_copy_from_user
+	blx     lr
+
+	/* If copy failed return the error */
+	cmp     r0, #0
+	bne     .Lret
+
+.Lno_args:
+	/* Load arguments to function */
+	add	lr, r8, #THREAD_SVC_REG_R0
+	ldm	lr, {r0-r3}
+	blx	r9
+.Lret:
+	mov	sp, r7
+	pop	{r5-r9, pc}
+UNWIND(	.fnend)
+END_FUNC tee_svc_do_call
+
+/*
+ * User space sees this function as:
+ * void syscall_sys_return(uint32_t ret) __noreturn;
+ *
+ * But internally the function depends on being called from
+ * tee_svc_do_call() with pointer to the struct thread_svc_regs saved by
+ * thread_svc_handler() in r8. The argument ret is already in r0 so we
+ * don't touch that and let it propagate as return value of the called
+ * thread_unwind_user_mode().
+ */
+FUNC syscall_sys_return , :
+UNWIND(	.fnstart)
+	mov	r1, #0	/* panic = false */
+	mov	r2, #0	/* panic_code = 0 */
+	mov	r3, r8
+	b	tee_svc_sys_return_helper
+UNWIND(	.fnend)
+END_FUNC syscall_sys_return
+
+/*
+ * User space sees this function as:
+ * void syscall_panic(uint32_t code) __noreturn;
+ *
+ * But internally the function depends on being called from
+ * tee_svc_do_call() with pointer to the struct thread_svc_regs saved by
+ * thread_svc_handler() in r8.
+ */
+FUNC syscall_panic , :
+UNWIND(	.fnstart)
+	mov	r1, #1	/* panic = true */
+	mov	r2, r0	/* panic_code = 0 */
+	mov	r3, r8
+	ldr	r0, =TEE_ERROR_TARGET_DEAD
+	b	tee_svc_sys_return_helper
+UNWIND(	.fnend)
+END_FUNC syscall_panic
diff --git a/core/arch/arm/tee/arch_svc_a64.S b/core/arch/arm/tee/arch_svc_a64.S
new file mode 100644
index 0000000..f76c2eb
--- /dev/null
+++ b/core/arch/arm/tee/arch_svc_a64.S
@@ -0,0 +1,205 @@
+/*
+ * 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 "tee_syscall_numbers.h"
+#include "trace_levels.h"
+#include <asm.S>
+#include <arm64_macros.S>
+#include <arm64.h>
+#include <tee_api_defines.h>
+#include <kernel/thread.h>
+#include <asm-defines.h>
+
+	.section .text.arch_svc_asm
+
+#if 0
+struct sc_rec {
+	uint64_t x0;
+	uint64_t x1;
+	uint64_t x19;
+	uint64_t x30;
+}
+#endif
+#define SC_REC_X0		(8 * 0)
+#define SC_REC_X1		(8 * 1)
+#define SC_REC_X19		(8 * 2)
+#define SC_REC_X30		(8 * 3)
+#define SC_REC_SIZE		(SC_REC_X30 + 8)
+
+/*
+ * uint32_t tee_svc_do_call(struct thread_svc_regs *regs, tee_svc_func func);
+ *
+ * Called from tee_svc_handler()
+ */
+FUNC tee_svc_do_call , :
+	sub	sp, sp, #SC_REC_SIZE
+	stp	x0, x1, [sp, #SC_REC_X0]
+	stp	x19, x30, [sp, #SC_REC_X19]
+	mov	x19, sp
+
+	ldr	x2, [x0, #THREAD_SVC_REG_SPSR]
+	tst	x2, #(SPSR_MODE_RW_32 << SPSR_MODE_RW_SHIFT)
+	b.eq	.Lcall_a64
+
+	ldp	x5, x6, [x0, #THREAD_SVC_REG_X5]
+	cmp	x6, #0
+	b.eq	.Lno_args_a32
+
+	/*
+	 * Calculate required space on stack to copy Aarch32 arguments
+	 * and to transform them into Aarch64 arguments.
+	 * x6 = nargs_on_stack
+         * n64 = (nargs_on_stack - 4) * 8
+         * n32 = nargs_on_stack * 4
+         * sp -= ROUNDUP(MAX(n32, n64), 16)
+	 *
+	 */
+	/* n64 = (nargs_on_stack - 4) * 8 */
+	sub	x1, x6, #0x4
+	lsl	x1, x1, #3
+	/* n32 = nargs_on_stack * 4 */
+	lsl	x0, x6, #2
+	/* sp -= ROUNDUP(MAX(n32, n64), 16) */
+	cmp	x1, x0
+	csel	x0, x1, x0, ge
+	add	x0, x0, #0xf
+	and	x0, x0, #0xfffffffffffffff0
+	sub	sp, sp, x0
+
+	/*
+	 * Find location on stack where to copy the Aarch32 arguments
+	 * and do the copy.
+	 * tee_svc_copy_from_user(sp, x5, nargs_on_stack * 4)
+	 */
+	mov	x0, sp
+	mov	x1, x5
+	add	x2, xzr, x6, lsl #2
+	bl	tee_svc_copy_from_user
+	/* If copy failed return the error */
+	cmp	x0, #0
+	bne	.Lret
+
+	/*
+	 * Load arguments into w4..w7, we're loading junk into unused
+	 * registers, but it's quicker than trying to figure out how
+	 * many registers to load into.
+	 */
+	/* x0 = nargs_on_stack */
+	ldr	x0, [x19, #SC_REC_X0]
+	ldr	x0, [x0, #THREAD_SVC_REG_X6]
+	load_wregs sp, 0, 4, 7
+
+	/*
+	 * Convert remaining Aarch32 parameters passed on stack as Aarch64
+	 * parameters on stack.
+	 *
+	 * nargs_on_stack is initialized in x0 above
+	 * n64 = (nargs_on_stack - 4) * 8
+	 * if n64 < 0 goro .Lno_args
+	 * x0 = x2 = x19 - n64
+	 * x1 points to next argument
+	 * while (x2 != x19) {
+	 *	w3 = *x1
+	 *	x1 += 4
+	 *	*x2 = x3
+	 *	x2 += 8
+	 * }
+	 * sp = x0
+	 */
+	/* n64 = (nargs_on_stack - 4) * 8 */
+	subs	x2, x0, #0x4
+	b.le	.Lno_args_a32
+	lsl	x2, x2, #3
+	mov	x0, x2
+
+.Lcpy_to_stack:
+	ldr	w3, [x1], #4
+	str	x3, [x2], #8
+	cmp	x2, x19
+	b.ne	.Lcpy_to_stack
+	mov	sp, x0
+
+
+.Lno_args_a32: /* Load the first 4 arguments to function */
+	ldr	x9, [x19, #SC_REC_X0]
+	load_xregs x9, THREAD_SVC_REG_X0, 0, 3
+	mov	w0, w0
+	mov	w1, w1
+	mov	w2, w2
+	mov	w3, w3
+
+	/* Call the svc function */
+	ldr	x16, [x19, #SC_REC_X1]
+	blr	x16
+	b	.Lret
+
+.Lcall_a64: /* Load the first 8 arguments to function */
+	ldr	x9, [x19, #SC_REC_X0]
+	load_xregs x9, THREAD_SVC_REG_X0, 0, 8
+
+	/* Call the svc function */
+	ldr	x16, [x19, #SC_REC_X1]
+	blr	x16
+
+.Lret:
+	mov	sp, x19
+	ldp	x19, x30, [sp, #SC_REC_X19]
+	add	sp, sp, #SC_REC_SIZE
+	ret
+END_FUNC tee_svc_do_call
+
+/*
+ * User space sees this function as:
+ * void syscall_sys_return(uint32_t ret) __noreturn;
+ *
+ * But internally the function depends on being called from
+ * tee_svc_do_call() with pointer to the struct thread_svc_regs saved by
+ * thread_svc_handler() in r8. The argument ret is already in r0 so we
+ * don't touch that and let it propagate as return value of the called
+ * tee_svc_unwind_enter_user_mode().
+ */
+FUNC syscall_sys_return , :
+	mov	x1, #0  /* panic = false */
+	mov	x2, #0  /* panic_code = 0 */
+	ldr	x3, [x19, #SC_REC_X0]
+	b	tee_svc_sys_return_helper
+END_FUNC syscall_sys_return
+
+/*
+ * User space sees this function as:
+ * void syscall_panic(uint32_t code) __noreturn;
+ *
+ * But internally the function depends on being called from
+ * tee_svc_do_call() with pointer to the struct thread_svc_regs saved by
+ * thread_svc_handler() in r8.
+ */
+FUNC syscall_panic , :
+	mov	x1, #1  /* panic = true */
+	mov	x2, x0  /* code */
+	ldr	w0, =TEE_ERROR_TARGET_DEAD
+	ldr	x3, [x19, #SC_REC_X0]
+	b	tee_svc_sys_return_helper
+END_FUNC syscall_panic
diff --git a/core/arch/arm/tee/arch_svc_private.h b/core/arch/arm/tee/arch_svc_private.h
new file mode 100644
index 0000000..7b2ea94
--- /dev/null
+++ b/core/arch/arm/tee/arch_svc_private.h
@@ -0,0 +1,38 @@
+/*
+ * Copyright (c) 2014, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+#ifndef ARCH_SVC_PRIVATE_H
+#define ARCH_SVC_PRIVATE_H
+
+#include <tee_api_types.h>
+
+/* void argument but in reality it can be any number of arguments */
+typedef TEE_Result (*syscall_t)(void);
+
+/* Helper function for tee_svc_handler() */
+uint32_t tee_svc_do_call(struct thread_svc_regs *regs, syscall_t func);
+
+#endif /*ARCH_SVC_PRIVATE_H*/
diff --git a/core/arch/arm/tee/entry_fast.c b/core/arch/arm/tee/entry_fast.c
new file mode 100644
index 0000000..0e80dc8
--- /dev/null
+++ b/core/arch/arm/tee/entry_fast.c
@@ -0,0 +1,231 @@
+/*
+ * Copyright (c) 2015, Linaro Limited
+ * All rights reserved.
+ * Copyright (c) 2014, STMicroelectronics International N.V.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <tee/entry_fast.h>
+#include <optee_msg.h>
+#include <sm/optee_smc.h>
+#include <kernel/generic_boot.h>
+#include <kernel/tee_l2cc_mutex.h>
+#include <kernel/misc.h>
+#include <mm/core_mmu.h>
+
+static void tee_entry_get_shm_config(struct thread_smc_args *args)
+{
+	args->a0 = OPTEE_SMC_RETURN_OK;
+	args->a1 = default_nsec_shm_paddr;
+	args->a2 = default_nsec_shm_size;
+	/* Should this be TEESMC cache attributes instead? */
+	args->a3 = core_mmu_is_shm_cached();
+}
+
+static void tee_entry_fastcall_l2cc_mutex(struct thread_smc_args *args)
+{
+	TEE_Result ret;
+#ifdef ARM32
+	paddr_t pa = 0;
+
+	switch (args->a1) {
+	case OPTEE_SMC_L2CC_MUTEX_GET_ADDR:
+		ret = tee_get_l2cc_mutex(&pa);
+		reg_pair_from_64(pa, &args->a2, &args->a3);
+		break;
+	case OPTEE_SMC_L2CC_MUTEX_SET_ADDR:
+		pa = reg_pair_to_64(args->a2, args->a3);
+		ret = tee_set_l2cc_mutex(&pa);
+		break;
+	case OPTEE_SMC_L2CC_MUTEX_ENABLE:
+		ret = tee_enable_l2cc_mutex();
+		break;
+	case OPTEE_SMC_L2CC_MUTEX_DISABLE:
+		ret = tee_disable_l2cc_mutex();
+		break;
+	default:
+		args->a0 = OPTEE_SMC_RETURN_EBADCMD;
+		return;
+	}
+#else
+	ret = TEE_ERROR_NOT_SUPPORTED;
+#endif
+	if (ret == TEE_ERROR_NOT_SUPPORTED)
+		args->a0 = OPTEE_SMC_RETURN_UNKNOWN_FUNCTION;
+	else if (ret)
+		args->a0 = OPTEE_SMC_RETURN_EBADADDR;
+	else
+		args->a0 = OPTEE_SMC_RETURN_OK;
+}
+
+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.
+		 */
+		args->a0 = OPTEE_SMC_RETURN_ENOTAVAIL;
+		return;
+	}
+
+	args->a0 = OPTEE_SMC_RETURN_OK;
+	args->a1 = OPTEE_SMC_SEC_CAP_HAVE_RESERVED_SHM;
+}
+
+static void tee_entry_disable_shm_cache(struct thread_smc_args *args)
+{
+	uint64_t cookie;
+
+	if (!thread_disable_prealloc_rpc_cache(&cookie)) {
+		args->a0 = OPTEE_SMC_RETURN_EBUSY;
+		return;
+	}
+
+	if (!cookie) {
+		args->a0 = OPTEE_SMC_RETURN_ENOTAVAIL;
+		return;
+	}
+
+	args->a0 = OPTEE_SMC_RETURN_OK;
+	args->a1 = cookie >> 32;
+	args->a2 = cookie;
+}
+
+static void tee_entry_enable_shm_cache(struct thread_smc_args *args)
+{
+	if (thread_enable_prealloc_rpc_cache())
+		args->a0 = OPTEE_SMC_RETURN_OK;
+	else
+		args->a0 = OPTEE_SMC_RETURN_EBUSY;
+}
+
+static void tee_entry_boot_secondary(struct thread_smc_args *args)
+{
+#if defined(CFG_BOOT_SECONDARY_REQUEST)
+	if (!generic_boot_core_release(args->a1, (paddr_t)(args->a3)))
+		args->a0 = OPTEE_SMC_RETURN_OK;
+	else
+		args->a0 = OPTEE_SMC_RETURN_EBADCMD;
+#else
+	args->a0 = OPTEE_SMC_RETURN_ENOTAVAIL;
+#endif
+}
+
+void tee_entry_fast(struct thread_smc_args *args)
+{
+	switch (args->a0) {
+
+	/* Generic functions */
+	case OPTEE_SMC_CALLS_COUNT:
+		tee_entry_get_api_call_count(args);
+		break;
+	case OPTEE_SMC_CALLS_UID:
+		tee_entry_get_api_uuid(args);
+		break;
+	case OPTEE_SMC_CALLS_REVISION:
+		tee_entry_get_api_revision(args);
+		break;
+	case OPTEE_SMC_CALL_GET_OS_UUID:
+		tee_entry_get_os_uuid(args);
+		break;
+	case OPTEE_SMC_CALL_GET_OS_REVISION:
+		tee_entry_get_os_revision(args);
+		break;
+
+	/* OP-TEE specific SMC functions */
+	case OPTEE_SMC_GET_SHM_CONFIG:
+		tee_entry_get_shm_config(args);
+		break;
+	case OPTEE_SMC_L2CC_MUTEX:
+		tee_entry_fastcall_l2cc_mutex(args);
+		break;
+	case OPTEE_SMC_EXCHANGE_CAPABILITIES:
+		tee_entry_exchange_capabilities(args);
+		break;
+	case OPTEE_SMC_DISABLE_SHM_CACHE:
+		tee_entry_disable_shm_cache(args);
+		break;
+	case OPTEE_SMC_ENABLE_SHM_CACHE:
+		tee_entry_enable_shm_cache(args);
+		break;
+	case OPTEE_SMC_BOOT_SECONDARY:
+		tee_entry_boot_secondary(args);
+		break;
+
+	default:
+		args->a0 = OPTEE_SMC_RETURN_UNKNOWN_FUNCTION;
+		break;
+	}
+}
+
+size_t tee_entry_generic_get_api_call_count(void)
+{
+	/*
+	 * All the different calls handled in this file. If the specific
+	 * target has additional calls it will call this function and
+	 * add the number of calls the target has added.
+	 */
+	return 9;
+}
+
+void __weak tee_entry_get_api_call_count(struct thread_smc_args *args)
+{
+	args->a0 = tee_entry_generic_get_api_call_count();
+}
+
+void __weak tee_entry_get_api_uuid(struct thread_smc_args *args)
+{
+	args->a0 = OPTEE_MSG_UID_0;
+	args->a1 = OPTEE_MSG_UID_1;
+	args->a2 = OPTEE_MSG_UID_2;
+	args->a3 = OPTEE_MSG_UID_3;
+}
+
+void __weak tee_entry_get_api_revision(struct thread_smc_args *args)
+{
+	args->a0 = OPTEE_MSG_REVISION_MAJOR;
+	args->a1 = OPTEE_MSG_REVISION_MINOR;
+}
+
+void __weak tee_entry_get_os_uuid(struct thread_smc_args *args)
+{
+	args->a0 = OPTEE_MSG_OS_OPTEE_UUID_0;
+	args->a1 = OPTEE_MSG_OS_OPTEE_UUID_1;
+	args->a2 = OPTEE_MSG_OS_OPTEE_UUID_2;
+	args->a3 = OPTEE_MSG_OS_OPTEE_UUID_3;
+}
+
+void __weak tee_entry_get_os_revision(struct thread_smc_args *args)
+{
+	args->a0 = CFG_OPTEE_REVISION_MAJOR;
+	args->a1 = CFG_OPTEE_REVISION_MINOR;
+}
diff --git a/core/arch/arm/tee/entry_std.c b/core/arch/arm/tee/entry_std.c
new file mode 100644
index 0000000..29c3b74
--- /dev/null
+++ b/core/arch/arm/tee/entry_std.c
@@ -0,0 +1,386 @@
+/*
+ * Copyright (c) 2015-2016, Linaro Limited
+ * All rights reserved.
+ * Copyright (c) 2014, STMicroelectronics International N.V.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <assert.h>
+#include <compiler.h>
+#include <initcall.h>
+#include <kernel/panic.h>
+#include <kernel/tee_misc.h>
+#include <mm/core_memprot.h>
+#include <mm/core_mmu.h>
+#include <mm/mobj.h>
+#include <optee_msg.h>
+#include <sm/optee_smc.h>
+#include <string.h>
+#include <tee/entry_std.h>
+#include <tee/tee_cryp_utl.h>
+#include <tee/uuid.h>
+#include <util.h>
+
+#define SHM_CACHE_ATTRS	\
+	(uint32_t)(core_mmu_is_shm_cached() ?  OPTEE_SMC_SHM_CACHED : 0)
+
+/* Sessions opened from normal world */
+static struct tee_ta_session_head tee_open_sessions =
+TAILQ_HEAD_INITIALIZER(tee_open_sessions);
+
+static struct mobj *shm_mobj;
+
+static TEE_Result set_mem_param(const struct optee_msg_param *param,
+				struct param_mem *mem)
+{
+	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");
+
+	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;
+
+	mem->mobj = shm_mobj;
+	mem->offs = param->u.tmem.buf_ptr - b;
+	mem->size = param->u.tmem.size;
+	return TEE_SUCCESS;
+}
+
+static TEE_Result copy_in_params(const struct optee_msg_param *params,
+		uint32_t num_params, struct tee_ta_param *ta_param)
+{
+	TEE_Result res;
+	size_t n;
+	uint8_t pt[TEE_NUM_PARAMS];
+
+	if (num_params > TEE_NUM_PARAMS)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	memset(ta_param, 0, sizeof(*ta_param));
+
+	for (n = 0; n < num_params; n++) {
+		uint32_t attr;
+
+		if (params[n].attr & OPTEE_MSG_ATTR_META)
+			return TEE_ERROR_BAD_PARAMETERS;
+		if (params[n].attr & OPTEE_MSG_ATTR_FRAGMENT)
+			return TEE_ERROR_BAD_PARAMETERS;
+
+		attr = params[n].attr & OPTEE_MSG_ATTR_TYPE_MASK;
+
+		switch (attr) {
+		case OPTEE_MSG_ATTR_TYPE_NONE:
+			pt[n] = TEE_PARAM_TYPE_NONE;
+			memset(&ta_param->u[n], 0, sizeof(ta_param->u[n]));
+			break;
+		case OPTEE_MSG_ATTR_TYPE_VALUE_INPUT:
+		case OPTEE_MSG_ATTR_TYPE_VALUE_OUTPUT:
+		case OPTEE_MSG_ATTR_TYPE_VALUE_INOUT:
+			pt[n] = TEE_PARAM_TYPE_VALUE_INPUT + attr -
+				OPTEE_MSG_ATTR_TYPE_VALUE_INPUT;
+			ta_param->u[n].val.a = params[n].u.value.a;
+			ta_param->u[n].val.b = params[n].u.value.b;
+			break;
+		case OPTEE_MSG_ATTR_TYPE_TMEM_INPUT:
+		case OPTEE_MSG_ATTR_TYPE_TMEM_OUTPUT:
+		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);
+			if (res != TEE_SUCCESS)
+				return res;
+			break;
+		default:
+			return TEE_ERROR_BAD_PARAMETERS;
+		}
+	}
+
+	ta_param->types = TEE_PARAM_TYPES(pt[0], pt[1], pt[2], pt[3]);
+
+	return TEE_SUCCESS;
+}
+
+static void copy_out_param(struct tee_ta_param *ta_param, uint32_t num_params,
+			   struct optee_msg_param *params)
+{
+	size_t n;
+
+	for (n = 0; n < num_params; n++) {
+		switch (TEE_PARAM_TYPE_GET(ta_param->types, n)) {
+		case TEE_PARAM_TYPE_MEMREF_OUTPUT:
+		case TEE_PARAM_TYPE_MEMREF_INOUT:
+			params[n].u.tmem.size = ta_param->u[n].mem.size;
+			break;
+		case TEE_PARAM_TYPE_VALUE_OUTPUT:
+		case TEE_PARAM_TYPE_VALUE_INOUT:
+			params[n].u.value.a = ta_param->u[n].val.a;
+			params[n].u.value.b = ta_param->u[n].val.b;
+			break;
+		default:
+			break;
+		}
+	}
+}
+
+/*
+ * Extracts mandatory parameter for open session.
+ *
+ * Returns
+ * false : mandatory parameter wasn't found or malformatted
+ * true  : paramater found and OK
+ */
+static TEE_Result get_open_session_meta(size_t num_params,
+					struct optee_msg_param *params,
+					size_t *num_meta, TEE_UUID *uuid,
+					TEE_Identity *clnt_id)
+{
+	const uint32_t req_attr = OPTEE_MSG_ATTR_META |
+				  OPTEE_MSG_ATTR_TYPE_VALUE_INPUT;
+
+	if (num_params < 2)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	if (params[0].attr != req_attr || params[1].attr != req_attr)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	tee_uuid_from_octets(uuid, (void *)&params[0].u.value);
+	clnt_id->login = params[1].u.value.c;
+	switch (clnt_id->login) {
+	case TEE_LOGIN_PUBLIC:
+		memset(&clnt_id->uuid, 0, sizeof(clnt_id->uuid));
+		break;
+	case TEE_LOGIN_USER:
+	case TEE_LOGIN_GROUP:
+	case TEE_LOGIN_APPLICATION:
+	case TEE_LOGIN_APPLICATION_USER:
+	case TEE_LOGIN_APPLICATION_GROUP:
+		tee_uuid_from_octets(&clnt_id->uuid,
+				     (void *)&params[1].u.value);
+		break;
+	default:
+		return TEE_ERROR_BAD_PARAMETERS;
+	}
+
+	*num_meta = 2;
+	return TEE_SUCCESS;
+}
+
+static void entry_open_session(struct thread_smc_args *smc_args,
+			       struct optee_msg_arg *arg, uint32_t num_params)
+{
+	TEE_Result res;
+	struct optee_msg_param *params = OPTEE_MSG_GET_PARAMS(arg);
+	TEE_ErrorOrigin err_orig = TEE_ORIGIN_TEE;
+	struct tee_ta_session *s = NULL;
+	TEE_Identity clnt_id;
+	TEE_UUID uuid;
+	struct tee_ta_param param;
+	size_t num_meta;
+
+	res = get_open_session_meta(num_params, params, &num_meta, &uuid,
+				    &clnt_id);
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	res = copy_in_params(params + num_meta, num_params - num_meta, &param);
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	res = tee_ta_open_session(&err_orig, &s, &tee_open_sessions, &uuid,
+				  &clnt_id, TEE_TIMEOUT_INFINITE, &param);
+	if (res != TEE_SUCCESS)
+		s = NULL;
+	copy_out_param(&param, num_params - num_meta, params + num_meta);
+
+	/*
+	 * The occurrence of open/close session command is usually
+	 * un-predictable, using this property to increase randomness
+	 * of prng
+	 */
+	plat_prng_add_jitter_entropy();
+
+out:
+	if (s)
+		arg->session = (vaddr_t)s;
+	else
+		arg->session = 0;
+	arg->ret = res;
+	arg->ret_origin = err_orig;
+	smc_args->a0 = OPTEE_SMC_RETURN_OK;
+}
+
+static void entry_close_session(struct thread_smc_args *smc_args,
+			struct optee_msg_arg *arg, uint32_t num_params)
+{
+	TEE_Result res;
+	struct tee_ta_session *s;
+
+	if (num_params) {
+		res = TEE_ERROR_BAD_PARAMETERS;
+		goto out;
+	}
+
+	plat_prng_add_jitter_entropy();
+
+	s = (struct tee_ta_session *)(vaddr_t)arg->session;
+	res = tee_ta_close_session(s, &tee_open_sessions, NSAPP_IDENTITY);
+out:
+	arg->ret = res;
+	arg->ret_origin = TEE_ORIGIN_TEE;
+	smc_args->a0 = OPTEE_SMC_RETURN_OK;
+}
+
+static void entry_invoke_command(struct thread_smc_args *smc_args,
+				 struct optee_msg_arg *arg, uint32_t num_params)
+{
+	TEE_Result res;
+	struct optee_msg_param *params = OPTEE_MSG_GET_PARAMS(arg);
+	TEE_ErrorOrigin err_orig = TEE_ORIGIN_TEE;
+	struct tee_ta_session *s;
+	struct tee_ta_param param;
+
+	res = copy_in_params(params, num_params, &param);
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	s = tee_ta_get_session(arg->session, true, &tee_open_sessions);
+	if (!s) {
+		res = TEE_ERROR_BAD_PARAMETERS;
+		goto out;
+	}
+
+	res = tee_ta_invoke_command(&err_orig, s, NSAPP_IDENTITY,
+				    TEE_TIMEOUT_INFINITE, arg->func, &param);
+
+	tee_ta_put_session(s);
+
+	copy_out_param(&param, num_params, params);
+
+out:
+	arg->ret = res;
+	arg->ret_origin = err_orig;
+	smc_args->a0 = OPTEE_SMC_RETURN_OK;
+}
+
+static void entry_cancel(struct thread_smc_args *smc_args,
+			struct optee_msg_arg *arg, uint32_t num_params)
+{
+	TEE_Result res;
+	TEE_ErrorOrigin err_orig = TEE_ORIGIN_TEE;
+	struct tee_ta_session *s;
+
+	if (num_params) {
+		res = TEE_ERROR_BAD_PARAMETERS;
+		goto out;
+	}
+
+	s = tee_ta_get_session(arg->session, false, &tee_open_sessions);
+	if (!s) {
+		res = TEE_ERROR_BAD_PARAMETERS;
+		goto out;
+	}
+
+	res = tee_ta_cancel_command(&err_orig, s, NSAPP_IDENTITY);
+	tee_ta_put_session(s);
+
+out:
+	arg->ret = res;
+	arg->ret_origin = err_orig;
+	smc_args->a0 = OPTEE_SMC_RETURN_OK;
+}
+
+void tee_entry_std(struct thread_smc_args *smc_args)
+{
+	paddr_t parg;
+	struct optee_msg_arg *arg = NULL;	/* fix gcc warning */
+	uint32_t num_params;
+
+	if (smc_args->a0 != OPTEE_SMC_CALL_WITH_ARG) {
+		EMSG("Unknown SMC 0x%" PRIx64, (uint64_t)smc_args->a0);
+		DMSG("Expected 0x%x\n", OPTEE_SMC_CALL_WITH_ARG);
+		smc_args->a0 = OPTEE_SMC_RETURN_EBADCMD;
+		return;
+	}
+	parg = (uint64_t)smc_args->a1 << 32 | smc_args->a2;
+	if (!tee_pbuf_is_non_sec(parg, sizeof(struct optee_msg_arg)) ||
+	    !ALIGNMENT_IS_OK(parg, struct optee_msg_arg) ||
+	    !(arg = phys_to_virt(parg, MEM_AREA_NSEC_SHM))) {
+		EMSG("Bad arg address 0x%" PRIxPA, parg);
+		smc_args->a0 = OPTEE_SMC_RETURN_EBADADDR;
+		return;
+	}
+
+	num_params = arg->num_params;
+	if (!tee_pbuf_is_non_sec(parg, OPTEE_MSG_GET_ARG_SIZE(num_params))) {
+		EMSG("Bad arg address 0x%" PRIxPA, parg);
+		smc_args->a0 = OPTEE_SMC_RETURN_EBADADDR;
+		return;
+	}
+
+	thread_set_irq(true);	/* Enable IRQ for STD calls */
+	switch (arg->cmd) {
+	case OPTEE_MSG_CMD_OPEN_SESSION:
+		entry_open_session(smc_args, arg, num_params);
+		break;
+	case OPTEE_MSG_CMD_CLOSE_SESSION:
+		entry_close_session(smc_args, arg, num_params);
+		break;
+	case OPTEE_MSG_CMD_INVOKE_COMMAND:
+		entry_invoke_command(smc_args, arg, num_params);
+		break;
+	case OPTEE_MSG_CMD_CANCEL:
+		entry_cancel(smc_args, arg, num_params);
+		break;
+	default:
+		EMSG("Unknown cmd 0x%x\n", arg->cmd);
+		smc_args->a0 = OPTEE_SMC_RETURN_EBADCMD;
+	}
+}
+
+static TEE_Result default_mobj_init(void)
+{
+	shm_mobj = mobj_phys_alloc(default_nsec_shm_paddr,
+				   default_nsec_shm_size, SHM_CACHE_ATTRS,
+				   CORE_MEM_NSEC_SHM);
+	if (!shm_mobj)
+		panic("Failed to register shared memory");
+
+	mobj_sec_ddr = mobj_phys_alloc(tee_mm_sec_ddr.lo,
+				       tee_mm_sec_ddr.hi - tee_mm_sec_ddr.lo,
+				       SHM_CACHE_ATTRS, CORE_MEM_TA_RAM);
+	if (!mobj_sec_ddr)
+		panic("Failed to register secure ta ram");
+
+	return TEE_SUCCESS;
+}
+
+driver_init_late(default_mobj_init);
diff --git a/core/arch/arm/tee/init.c b/core/arch/arm/tee/init.c
new file mode 100644
index 0000000..66d2a2b
--- /dev/null
+++ b/core/arch/arm/tee/init.c
@@ -0,0 +1,84 @@
+/*
+ * 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 <initcall.h>
+#include <malloc.h>		/* required for inits */
+
+#include <sm/tee_mon.h>
+#include <kernel/tee_misc.h>
+#include <mm/core_memprot.h>
+#include <trace.h>
+#include <kernel/time_source.h>
+#include <kernel/generic_boot.h>
+#include <mm/tee_mmu.h>
+#include <tee/tee_fs.h>
+#include <tee/tee_cryp_provider.h>
+#include <tee/tee_svc.h>
+#include <platform_config.h>
+
+
+#define TEE_MON_MAX_NUM_ARGS    8
+
+static void call_initcalls(void)
+{
+	initcall_t *call;
+
+	for (call = &__initcall_start; call < &__initcall_end; call++) {
+		TEE_Result ret;
+		ret = (*call)();
+		if (ret != TEE_SUCCESS) {
+			EMSG("Initial call 0x%08" PRIxVA " failed",
+			     (vaddr_t)call);
+		}
+	}
+}
+
+TEE_Result init_teecore(void)
+{
+	static int is_first = 1;
+
+	/* (DEBUG) for inits at 1st TEE service: when UART is setup */
+	if (!is_first)
+		return TEE_SUCCESS;
+	is_first = 0;
+
+#ifdef CFG_WITH_USER_TA
+	tee_svc_uref_base = CFG_TEE_LOAD_ADDR;
+#endif
+
+	/* init support for future mapping of TAs */
+	teecore_init_pub_ram();
+
+	/* time initialization */
+	time_source_init();
+
+	/* call pre-define initcall routines */
+	call_initcalls();
+
+	IMSG("Initialized");
+	return TEE_SUCCESS;
+}
diff --git a/core/arch/arm/tee/pta_socket.c b/core/arch/arm/tee/pta_socket.c
new file mode 100644
index 0000000..d696773
--- /dev/null
+++ b/core/arch/arm/tee/pta_socket.c
@@ -0,0 +1,320 @@
+/*
+ * Copyright (c) 2016-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 <kernel/pseudo_ta.h>
+#include <optee_msg.h>
+#include <optee_msg_supplicant.h>
+#include <pta_socket.h>
+#include <string.h>
+#include <tee/tee_fs_rpc.h>
+
+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,
+			      TEE_Param params[TEE_NUM_PARAMS])
+{
+	TEE_Result res;
+	paddr_t pa;
+	uint64_t cookie;
+	void *va;
+	struct optee_msg_param msg_params[4];
+	uint32_t exp_pt = TEE_PARAM_TYPES(TEE_PARAM_TYPE_VALUE_INPUT,
+					  TEE_PARAM_TYPE_MEMREF_INPUT,
+					  TEE_PARAM_TYPE_VALUE_INPUT,
+					  TEE_PARAM_TYPE_VALUE_OUTPUT);
+
+	if (exp_pt != param_types) {
+		DMSG("got param_types 0x%x, expected 0x%x",
+		     param_types, exp_pt);
+		return TEE_ERROR_BAD_PARAMETERS;
+	}
+
+	memset(msg_params, 0, sizeof(msg_params));
+
+	va = tee_fs_rpc_cache_alloc(params[1].memref.size, &pa, &cookie);
+	if (!va)
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	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[1].attr = OPTEE_MSG_ATTR_TYPE_VALUE_INPUT;
+	msg_params[1].u.value.a = params[0].value.b; /* server port number */
+	msg_params[1].u.value.b = params[2].value.a; /* protocol */
+	msg_params[1].u.value.c = params[0].value.a; /* ip version */
+
+	/* server address */
+	msg_params[2].attr = OPTEE_MSG_ATTR_TYPE_TMEM_INPUT;
+	msg_params[2].u.tmem.buf_ptr = pa;
+	msg_params[2].u.tmem.size = params[1].memref.size;
+	msg_params[2].u.tmem.shm_ref = cookie;
+	memcpy(va, params[1].memref.buffer, params[1].memref.size);
+
+	/* socket handle */
+	msg_params[3].attr = OPTEE_MSG_ATTR_TYPE_VALUE_OUTPUT;
+
+	res = thread_rpc_cmd(OPTEE_MSG_RPC_CMD_SOCKET, 4, msg_params);
+
+	if (res == TEE_SUCCESS)
+		params[3].value.a = msg_params[3].u.value.a;
+
+	return res;
+}
+
+static TEE_Result socket_close(struct tee_ta_session *sess,
+			       uint32_t param_types,
+			       TEE_Param params[TEE_NUM_PARAMS])
+{
+	struct optee_msg_param msg_params[1];
+	uint32_t exp_pt = TEE_PARAM_TYPES(TEE_PARAM_TYPE_VALUE_INPUT,
+					  TEE_PARAM_TYPE_NONE,
+					  TEE_PARAM_TYPE_NONE,
+					  TEE_PARAM_TYPE_NONE);
+
+	if (exp_pt != param_types) {
+		DMSG("got param_types 0x%x, expected 0x%x",
+		     param_types, exp_pt);
+		return TEE_ERROR_BAD_PARAMETERS;
+	}
+
+	memset(msg_params, 0, sizeof(msg_params));
+
+	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.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,
+			      TEE_Param params[TEE_NUM_PARAMS])
+{
+	TEE_Result res;
+	paddr_t pa;
+	uint64_t cookie;
+	void *va;
+	struct optee_msg_param msg_params[3];
+	uint32_t exp_pt = TEE_PARAM_TYPES(TEE_PARAM_TYPE_VALUE_INPUT,
+					  TEE_PARAM_TYPE_MEMREF_INPUT,
+					  TEE_PARAM_TYPE_VALUE_OUTPUT,
+					  TEE_PARAM_TYPE_NONE);
+
+	if (exp_pt != param_types) {
+		DMSG("got param_types 0x%x, expected 0x%x",
+		     param_types, exp_pt);
+		return TEE_ERROR_BAD_PARAMETERS;
+	}
+
+	memset(msg_params, 0, sizeof(msg_params));
+
+	va = tee_fs_rpc_cache_alloc(params[1].memref.size, &pa, &cookie);
+	if (!va)
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	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.c = params[0].value.a; /* handle */
+
+	/* buffer */
+	msg_params[1].attr = OPTEE_MSG_ATTR_TYPE_TMEM_INPUT;
+	msg_params[1].u.tmem.buf_ptr = pa;
+	msg_params[1].u.tmem.size = params[1].memref.size;
+	msg_params[1].u.tmem.shm_ref = cookie;
+	memcpy(va, params[1].memref.buffer, params[1].memref.size);
+
+	msg_params[2].attr = OPTEE_MSG_ATTR_TYPE_VALUE_INOUT;
+	msg_params[2].u.value.a = params[0].value.b /* timeout */;
+
+
+	res = thread_rpc_cmd(OPTEE_MSG_RPC_CMD_SOCKET, 3, msg_params);
+	params[2].value.a = msg_params[2].u.value.b; /* transmitted bytes */
+	return res;
+}
+
+static TEE_Result socket_recv(struct tee_ta_session *sess, uint32_t param_types,
+			      TEE_Param params[TEE_NUM_PARAMS])
+{
+	TEE_Result res;
+	paddr_t pa;
+	uint64_t cookie;
+	void *va;
+	struct optee_msg_param msg_params[3];
+	uint32_t exp_pt = TEE_PARAM_TYPES(TEE_PARAM_TYPE_VALUE_INPUT,
+					  TEE_PARAM_TYPE_MEMREF_OUTPUT,
+					  TEE_PARAM_TYPE_NONE,
+					  TEE_PARAM_TYPE_NONE);
+
+	if (exp_pt != param_types) {
+		DMSG("got param_types 0x%x, expected 0x%x",
+		     param_types, exp_pt);
+		return TEE_ERROR_BAD_PARAMETERS;
+	}
+
+	memset(msg_params, 0, sizeof(msg_params));
+
+	va = tee_fs_rpc_cache_alloc(params[1].memref.size, &pa, &cookie);
+	if (!va)
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	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.c = params[0].value.a; /* handle */
+
+	/* buffer */
+	msg_params[1].attr = OPTEE_MSG_ATTR_TYPE_TMEM_OUTPUT;
+	msg_params[1].u.tmem.buf_ptr = pa;
+	msg_params[1].u.tmem.size = params[1].memref.size;
+	msg_params[1].u.tmem.shm_ref = cookie;
+
+	msg_params[2].attr = OPTEE_MSG_ATTR_TYPE_VALUE_INPUT;
+	msg_params[2].u.value.a = params[0].value.b /* timeout */;
+
+
+	res = thread_rpc_cmd(OPTEE_MSG_RPC_CMD_SOCKET, 3, msg_params);
+	params[1].memref.size = msg_params[1].u.tmem.size;
+	if (msg_params[1].u.tmem.size)
+		memcpy(params[1].memref.buffer, va, msg_params[1].u.tmem.size);
+	return res;
+}
+
+static TEE_Result socket_ioctl(struct tee_ta_session *sess,
+			       uint32_t param_types,
+			       TEE_Param params[TEE_NUM_PARAMS])
+{
+	TEE_Result res;
+	paddr_t pa;
+	uint64_t cookie;
+	void *va;
+	struct optee_msg_param msg_params[3];
+	uint32_t exp_pt = TEE_PARAM_TYPES(TEE_PARAM_TYPE_VALUE_INPUT,
+					  TEE_PARAM_TYPE_MEMREF_INOUT,
+					  TEE_PARAM_TYPE_NONE,
+					  TEE_PARAM_TYPE_NONE);
+
+	if (exp_pt != param_types) {
+		DMSG("got param_types 0x%x, expected 0x%x",
+		     param_types, exp_pt);
+		return TEE_ERROR_BAD_PARAMETERS;
+	}
+
+	memset(msg_params, 0, sizeof(msg_params));
+
+	va = tee_fs_rpc_cache_alloc(params[1].memref.size, &pa, &cookie);
+	if (!va)
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	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.c = params[0].value.a; /* handle */
+
+	/* buffer */
+	msg_params[1].attr = OPTEE_MSG_ATTR_TYPE_TMEM_INOUT;
+	msg_params[1].u.tmem.buf_ptr = pa;
+	msg_params[1].u.tmem.size = params[1].memref.size;
+	msg_params[1].u.tmem.shm_ref = cookie;
+	memcpy(va, params[1].memref.buffer, params[1].memref.size);
+
+	msg_params[2].attr = OPTEE_MSG_ATTR_TYPE_VALUE_INPUT;
+	msg_params[2].u.value.a = params[0].value.b; /* ioctl command */
+
+	res = thread_rpc_cmd(OPTEE_MSG_RPC_CMD_SOCKET, 3, msg_params);
+	if (msg_params[1].u.tmem.size <= params[1].memref.size)
+		memcpy(params[1].memref.buffer, va, msg_params[1].u.tmem.size);
+	params[1].memref.size = msg_params[1].u.tmem.size;
+	return res;
+}
+
+typedef TEE_Result (*ta_func)(struct tee_ta_session *sess, uint32_t param_types,
+			      TEE_Param params[TEE_NUM_PARAMS]);
+
+static const ta_func ta_funcs[] = {
+	[PTA_SOCKET_OPEN] = socket_open,
+	[PTA_SOCKET_CLOSE] = socket_close,
+	[PTA_SOCKET_SEND] = socket_send,
+	[PTA_SOCKET_RECV] = socket_recv,
+	[PTA_SOCKET_IOCTL] = socket_ioctl,
+};
+
+/*
+ * Trusted Application Entry Points
+ */
+
+static TEE_Result pta_socket_open_session(uint32_t param_types __unused,
+			TEE_Param pParams[TEE_NUM_PARAMS] __unused,
+			void **sess_ctx __unused)
+{
+	struct tee_ta_session *s;
+
+	/* Check that we're called from a TA */
+	s = tee_ta_get_calling_session();
+	if (!s)
+		return TEE_ERROR_ACCESS_DENIED;
+
+	*sess_ctx = s;
+
+	return TEE_SUCCESS;
+}
+
+static void pta_socket_close_session(void *sess_ctx)
+{
+	TEE_Result res;
+	struct optee_msg_param msg_params[1];
+
+	memset(msg_params, 0, sizeof(msg_params));
+
+	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);
+
+	res = thread_rpc_cmd(OPTEE_MSG_RPC_CMD_SOCKET, 1, msg_params);
+	if (res != TEE_SUCCESS)
+		DMSG("OPTEE_MRC_SOCKET_CLOSE_ALL failed: %#" PRIx32, res);
+}
+
+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 TEE_ERROR_NOT_IMPLEMENTED;
+}
+
+pseudo_ta_register(.uuid = PTA_SOCKET_UUID, .name = "socket",
+		   .flags = PTA_DEFAULT_FLAGS,
+		   .open_session_entry_point = pta_socket_open_session,
+		   .close_session_entry_point = pta_socket_close_session,
+		   .invoke_command_entry_point = pta_socket_invoke_command);
diff --git a/core/arch/arm/tee/sub.mk b/core/arch/arm/tee/sub.mk
new file mode 100644
index 0000000..0ee9f64
--- /dev/null
+++ b/core/arch/arm/tee/sub.mk
@@ -0,0 +1,12 @@
+ifeq ($(CFG_WITH_USER_TA),y)
+srcs-$(CFG_ARM32_core) += arch_svc_a32.S
+srcs-$(CFG_ARM64_core) += arch_svc_a64.S
+srcs-$(CFG_CACHE_API) += svc_cache.c
+srcs-y += arch_svc.c
+srcs-$(CFG_GP_SOCKETS) += pta_socket.c
+else
+srcs-y += svc_dummy.c
+endif
+srcs-y += entry_std.c
+srcs-y += entry_fast.c
+srcs-y += init.c
diff --git a/core/arch/arm/tee/svc_cache.c b/core/arch/arm/tee/svc_cache.c
new file mode 100644
index 0000000..88b89a9
--- /dev/null
+++ b/core/arch/arm/tee/svc_cache.c
@@ -0,0 +1,114 @@
+/*
+ * 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 <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"
+
+/*
+ * 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 ret;
+	paddr_t pa = 0;
+	struct user_ta_ctx *utc = to_user_ta_ctx(sess->ctx);
+
+	if ((sess->ctx->flags & TA_FLAG_CACHE_MAINTENANCE) == 0)
+		return TEE_ERROR_NOT_SUPPORTED;
+
+	/*
+	 * TAs are allowed to operate cache maintenance on TA memref parameters
+	 * only, not on the TA private memory.
+	 */
+	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 |
+					  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 cache_operation(s, op, va, len);
+}
diff --git a/core/arch/arm/tee/svc_cache.h b/core/arch/arm/tee/svc_cache.h
new file mode 100644
index 0000000..d5d4972
--- /dev/null
+++ b/core/arch/arm/tee/svc_cache.h
@@ -0,0 +1,39 @@
+/*
+ * 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.
+ */
+#ifndef SVC_CACHE_H
+#define SVC_CACHE_H
+
+#include <types_ext.h>
+#include <tee_api_types.h>
+
+#ifdef CFG_CACHE_API
+TEE_Result syscall_cache_operation(void *va, size_t len, unsigned long op);
+#else
+#define  syscall_cache_operation syscall_not_supported
+#endif
+
+#endif /*SVC_CACHE_H*/
diff --git a/core/arch/arm/tee/svc_dummy.c b/core/arch/arm/tee/svc_dummy.c
new file mode 100644
index 0000000..4e8a924
--- /dev/null
+++ b/core/arch/arm/tee/svc_dummy.c
@@ -0,0 +1,35 @@
+/*
+ * 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/thread.h>
+#include <kernel/panic.h>
+#include <tee/arch_svc.h>
+
+void __noreturn tee_svc_handler(struct thread_svc_regs *regs __unused)
+{
+	/* "Can't happen" as we have no user space TAs */
+	panic();
+}
diff --git a/core/core.mk b/core/core.mk
new file mode 100644
index 0000000..075bc01
--- /dev/null
+++ b/core/core.mk
@@ -0,0 +1,108 @@
+include mk/cleanvars.mk
+
+# Set current submodule (used for module specific flags compile result etc)
+sm := core
+sm-$(sm) := y
+
+arch-dir	:= core/arch/$(ARCH)
+platform-dir	:= $(arch-dir)/plat-$(PLATFORM)
+include mk/checkconf.mk
+include $(platform-dir)/conf.mk
+include mk/config.mk
+include core/arch/$(ARCH)/$(ARCH).mk
+
+PLATFORM_$(PLATFORM) := y
+PLATFORM_FLAVOR_$(PLATFORM_FLAVOR) := y
+
+$(call cfg-depends-all,CFG_PAGED_USER_TA,CFG_WITH_PAGER \
+	CFG_SMALL_PAGE_USER_TA CFG_WITH_USER_TA)
+
+# Setup compiler for this sub module
+COMPILER_$(sm)		?= $(COMPILER)
+include mk/$(COMPILER_$(sm)).mk
+
+cppflags$(sm)	+= -D__KERNEL__
+
+cppflags$(sm)	+= -Icore/include
+cppflags$(sm)	+= -include $(conf-file)
+cppflags$(sm)	+= -I$(out-dir)/core/include/generated
+cppflags$(sm)	+= $(core-platform-cppflags)
+cflags$(sm)	+= $(core-platform-cflags)
+ifeq ($(CFG_CORE_SANITIZE_UNDEFINED),y)
+cflags$(sm)	+= -fsanitize=undefined
+endif
+ifeq ($(CFG_CORE_SANITIZE_KADDRESS),y)
+ifeq ($(CFG_ASAN_SHADOW_OFFSET),)
+$(error error: CFG_CORE_SANITIZE_KADDRESS not supported by platform (flavor))
+endif
+cflags_kasan	+= -fsanitize=kernel-address \
+		   -fasan-shadow-offset=$(CFG_ASAN_SHADOW_OFFSET)\
+		   --param asan-stack=1 --param asan-globals=1 \
+		   --param asan-instrumentation-with-call-threshold=0
+cflags$(sm)	+= $(cflags_kasan)
+endif
+aflags$(sm)	+= $(core-platform-aflags)
+
+cppflags$(sm) += -DTRACE_LEVEL=$(CFG_TEE_CORE_LOG_LEVEL)
+ifeq ($(CFG_TEE_CORE_MALLOC_DEBUG),y)
+cppflags$(sm) += -DENABLE_MDBG=1
+endif
+ifneq ($(CFG_TEE_CORE_DEBUG),y)
+cppflags$(sm)  += -DNDEBUG
+endif
+
+cppflags$(sm)	+= -Ilib/libutee/include
+
+# Tell all libraries and sub-directories (included below) that we have a
+# configuration file
+
+conf-file := $(out-dir)/include/generated/conf.h
+conf-mk-file := $(out-dir)/conf.mk
+$(conf-file): $(conf-mk-file)
+
+cleanfiles += $(conf-file)
+cleanfiles += $(conf-mk-file)
+
+$(conf-file): FORCE
+	$(call check-conf-h)
+
+$(conf-mk-file):  FORCE
+	$(call check-conf-mk)
+
+#
+# Do libraries
+#
+
+# Set a prefix to avoid conflicts with user TAs that will use the same
+# source but with different flags below
+base-prefix := $(sm)-
+libname = utils
+libdir = lib/libutils
+include mk/lib.mk
+
+libname = mpa
+libdir = lib/libmpa
+include mk/lib.mk
+base-prefix :=
+
+libname = tomcrypt
+libdir = core/lib/libtomcrypt
+include mk/lib.mk
+
+ifeq ($(CFG_DT),y)
+libname = fdt
+libdir = core/lib/libfdt
+include mk/lib.mk
+endif
+
+#
+# Do main source
+#
+
+subdirs = $(core-platform-subdirs) core
+include mk/subdir.mk
+
+asm-defines-file := core/arch/$(ARCH)/kernel/asm-defines.c
+include mk/compile.mk
+
+include $(platform-dir)/link.mk
diff --git a/core/drivers/cdns_uart.c b/core/drivers/cdns_uart.c
new file mode 100644
index 0000000..3b4e4b9
--- /dev/null
+++ b/core/drivers/cdns_uart.c
@@ -0,0 +1,100 @@
+/*
+ * Copyright (c) 2016, Xilinx Inc.
+ * 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 <compiler.h>
+#include <drivers/cdns_uart.h>
+#include <io.h>
+#include <util.h>
+
+#define CDNS_UART_CONTROL		0
+#define CDNS_UART_MODE			4
+#define CDNS_UART_IEN			8
+#define CDNS_UART_IRQ_STATUS		0x14
+#define CDNS_UART_CHANNEL_STATUS	0x2c
+#define CDNS_UART_FIFO			0x30
+
+#define CDNS_UART_CONTROL_RXRES		BIT(0)
+#define CDNS_UART_CONTROL_TXRES		BIT(1)
+#define CDNS_UART_CONTROL_RXEN		BIT(2)
+#define CDNS_UART_CONTROL_TXEN		BIT(4)
+
+#define CDNS_UART_MODE_8BIT		(0 << 1)
+#define CDNS_UART_MODE_PARITY_NONE	(0x4 << 3)
+#define CDNS_UART_MODE_1STP		(0 << 6)
+
+#define CDNS_UART_CHANNEL_STATUS_TFUL	BIT(4)
+#define CDNS_UART_CHANNEL_STATUS_TEMPTY	BIT(3)
+#define CDNS_UART_CHANNEL_STATUS_REMPTY	BIT(1)
+
+#define CDNS_UART_IRQ_RXTRIG		BIT(0)
+#define CDNS_UART_IRQ_RXTOUT		BIT(8)
+
+void cdns_uart_flush(vaddr_t base)
+{
+	while (!(read32(base + CDNS_UART_CHANNEL_STATUS) &
+				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)
+{
+	if (!base || !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(base);
+}
+
+void cdns_uart_putc(int ch, vaddr_t base)
+{
+	/* Wait until there is space in the FIFO */
+	while (read32(base + CDNS_UART_CHANNEL_STATUS) &
+			CDNS_UART_CHANNEL_STATUS_TFUL)
+		;
+
+	/* Send the character */
+	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)
+{
+	while (!cdns_uart_have_rx_data(base))
+		;
+	return read32(base + CDNS_UART_FIFO) & 0xff;
+}
diff --git a/core/drivers/dra7_rng.c b/core/drivers/dra7_rng.c
new file mode 100644
index 0000000..f9238f8
--- /dev/null
+++ b/core/drivers/dra7_rng.c
@@ -0,0 +1,204 @@
+/*
+ * Copyright (c) 2016, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <initcall.h>
+#include <io.h>
+#include <keep.h>
+#include <kernel/interrupt.h>
+#include <kernel/misc.h>
+#include <kernel/spinlock.h>
+#include <mm/core_memprot.h>
+#include <mm/core_mmu.h>
+#include <platform_config.h>
+#include <rng_support.h>
+
+#define	RNG_OUTPUT_L            0x0000
+#define	RNG_OUTPUT_H            0x0004
+#define	RNG_STATUS              0x0008
+#  define RNG_READY             BIT(0)
+#  define SHUTDOWN_OFLO         BIT(1)
+#define	RNG_INTMASK             0x000C
+#define	RNG_INTACK              0x0010
+#define	RNG_CONTROL             0x0014
+#  define ENABLE_TRNG           BIT(10)
+#define	RNG_CONFIG              0x0018
+#define	RNG_ALARMCNT            0x001C
+#define	RNG_FROENABLE           0x0020
+#define	RNG_FRODETUNE           0x0024
+#define	RNG_ALARMMASK           0x0028
+#define	RNG_ALARMSTOP           0x002C
+#define	RNG_LFSR_L              0x0030
+#define	RNG_LFSR_M              0x0034
+#define	RNG_LFSR_H              0x0038
+#define	RNG_COUNT               0x003C
+#define	RNG_OPTIONS             0x0078
+#define	RNG_EIP_REV             0x007C
+#define	RNG_MMR_STATUS_EN       0x1FD8
+#define	RNG_REV                 0x1FE0
+#define	RNG_SYS_CONFIG_REG      0x1FE4
+#  define RNG_AUTOIDLE          BIT(0)
+#define	RNG_MMR_STATUS_SET      0x1FEC
+#define	RNG_SOFT_RESET_REG      0x1FF0
+#  define RNG_SOFT_RESET        BIT(0)
+#define	RNG_IRQ_EOI_REG         0x1FF4
+#define	RNG_IRQSTATUS           0x1FF8
+
+#define RNG_CONTROL_STARTUP_CYCLES_SHIFT        16
+#define RNG_CONTROL_STARTUP_CYCLES_MASK         GENMASK_32(31, 16)
+
+#define RNG_CONFIG_MAX_REFIL_CYCLES_SHIFT       16
+#define RNG_CONFIG_MAX_REFIL_CYCLES_MASK        GENMASK_32(31, 16)
+#define RNG_CONFIG_MIN_REFIL_CYCLES_SHIFT       0
+#define RNG_CONFIG_MIN_REFIL_CYCLES_MASK        GENMASK_32(7, 0)
+
+#define RNG_ALARMCNT_ALARM_TH_SHIFT             0
+#define RNG_ALARMCNT_ALARM_TH_MASK              GENMASK_32(7, 0)
+#define RNG_ALARMCNT_SHUTDOWN_TH_SHIFT          16
+#define RNG_ALARMCNT_SHUTDOWN_TH_MASK           GENMASK_32(20, 16)
+
+#define RNG_CONTROL_STARTUP_CYCLES              0xff
+#define RNG_CONFIG_MIN_REFIL_CYCLES             0x21
+#define RNG_CONFIG_MAX_REFIL_CYCLES             0x22
+#define RNG_ALARM_THRESHOLD                     0xff
+#define RNG_SHUTDOWN_THRESHOLD                  0x4
+
+#define RNG_FRO_MASK    GENMASK_32(23, 0)
+
+#define RNG_REG_SIZE    0x2000
+
+register_phys_mem(MEM_AREA_IO_SEC, RNG_BASE, RNG_REG_SIZE);
+
+static unsigned int rng_lock = SPINLOCK_UNLOCK;
+
+uint8_t hw_get_random_byte(void)
+{
+	static int pos;
+	static union {
+		uint32_t val[2];
+		uint8_t byte[8];
+	} random;
+	vaddr_t rng = (vaddr_t)phys_to_virt(RNG_BASE, MEM_AREA_IO_SEC);
+	uint8_t ret;
+
+	uint32_t exceptions = thread_mask_exceptions(THREAD_EXCP_ALL);
+	cpu_spin_lock(&rng_lock);
+
+	if (!pos) {
+		/* Is the result ready (available)? */
+		while (!(read32(rng + RNG_STATUS) & RNG_READY)) {
+			/* Is the shutdown threshold reached? */
+			if (read32(rng + RNG_STATUS) & SHUTDOWN_OFLO) {
+				uint32_t alarm = read32(rng + RNG_ALARMSTOP);
+				uint32_t tuning = read32(rng + RNG_FRODETUNE);
+				/* Clear the alarm events */
+				write32(0x0, rng + RNG_ALARMMASK);
+				write32(0x0, rng + RNG_ALARMSTOP);
+				/* De-tune offending FROs */
+				write32(tuning ^ alarm, rng + RNG_FRODETUNE);
+				/* Re-enable the shut down FROs */
+				write32(RNG_FRO_MASK, rng + RNG_FROENABLE);
+				/* Clear the shutdown overflow event */
+				write32(SHUTDOWN_OFLO, rng + RNG_INTACK);
+
+				DMSG("Fixed FRO shutdown\n");
+			}
+		}
+		/* Read random value */
+		random.val[0] = read32(rng + RNG_OUTPUT_L);
+		random.val[1] = read32(rng + RNG_OUTPUT_H);
+		/* Acknowledge read complete */
+		write32(RNG_READY, rng + RNG_INTACK);
+	}
+
+	ret = random.byte[pos];
+
+	pos = (pos + 1) % 8;
+
+	cpu_spin_unlock(&rng_lock);
+	thread_set_exceptions(exceptions);
+
+	return ret;
+}
+
+static TEE_Result dra7_rng_init(void)
+{
+	vaddr_t rng = (vaddr_t)phys_to_virt(RNG_BASE, MEM_AREA_IO_SEC);
+	uint32_t val;
+
+	/* Execute a software reset */
+	write32(RNG_SOFT_RESET, rng + RNG_SOFT_RESET_REG);
+
+	/* Wait for the software reset completion by polling */
+	while (read32(rng + RNG_SOFT_RESET_REG) & RNG_SOFT_RESET)
+		;
+
+	/* Switch to low-power operating mode */
+	write32(RNG_AUTOIDLE, rng + RNG_SYS_CONFIG_REG);
+
+	/*
+	 * Select the number of clock input cycles to the
+	 * FROs between two samples
+	 */
+	val = 0;
+
+	/* Ensure initial latency */
+	val |= RNG_CONFIG_MIN_REFIL_CYCLES <<
+			RNG_CONFIG_MIN_REFIL_CYCLES_SHIFT;
+	val |= RNG_CONFIG_MAX_REFIL_CYCLES <<
+			RNG_CONFIG_MAX_REFIL_CYCLES_SHIFT;
+	write32(val, rng + RNG_CONFIG);
+
+	/* Configure the desired FROs */
+	write32(0x0, rng + RNG_FRODETUNE);
+
+	/* Enable all FROs */
+	write32(0xffffff, rng + RNG_FROENABLE);
+
+	/*
+	 * Select the maximum number of samples after
+	 * which if a repeating pattern is still detected, an
+	 * alarm event is generated
+	 */
+	val = RNG_ALARM_THRESHOLD << RNG_ALARMCNT_ALARM_TH_SHIFT;
+
+	/*
+	 * Set the shutdown threshold to the number of FROs
+	 * allowed to be shut downed
+	 */
+	val |= RNG_SHUTDOWN_THRESHOLD << RNG_ALARMCNT_SHUTDOWN_TH_SHIFT;
+	write32(val, rng + RNG_ALARMCNT);
+
+	/* Enable the RNG module */
+	val = RNG_CONTROL_STARTUP_CYCLES << RNG_CONTROL_STARTUP_CYCLES_SHIFT;
+	val |= ENABLE_TRNG;
+	write32(val, rng + RNG_CONTROL);
+
+	IMSG("DRA7x TRNG initialized");
+
+	return TEE_SUCCESS;
+}
+driver_init(dra7_rng_init);
diff --git a/core/drivers/frame_buffer.c b/core/drivers/frame_buffer.c
new file mode 100644
index 0000000..8ef9a4e
--- /dev/null
+++ b/core/drivers/frame_buffer.c
@@ -0,0 +1,59 @@
+/*
+ * Copyright (c) 2016, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <compiler.h>
+#include <drivers/frame_buffer.h>
+
+size_t frame_buffer_get_image_size(struct frame_buffer *fb __unused,
+			size_t width, size_t height)
+{
+	return width * height * sizeof(uint32_t);
+}
+
+void frame_buffer_clear(struct frame_buffer *fb, uint32_t color)
+{
+	size_t n;
+	uint32_t *base = fb->base;
+
+	for (n = 0; n < fb->width * fb->height; n++)
+		base[n] = color;
+}
+
+void frame_buffer_set_image(struct frame_buffer *fb, size_t xpos, size_t ypos,
+			size_t width, size_t height, const void *image)
+{
+	size_t x;
+	size_t y;
+	uint32_t *base = fb->base;
+	const uint32_t *img = image;
+
+	for (y = 0; y < height && (y + ypos) < fb->height; y++)
+		for (x = 0; x < width && (x + xpos) < fb->width; x++)
+			base[x + xpos + (y + ypos) * fb->width] =
+				img[x + y * width];
+}
+
diff --git a/core/drivers/gic.c b/core/drivers/gic.c
new file mode 100644
index 0000000..93979a0
--- /dev/null
+++ b/core/drivers/gic.c
@@ -0,0 +1,457 @@
+/*
+ * Copyright (c) 2016, Linaro Limited
+ * Copyright (c) 2014, STMicroelectronics International N.V.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <assert.h>
+#include <drivers/gic.h>
+#include <kernel/interrupt.h>
+#include <kernel/panic.h>
+#include <util.h>
+#include <io.h>
+#include <trace.h>
+
+/* Offsets from gic.gicc_base */
+#define GICC_CTLR		(0x000)
+#define GICC_PMR		(0x004)
+#define GICC_IAR		(0x00C)
+#define GICC_EOIR		(0x010)
+
+#define GICC_CTLR_ENABLEGRP0	(1 << 0)
+#define GICC_CTLR_ENABLEGRP1	(1 << 1)
+#define GICC_CTLR_FIQEN		(1 << 3)
+
+/* Offsets from gic.gicd_base */
+#define GICD_CTLR		(0x000)
+#define GICD_TYPER		(0x004)
+#define GICD_IGROUPR(n)		(0x080 + (n) * 4)
+#define GICD_ISENABLER(n)	(0x100 + (n) * 4)
+#define GICD_ICENABLER(n)	(0x180 + (n) * 4)
+#define GICD_ISPENDR(n)		(0x200 + (n) * 4)
+#define GICD_ICPENDR(n)		(0x280 + (n) * 4)
+#define GICD_IPRIORITYR(n)	(0x400 + (n) * 4)
+#define GICD_ITARGETSR(n)	(0x800 + (n) * 4)
+#define GICD_SGIR		(0xF00)
+
+#define GICD_CTLR_ENABLEGRP0	(1 << 0)
+#define GICD_CTLR_ENABLEGRP1	(1 << 1)
+
+/* Number of Private Peripheral Interrupt */
+#define NUM_PPI	32
+
+/* Number of Software Generated Interrupt */
+#define NUM_SGI			16
+
+/* Number of Non-secure Software Generated Interrupt */
+#define NUM_NS_SGI		8
+
+/* Number of interrupts in one register */
+#define NUM_INTS_PER_REG	32
+
+/* Number of targets in one register */
+#define NUM_TARGETS_PER_REG	4
+
+/* Accessors to access ITARGETSRn */
+#define ITARGETSR_FIELD_BITS	8
+#define ITARGETSR_FIELD_MASK	0xff
+
+/* Maximum number of interrups a GIC can support */
+#define GIC_MAX_INTS		1020
+
+#define GIC_SPURIOUS_ID		1023
+
+#define GICC_IAR_IT_ID_MASK	0x3ff
+#define GICC_IAR_CPU_ID_MASK	0x7
+#define GICC_IAR_CPU_ID_SHIFT	10
+
+static void gic_op_add(struct itr_chip *chip, size_t it, uint32_t flags);
+static void gic_op_enable(struct itr_chip *chip, size_t it);
+static void gic_op_disable(struct itr_chip *chip, size_t it);
+static void gic_op_raise_pi(struct itr_chip *chip, size_t it);
+static void gic_op_raise_sgi(struct itr_chip *chip, size_t it,
+			uint8_t cpu_mask);
+static void gic_op_set_affinity(struct itr_chip *chip, size_t it,
+			uint8_t cpu_mask);
+
+static const struct itr_ops gic_ops = {
+	.add = gic_op_add,
+	.enable = gic_op_enable,
+	.disable = gic_op_disable,
+	.raise_pi = gic_op_raise_pi,
+	.raise_sgi = gic_op_raise_sgi,
+	.set_affinity = gic_op_set_affinity,
+};
+
+static size_t probe_max_it(vaddr_t gicc_base, vaddr_t gicd_base)
+{
+	int i;
+	uint32_t old_ctlr;
+	size_t ret = 0;
+	const size_t max_regs = ((GIC_MAX_INTS + NUM_INTS_PER_REG - 1) /
+					NUM_INTS_PER_REG) - 1;
+
+	/*
+	 * Probe which interrupt number is the largest.
+	 */
+	old_ctlr = read32(gicc_base + GICC_CTLR);
+	write32(0, gicc_base + GICC_CTLR);
+	for (i = max_regs; i >= 0; i--) {
+		uint32_t old_reg;
+		uint32_t reg;
+		int b;
+
+		old_reg = read32(gicd_base + GICD_ISENABLER(i));
+		write32(0xffffffff, gicd_base + GICD_ISENABLER(i));
+		reg = read32(gicd_base + GICD_ISENABLER(i));
+		write32(old_reg, gicd_base + GICD_ICENABLER(i));
+		for (b = NUM_INTS_PER_REG - 1; b >= 0; b--) {
+			if (BIT32(b) & reg) {
+				ret = i * NUM_INTS_PER_REG + b;
+				goto out;
+			}
+		}
+	}
+out:
+	write32(old_ctlr, gicc_base + GICC_CTLR);
+	return ret;
+}
+
+void gic_cpu_init(struct gic_data *gd)
+{
+	assert(gd->gicd_base && gd->gicc_base);
+
+	/* per-CPU interrupts config:
+	 * ID0-ID7(SGI)   for Non-secure interrupts
+	 * ID8-ID15(SGI)  for Secure interrupts.
+	 * All PPI config as Non-secure interrupts.
+	 */
+	write32(0xffff00ff, gd->gicd_base + GICD_IGROUPR(0));
+
+	/* Set the priority mask to permit Non-secure interrupts, and to
+	 * allow the Non-secure world to adjust the priority mask itself
+	 */
+	write32(0x80, gd->gicc_base + GICC_PMR);
+
+	/* Enable GIC */
+	write32(GICC_CTLR_ENABLEGRP0 | GICC_CTLR_ENABLEGRP1 | GICC_CTLR_FIQEN,
+		gd->gicc_base + GICC_CTLR);
+}
+
+void gic_init(struct gic_data *gd, vaddr_t gicc_base, vaddr_t gicd_base)
+{
+	size_t n;
+
+	gic_init_base_addr(gd, gicc_base, gicd_base);
+
+	for (n = 0; n <= gd->max_it / NUM_INTS_PER_REG; n++) {
+		/* Disable interrupts */
+		write32(0xffffffff, gd->gicd_base + GICD_ICENABLER(n));
+
+		/* Make interrupts non-pending */
+		write32(0xffffffff, gd->gicd_base + GICD_ICPENDR(n));
+
+		/* Mark interrupts non-secure */
+		if (n == 0) {
+			/* per-CPU inerrupts config:
+                         * ID0-ID7(SGI)   for Non-secure interrupts
+                         * ID8-ID15(SGI)  for Secure interrupts.
+                         * All PPI config as Non-secure interrupts.
+			 */
+			write32(0xffff00ff, gd->gicd_base + GICD_IGROUPR(n));
+		} else {
+			write32(0xffffffff, gd->gicd_base + GICD_IGROUPR(n));
+		}
+	}
+
+	/* Set the priority mask to permit Non-secure interrupts, and to
+	 * allow the Non-secure world to adjust the priority mask itself
+	 */
+	write32(0x80, gd->gicc_base + GICC_PMR);
+
+	/* Enable GIC */
+	write32(GICC_CTLR_ENABLEGRP0 | GICC_CTLR_ENABLEGRP1 | GICC_CTLR_FIQEN,
+		gd->gicc_base + GICC_CTLR);
+	write32(GICD_CTLR_ENABLEGRP0 | GICD_CTLR_ENABLEGRP1,
+		gd->gicd_base + GICD_CTLR);
+}
+
+void gic_init_base_addr(struct gic_data *gd, vaddr_t gicc_base,
+			vaddr_t gicd_base)
+{
+	gd->gicc_base = gicc_base;
+	gd->gicd_base = gicd_base;
+	gd->max_it = probe_max_it(gicc_base, gicd_base);
+	gd->chip.ops = &gic_ops;
+}
+
+static void gic_it_add(struct gic_data *gd, size_t it)
+{
+	size_t idx = it / NUM_INTS_PER_REG;
+	uint32_t mask = 1 << (it % NUM_INTS_PER_REG);
+
+	/* Disable the interrupt */
+	write32(mask, gd->gicd_base + GICD_ICENABLER(idx));
+	/* Make it non-pending */
+	write32(mask, gd->gicd_base + GICD_ICPENDR(idx));
+	/* Assign it to group0 */
+	write32(read32(gd->gicd_base + GICD_IGROUPR(idx)) & ~mask,
+			gd->gicd_base + GICD_IGROUPR(idx));
+}
+
+static void gic_it_set_cpu_mask(struct gic_data *gd, size_t it,
+				uint8_t cpu_mask)
+{
+	size_t idx __maybe_unused = it / NUM_INTS_PER_REG;
+	uint32_t mask __maybe_unused = 1 << (it % NUM_INTS_PER_REG);
+	uint32_t target, target_shift;
+
+	/* Assigned to group0 */
+	assert(!(read32(gd->gicd_base + GICD_IGROUPR(idx)) & mask));
+
+	/* Route it to selected CPUs */
+	target = read32(gd->gicd_base +
+			GICD_ITARGETSR(it / NUM_TARGETS_PER_REG));
+	target_shift = (it % NUM_TARGETS_PER_REG) * ITARGETSR_FIELD_BITS;
+	target &= ~(ITARGETSR_FIELD_MASK << target_shift);
+	target |= cpu_mask << target_shift;
+	DMSG("cpu_mask: writing 0x%x to 0x%" PRIxVA,
+	     target, gd->gicd_base + GICD_ITARGETSR(it / NUM_TARGETS_PER_REG));
+	write32(target,
+		gd->gicd_base + GICD_ITARGETSR(it / NUM_TARGETS_PER_REG));
+	DMSG("cpu_mask: 0x%x\n",
+	     read32(gd->gicd_base + GICD_ITARGETSR(it / NUM_TARGETS_PER_REG)));
+}
+
+static void gic_it_set_prio(struct gic_data *gd, size_t it, uint8_t prio)
+{
+	size_t idx __maybe_unused = it / NUM_INTS_PER_REG;
+	uint32_t mask __maybe_unused = 1 << (it % NUM_INTS_PER_REG);
+
+	/* Assigned to group0 */
+	assert(!(read32(gd->gicd_base + GICD_IGROUPR(idx)) & mask));
+
+	/* Set prio it to selected CPUs */
+	DMSG("prio: writing 0x%x to 0x%" PRIxVA,
+		prio, gd->gicd_base + GICD_IPRIORITYR(0) + it);
+	write8(prio, gd->gicd_base + GICD_IPRIORITYR(0) + it);
+}
+
+static void gic_it_enable(struct gic_data *gd, size_t it)
+{
+	size_t idx = it / NUM_INTS_PER_REG;
+	uint32_t mask = 1 << (it % NUM_INTS_PER_REG);
+
+	/* Assigned to group0 */
+	assert(!(read32(gd->gicd_base + GICD_IGROUPR(idx)) & mask));
+	if (it >= NUM_SGI) {
+		/*
+		 * Not enabled yet, except Software Generated Interrupt
+		 * which is implementation defined
+		 */
+		assert(!(read32(gd->gicd_base + GICD_ISENABLER(idx)) & mask));
+	}
+
+	/* Enable the interrupt */
+	write32(mask, gd->gicd_base + GICD_ISENABLER(idx));
+}
+
+static void gic_it_disable(struct gic_data *gd, size_t it)
+{
+	size_t idx = it / NUM_INTS_PER_REG;
+	uint32_t mask = 1 << (it % NUM_INTS_PER_REG);
+
+	/* Assigned to group0 */
+	assert(!(read32(gd->gicd_base + GICD_IGROUPR(idx)) & mask));
+
+	/* Disable the interrupt */
+	write32(mask, gd->gicd_base + GICD_ICENABLER(idx));
+}
+
+static void gic_it_set_pending(struct gic_data *gd, size_t it)
+{
+	size_t idx = it / NUM_INTS_PER_REG;
+	uint32_t mask = BIT32(it % NUM_INTS_PER_REG);
+
+	/* Should be Peripheral Interrupt */
+	assert(it >= NUM_SGI);
+	/* Assigned to group0 */
+	assert(!(read32(gd->gicd_base + GICD_IGROUPR(idx)) & mask));
+
+	/* Raise the interrupt */
+	write32(mask, gd->gicd_base + GICD_ISPENDR(idx));
+}
+
+static void gic_it_raise_sgi(struct gic_data *gd, size_t it,
+		uint8_t cpu_mask, uint8_t group)
+{
+	uint32_t mask_id = it & 0xf;
+	uint32_t mask_group = group & 0x1;
+	uint32_t mask_cpu = cpu_mask & 0xff;
+	uint32_t mask = (mask_id | SHIFT_U32(mask_group, 15) |
+		SHIFT_U32(mask_cpu, 16));
+
+	/* Should be Software Generated Interrupt */
+	assert(it < NUM_SGI);
+
+	/* Raise the interrupt */
+	write32(mask, gd->gicd_base + GICD_SGIR);
+}
+
+static uint32_t gic_read_iar(struct gic_data *gd)
+{
+	return read32(gd->gicc_base + GICC_IAR);
+}
+
+static void gic_write_eoir(struct gic_data *gd, uint32_t eoir)
+{
+	write32(eoir, gd->gicc_base + GICC_EOIR);
+}
+
+static bool gic_it_is_enabled(struct gic_data *gd, size_t it)
+{
+	size_t idx = it / NUM_INTS_PER_REG;
+	uint32_t mask = 1 << (it % NUM_INTS_PER_REG);
+	return !!(read32(gd->gicd_base + GICD_ISENABLER(idx)) & mask);
+}
+
+static bool __maybe_unused gic_it_get_group(struct gic_data *gd, size_t it)
+{
+	size_t idx = it / NUM_INTS_PER_REG;
+	uint32_t mask = 1 << (it % NUM_INTS_PER_REG);
+	return !!(read32(gd->gicd_base + GICD_IGROUPR(idx)) & mask);
+}
+
+static uint32_t __maybe_unused gic_it_get_target(struct gic_data *gd, size_t it)
+{
+	size_t reg_idx = it / NUM_TARGETS_PER_REG;
+	uint32_t target_shift = (it % NUM_TARGETS_PER_REG) *
+				ITARGETSR_FIELD_BITS;
+	uint32_t target_mask = ITARGETSR_FIELD_MASK << target_shift;
+	uint32_t target =
+		read32(gd->gicd_base + GICD_ITARGETSR(reg_idx)) & target_mask;
+
+	target = target >> target_shift;
+	return target;
+}
+
+void gic_dump_state(struct gic_data *gd)
+{
+	int i;
+
+	DMSG("GICC_CTLR: 0x%x", read32(gd->gicc_base + GICC_CTLR));
+	DMSG("GICD_CTLR: 0x%x", read32(gd->gicd_base + GICD_CTLR));
+
+	for (i = 0; i < (int)gd->max_it; i++) {
+		if (gic_it_is_enabled(gd, i)) {
+			DMSG("irq%d: enabled, group:%d, target:%x", i,
+			     gic_it_get_group(gd, i), gic_it_get_target(gd, i));
+		}
+	}
+}
+
+void gic_it_handle(struct gic_data *gd)
+{
+	uint32_t iar;
+	uint32_t id;
+
+	iar = gic_read_iar(gd);
+	id = iar & GICC_IAR_IT_ID_MASK;
+
+	if (id == GIC_SPURIOUS_ID)
+		DMSG("ignoring spurious interrupt");
+	else
+		itr_handle(id);
+
+	gic_write_eoir(gd, iar);
+}
+
+static void gic_op_add(struct itr_chip *chip, size_t it,
+		       uint32_t flags __unused)
+{
+	struct gic_data *gd = container_of(chip, struct gic_data, chip);
+
+	if (it >= gd->max_it)
+		panic();
+
+	gic_it_add(gd, it);
+	/* Set the CPU mask to deliver interrupts to any online core */
+	gic_it_set_cpu_mask(gd, it, 0xff);
+	gic_it_set_prio(gd, it, 0x1);
+}
+
+static void gic_op_enable(struct itr_chip *chip, size_t it)
+{
+	struct gic_data *gd = container_of(chip, struct gic_data, chip);
+
+	if (it >= gd->max_it)
+		panic();
+
+	gic_it_enable(gd, it);
+}
+
+static void gic_op_disable(struct itr_chip *chip, size_t it)
+{
+	struct gic_data *gd = container_of(chip, struct gic_data, chip);
+
+	if (it >= gd->max_it)
+		panic();
+
+	gic_it_disable(gd, it);
+}
+
+static void gic_op_raise_pi(struct itr_chip *chip, size_t it)
+{
+	struct gic_data *gd = container_of(chip, struct gic_data, chip);
+
+	if (it >= gd->max_it)
+		panic();
+
+	gic_it_set_pending(gd, it);
+}
+
+static void gic_op_raise_sgi(struct itr_chip *chip, size_t it,
+			uint8_t cpu_mask)
+{
+	struct gic_data *gd = container_of(chip, struct gic_data, chip);
+
+	if (it >= gd->max_it)
+		panic();
+
+	if (it < NUM_NS_SGI)
+		gic_it_raise_sgi(gd, it, cpu_mask, 1);
+	else
+		gic_it_raise_sgi(gd, it, cpu_mask, 0);
+}
+static void gic_op_set_affinity(struct itr_chip *chip, size_t it,
+			uint8_t cpu_mask)
+{
+	struct gic_data *gd = container_of(chip, struct gic_data, chip);
+
+	if (it >= gd->max_it)
+		panic();
+
+	gic_it_set_cpu_mask(gd, it, cpu_mask);
+}
diff --git a/core/drivers/hi16xx_rng.c b/core/drivers/hi16xx_rng.c
new file mode 100644
index 0000000..a837477
--- /dev/null
+++ b/core/drivers/hi16xx_rng.c
@@ -0,0 +1,113 @@
+/*
+ * Copyright (c) 2016, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/* Driver for the internal Random Number Generator of HiSilicon P660/Hi16xx */
+
+#include <initcall.h>
+#include <io.h>
+#include <kernel/mutex.h>
+#include <kernel/tee_time.h>
+#include <mm/core_memprot.h>
+#include <mm/core_mmu.h>
+#include <platform_config.h>
+#include <rng_support.h>
+#include <trace.h>
+#include <types_ext.h>
+#include <util.h>
+
+/* ALG sub-controller registers */
+
+#define ALG_SC_RNG_RESET_DREQ	0xAB4	/* RNG reset cancel */
+#  define ALG_SC_SRST_DREQ_RNG	BIT(0)
+
+/* RNG registers */
+
+#define	RNG_SEED	0x0	/* Initial seed */
+#define RNG_CTRL	0x4	/* Control register */
+#  define RNG_SEED_SEL	BIT(2)	/* Re-seed source: 1: ring osc., 0: LFSR */
+#  define RNG_RING_EN	BIT(1)	/* Enable ring oscillator */
+#  define RNG_EN	BIT(0)	/* Enable RNG */
+#define RNG_NUM		0x10	/* Random number output */
+#define RNG_PHY_SEED	0x14	/* Ring oscillator output */
+
+register_phys_mem(MEM_AREA_IO_SEC, ALG_SC_BASE, ALG_SC_REG_SIZE);
+register_phys_mem(MEM_AREA_IO_SEC, RNG_BASE, RNG_REG_SIZE);
+
+static struct mutex rng_mutex = MUTEX_INITIALIZER;
+
+static TEE_Result hi16xx_rng_init(void)
+{
+	vaddr_t alg = (vaddr_t)phys_to_virt(ALG_SC_BASE, MEM_AREA_IO_SEC);
+	vaddr_t rng = (vaddr_t)phys_to_virt(RNG_BASE, MEM_AREA_IO_SEC);
+	TEE_Time time;
+
+	/* ALG sub-controller must allow RNG out of reset */
+	write32(ALG_SC_SRST_DREQ_RNG, alg + ALG_SC_RNG_RESET_DREQ);
+
+	/* Set initial seed */
+	tee_time_get_sys_time(&time);
+	write32(time.seconds * 1000 + time.millis, rng + RNG_SEED);
+
+	/*
+	 * Enable RNG and configure it to re-seed automatically from the
+	 * internal ring oscillator
+	 */
+	write32(RNG_EN | RNG_RING_EN | RNG_SEED_SEL, rng + RNG_CTRL);
+
+	IMSG("Hi16xx RNG initialized");
+	return TEE_SUCCESS;
+}
+
+uint8_t hw_get_random_byte(void)
+{
+	static vaddr_t r;
+	static int pos;
+	static union {
+		uint32_t val;
+		uint8_t byte[4];
+	} random;
+	uint8_t ret;
+
+	mutex_lock(&rng_mutex);
+
+	if (!r)
+		r = (vaddr_t)phys_to_virt(RNG_BASE, MEM_AREA_IO_SEC) + RNG_NUM;
+
+	if (!pos)
+		random.val = read32(r);
+
+	ret = random.byte[pos++];
+
+	if (pos == 4)
+		pos = 0;
+
+	mutex_unlock(&rng_mutex);
+
+	return ret;
+}
+
+driver_init(hi16xx_rng_init);
diff --git a/core/drivers/hi16xx_uart.c b/core/drivers/hi16xx_uart.c
new file mode 100644
index 0000000..76e769d
--- /dev/null
+++ b/core/drivers/hi16xx_uart.c
@@ -0,0 +1,131 @@
+/*
+ * Copyright (c) 2016, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+#include <drivers/hi16xx_uart.h>
+#include <io.h>
+
+/* Register offsets */
+
+#define UART_RBR	0x00	/* RX data buffer register */
+#define UART_THR	0x00	/* TX data buffer register */
+#define UART_DLL	0x00	/* Lower-bit frequency divider register */
+
+#define UART_IEL	0x04	/* Interrupt enable register */
+#define UART_DLH	0x04	/* Upper-bit frequency divider register */
+
+#define UART_FCR	0x08	/* FIFO control register */
+
+#define UART_LCR	0x0C	/* Line control register */
+
+#define UART_LSR	0x14	/* Line status register */
+
+#define UART_USR	0x7C	/* Status register */
+
+/*
+ * Line control register
+ */
+
+/* Data length selection */
+#define UART_LCR_DLS5	0x0	/* 5 bits */
+#define UART_LCR_DLS6	0x1	/* 6 bits */
+#define UART_LCR_DLS7	0x2	/* 7 bits */
+#define UART_LCR_DLS8	0x3	/* 8 bits */
+
+/* Enable access to UART_DLL and UART_DLH */
+#define UART_LCR_DLAB	0x80
+
+/*
+ * FIFO control register
+ */
+
+#define UART_FCR_FIFO_EN	0x1	/* Enable FIFO (depth: 32 bytes) */
+#define UART_FCR_RX_FIFO_RST	0x2	/* Clear receive FIFO (auto reset) */
+#define UART_FCR_TX_FIFO_RST	0x4	/* Clear send FIFO (auto reset) */
+
+
+/*
+ * Status register
+ */
+
+#define UART_USR_BUSY_BIT	0	/* 0: idle/non-activated, 1: busy */
+#define UART_USR_TFNF_BIT	1	/* Transmit FIFO not full bit */
+#define UART_USR_TFE_BIT	2	/* Transmit FIFO empty bit */
+#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)
+{
+	while (!(read32(base + UART_USR) & UART_USR_TFE_BIT))
+		;
+}
+
+void hi16xx_uart_init(vaddr_t base, uint32_t uart_clk, uint32_t baud_rate)
+{
+	uint16_t freq_div = uart_clk / (16 * baud_rate);
+
+	/* Enable (and clear) FIFOs */
+	write32(UART_FCR_FIFO_EN, base + UART_FCR);
+
+	/* Enable access to _DLL and _DLH */
+	write32(UART_LCR_DLAB, base + UART_LCR);
+
+	/* Calculate and set UART_DLL */
+	write32(freq_div & 0xFF, base + UART_DLL);
+
+	/* Calculate and set UART_DLH */
+	write32((freq_div >> 8) & 0xFF, base + UART_DLH);
+
+	/* Clear _DLL/_DLH access bit, set data size (8 bits), parity etc. */
+	write32(UART_LCR_DLS8, base + UART_LCR);
+
+	/* 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;
+}
+
diff --git a/core/drivers/imx_uart.c b/core/drivers/imx_uart.c
new file mode 100644
index 0000000..b66d905
--- /dev/null
+++ b/core/drivers/imx_uart.c
@@ -0,0 +1,112 @@
+/*
+ * Copyright (C) 2015 Freescale Semiconductor, Inc.
+ * 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 <drivers/imx_uart.h>
+#include <console.h>
+#include <io.h>
+#include <compiler.h>
+
+/* Register definitions */
+#define URXD  0x0  /* Receiver Register */
+#define UTXD  0x40 /* Transmitter Register */
+#define UCR1  0x80 /* Control Register 1 */
+#define UCR2  0x84 /* Control Register 2 */
+#define UCR3  0x88 /* Control Register 3 */
+#define UCR4  0x8c /* Control Register 4 */
+#define UFCR  0x90 /* FIFO Control Register */
+#define USR1  0x94 /* Status Register 1 */
+#define USR2  0x98 /* Status Register 2 */
+#define UESC  0x9c /* Escape Character Register */
+#define UTIM  0xa0 /* Escape Timer Register */
+#define UBIR  0xa4 /* BRM Incremental Register */
+#define UBMR  0xa8 /* BRM Modulator Register */
+#define UBRC  0xac /* Baud Rate Count Register */
+#define UTS   0xb4 /* UART Test Register (mx31) */
+
+/* UART Control Register Bit Fields.*/
+#define  URXD_CHARRDY    (1<<15)
+#define  URXD_ERR        (1<<14)
+#define  URXD_OVRRUN     (1<<13)
+#define  URXD_FRMERR     (1<<12)
+#define  URXD_BRK        (1<<11)
+#define  URXD_PRERR      (1<<10)
+#define  URXD_RX_DATA    (0xFF)
+#define  UCR1_ADEN       (1<<15) /* Auto dectect interrupt */
+#define  UCR1_ADBR       (1<<14) /* Auto detect baud rate */
+#define  UCR1_TRDYEN     (1<<13) /* Transmitter ready interrupt enable */
+#define  UCR1_IDEN       (1<<12) /* Idle condition interrupt */
+#define  UCR1_RRDYEN     (1<<9)	 /* Recv ready interrupt enable */
+#define  UCR1_RDMAEN     (1<<8)	 /* Recv ready DMA enable */
+#define  UCR1_IREN       (1<<7)	 /* Infrared interface enable */
+#define  UCR1_TXMPTYEN   (1<<6)	 /* Transimitter empty interrupt enable */
+#define  UCR1_RTSDEN     (1<<5)	 /* RTS delta interrupt enable */
+#define  UCR1_SNDBRK     (1<<4)	 /* Send break */
+#define  UCR1_TDMAEN     (1<<3)	 /* Transmitter ready DMA enable */
+#define  UCR1_UARTCLKEN  (1<<2)	 /* UART clock enabled */
+#define  UCR1_DOZE       (1<<1)	 /* Doze */
+#define  UCR1_UARTEN     (1<<0)	 /* UART enabled */
+
+#define  UTS_FRCPERR	 (1<<13) /* Force parity error */
+#define  UTS_LOOP        (1<<12) /* Loop tx and rx */
+#define  UTS_TXEMPTY	 (1<<6)	 /* TxFIFO empty */
+#define  UTS_RXEMPTY	 (1<<5)	 /* RxFIFO empty */
+#define  UTS_TXFULL	 (1<<4)	 /* TxFIFO full */
+#define  UTS_RXFULL	 (1<<3)	 /* RxFIFO full */
+#define  UTS_SOFTRST	 (1<<0)	 /* Software reset */
+
+void imx_uart_init(vaddr_t __unused vbase)
+{
+	/*
+	 * Do nothing, debug uart(uart0) share with normal world,
+	 * everything for uart0 intialization is done in bootloader.
+	 */
+}
+
+void imx_uart_flush_tx_fifo(vaddr_t base)
+{
+	while (!(read32(base + UTS) & UTS_TXEMPTY))
+		;
+}
+
+int imx_uart_getchar(vaddr_t base)
+{
+	while (read32(base + UTS) & UTS_RXEMPTY)
+		;
+
+	return (read32(base + URXD) & URXD_RX_DATA);
+}
+
+void imx_uart_putc(const char c, vaddr_t base)
+{
+	write32(c, base + UTXD);
+
+	/* wait until sent */
+	while (!(read32(base + UTS) & UTS_TXEMPTY))
+		;
+}
diff --git a/core/drivers/ns16550.c b/core/drivers/ns16550.c
new file mode 100644
index 0000000..710b351
--- /dev/null
+++ b/core/drivers/ns16550.c
@@ -0,0 +1,57 @@
+/*
+ * Copyright (C) 2015 Freescale Semiconductor, Inc.
+ * 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 <drivers/ns16550.h>
+#include <io.h>
+
+/* uart register defines */
+#define UART_RBR	0x0
+#define UART_THR	0x0
+#define UART_IER	0x1
+#define UART_FCR	0x2
+#define UART_LCR	0x3
+#define UART_MCR	0x4
+#define UART_LSR	0x5
+#define UART_MSR	0x6
+#define UART_SPR	0x7
+
+/* uart status register bits */
+#define UART_LSR_THRE	0x20 /* Transmit-hold-register empty */
+
+void ns16550_flush(vaddr_t base)
+{
+	while ((read8(base + UART_LSR) & UART_LSR_THRE) == 0)
+		;
+}
+
+void ns16550_putc(int ch, vaddr_t base)
+{
+	ns16550_flush(base);
+
+	/* write out charset to Transmit-hold-register */
+	write8(ch, base + UART_THR);
+}
diff --git a/core/drivers/pl011.c b/core/drivers/pl011.c
new file mode 100644
index 0000000..8c03090
--- /dev/null
+++ b/core/drivers/pl011.c
@@ -0,0 +1,147 @@
+/*
+ * Copyright (c) 2014, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+#include <drivers/pl011.h>
+#include <io.h>
+
+#define UART_DR		0x00 /* data register */
+#define UART_RSR_ECR	0x04 /* receive status or error clear */
+#define UART_DMAWM	0x08 /* DMA watermark configure */
+#define UART_TIMEOUT	0x0C /* Timeout period */
+/* reserved space */
+#define UART_FR		0x18 /* flag register */
+#define UART_ILPR	0x20 /* IrDA low-poer */
+#define UART_IBRD	0x24 /* integer baud register */
+#define UART_FBRD	0x28 /* fractional baud register */
+#define UART_LCR_H	0x2C /* line control register */
+#define UART_CR		0x30 /* control register */
+#define UART_IFLS	0x34 /* interrupt FIFO level select */
+#define UART_IMSC	0x38 /* interrupt mask set/clear */
+#define UART_RIS	0x3C /* raw interrupt register */
+#define UART_MIS	0x40 /* masked interrupt register */
+#define UART_ICR	0x44 /* interrupt clear register */
+#define UART_DMACR	0x48 /* DMA control register */
+
+/* flag register bits */
+#define UART_FR_RTXDIS	(1 << 13)
+#define UART_FR_TERI	(1 << 12)
+#define UART_FR_DDCD	(1 << 11)
+#define UART_FR_DDSR	(1 << 10)
+#define UART_FR_DCTS	(1 << 9)
+#define UART_FR_RI	(1 << 8)
+#define UART_FR_TXFE	(1 << 7)
+#define UART_FR_RXFF	(1 << 6)
+#define UART_FR_TXFF	(1 << 5)
+#define UART_FR_RXFE	(1 << 4)
+#define UART_FR_BUSY	(1 << 3)
+#define UART_FR_DCD	(1 << 2)
+#define UART_FR_DSR	(1 << 1)
+#define UART_FR_CTS	(1 << 0)
+
+/* transmit/receive line register bits */
+#define UART_LCRH_SPS		(1 << 7)
+#define UART_LCRH_WLEN_8	(3 << 5)
+#define UART_LCRH_WLEN_7	(2 << 5)
+#define UART_LCRH_WLEN_6	(1 << 5)
+#define UART_LCRH_WLEN_5	(0 << 5)
+#define UART_LCRH_FEN		(1 << 4)
+#define UART_LCRH_STP2		(1 << 3)
+#define UART_LCRH_EPS		(1 << 2)
+#define UART_LCRH_PEN		(1 << 1)
+#define UART_LCRH_BRK		(1 << 0)
+
+/* control register bits */
+#define UART_CR_CTSEN		(1 << 15)
+#define UART_CR_RTSEN		(1 << 14)
+#define UART_CR_OUT2		(1 << 13)
+#define UART_CR_OUT1		(1 << 12)
+#define UART_CR_RTS		(1 << 11)
+#define UART_CR_DTR		(1 << 10)
+#define UART_CR_RXE		(1 << 9)
+#define UART_CR_TXE		(1 << 8)
+#define UART_CR_LPE		(1 << 7)
+#define UART_CR_OVSFACT		(1 << 3)
+#define UART_CR_UARTEN		(1 << 0)
+
+#define UART_IMSC_RTIM		(1 << 6)
+#define UART_IMSC_RXIM		(1 << 4)
+
+void pl011_flush(vaddr_t base)
+{
+	while (!(read32(base + UART_FR) & UART_FR_TXFE))
+		;
+}
+
+void pl011_init(vaddr_t base, uint32_t uart_clk, uint32_t baud_rate)
+{
+	/* Clear all errors */
+	write32(0, base + UART_RSR_ECR);
+	/* Disable everything */
+	write32(0, base + UART_CR);
+
+	if (baud_rate) {
+		uint32_t divisor = (uart_clk * 4) / baud_rate;
+
+		write32(divisor >> 6, base + UART_IBRD);
+		write32(divisor & 0x3f, base + UART_FBRD);
+	}
+
+	/* Configure TX to 8 bits, 1 stop bit, no parity, fifo disabled. */
+	write32(UART_LCRH_WLEN_8, base + UART_LCR_H);
+
+	/* Enable interrupts for receive and receive timeout */
+	write32(UART_IMSC_RXIM | UART_IMSC_RTIM, base + UART_IMSC);
+
+	/* 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;
+}
+
diff --git a/core/drivers/pl022_spi.c b/core/drivers/pl022_spi.c
new file mode 100644
index 0000000..e5e3512
--- /dev/null
+++ b/core/drivers/pl022_spi.c
@@ -0,0 +1,530 @@
+/*
+ * Copyright (c) 2016, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#include <assert.h>
+#include <drivers/pl022_spi.h>
+#include <initcall.h>
+#include <io.h>
+#include <kernel/panic.h>
+#include <kernel/tee_time.h>
+#include <platform_config.h>
+#include <trace.h>
+#include <util.h>
+
+/* SPI register offsets */
+#define SSPCR0		0x000
+#define SSPCR1		0x004
+#define SSPDR		0x008
+#define SSPSR		0x00C
+#define SSPCPSR		0x010
+#define SSPIMSC		0x014
+#define SSPRIS		0x018
+#define SSPMIS		0x01C
+#define SSPICR		0x020
+#define SSPDMACR	0x024
+
+#ifdef PLATFORM_hikey
+/* HiKey extensions */
+#define SSPTXFIFOCR	0x028
+#define SSPRXFIFOCR	0x02C
+#define SSPB2BTRANS	0x030
+#endif
+
+/* test registers */
+#define SSPTCR		0x080
+#define SSPITIP		0x084
+#define SSPITOP		0x088
+#define SSPTDR		0x08C
+
+#define SSPPeriphID0	0xFE0
+#define SSPPeriphID1	0xFE4
+#define SSPPeriphID2	0xFE8
+#define SSPPeriphID3	0xFEC
+
+#define SSPPCellID0	0xFF0
+#define SSPPCellID1	0xFF4
+#define SSPPCellID2	0xFF8
+#define SSPPCellID3	0xFFC
+
+/* SPI register masks */
+#define SSPCR0_SCR		SHIFT_U32(0xFF, 8)
+#define SSPCR0_SPH		SHIFT_U32(1, 7)
+#define SSPCR0_SPH1		SHIFT_U32(1, 7)
+#define SSPCR0_SPH0		SHIFT_U32(0, 7)
+#define SSPCR0_SPO		SHIFT_U32(1, 6)
+#define SSPCR0_SPO1		SHIFT_U32(1, 6)
+#define SSPCR0_SPO0		SHIFT_U32(0, 6)
+#define SSPCR0_FRF		SHIFT_U32(3, 4)
+#define SSPCR0_FRF_SPI		SHIFT_U32(0, 4)
+#define SSPCR0_DSS		SHIFT_U32(0xFF, 0)
+#define SSPCR0_DSS_16BIT	SHIFT_U32(0xF, 0)
+#define SSPCR0_DSS_8BIT		SHIFT_U32(7, 0)
+
+#define SSPCR1_SOD		SHIFT_U32(1, 3)
+#define SSPCR1_SOD_ENABLE	SHIFT_U32(1, 3)
+#define SSPCR1_SOD_DISABLE	SHIFT_U32(0, 3)
+#define SSPCR1_MS		SHIFT_U32(1, 2)
+#define SSPCR1_MS_SLAVE		SHIFT_U32(1, 2)
+#define SSPCR1_MS_MASTER	SHIFT_U32(0, 2)
+#define SSPCR1_SSE		SHIFT_U32(1, 1)
+#define SSPCR1_SSE_ENABLE	SHIFT_U32(1, 1)
+#define SSPCR1_SSE_DISABLE	SHIFT_U32(0, 1)
+#define SSPCR1_LBM		SHIFT_U32(1, 0)
+#define SSPCR1_LBM_YES		SHIFT_U32(1, 0)
+#define SSPCR1_LBM_NO		SHIFT_U32(0, 0)
+
+#define SSPDR_DATA	SHIFT_U32(0xFFFF, 0)
+
+#define SSPSR_BSY	SHIFT_U32(1, 4)
+#define SSPSR_RNF	SHIFT_U32(1, 3)
+#define SSPSR_RNE	SHIFT_U32(1, 2)
+#define SSPSR_TNF	SHIFT_U32(1, 1)
+#define SSPSR_TFE	SHIFT_U32(1, 0)
+
+#define SSPCPSR_CPSDVR	SHIFT_U32(0xFF, 0)
+
+#define SSPIMSC_TXIM	SHIFT_U32(1, 3)
+#define SSPIMSC_RXIM	SHIFT_U32(1, 2)
+#define SSPIMSC_RTIM	SHIFT_U32(1, 1)
+#define SSPIMSC_RORIM	SHIFT_U32(1, 0)
+
+#define SSPRIS_TXRIS	SHIFT_U32(1, 3)
+#define SSPRIS_RXRIS	SHIFT_U32(1, 2)
+#define SSPRIS_RTRIS	SHIFT_U32(1, 1)
+#define SSPRIS_RORRIS	SHIFT_U32(1, 0)
+
+#define SSPMIS_TXMIS	SHIFT_U32(1, 3)
+#define SSPMIS_RXMIS	SHIFT_U32(1, 2)
+#define SSPMIS_RTMIS	SHIFT_U32(1, 1)
+#define SSPMIS_RORMIS	SHIFT_U32(1, 0)
+
+#define SSPICR_RTIC		SHIFT_U32(1, 1)
+#define SSPICR_RORIC		SHIFT_U32(1, 0)
+
+#define SSPDMACR_TXDMAE	SHIFT_U32(1, 1)
+#define SSPDMACR_RXDMAE	SHIFT_U32(1, 0)
+
+#define SSPPeriphID0_PartNumber0	SHIFT_U32(0xFF, 0) /* 0x22 */
+#define SSPPeriphID1_Designer0		SHIFT_U32(0xF, 4) /* 0x1 */
+#define SSPPeriphID1_PartNumber1	SHIFT_U32(0xF, 0) /* 0x0 */
+#define SSPPeriphID2_Revision		SHIFT_U32(0xF, 4)
+#define SSPPeriphID2_Designer1		SHIFT_U32(0xF, 0) /* 0x4 */
+#define SSPPeriphID3_Configuration	SHIFT_U32(0xFF, 0) /* 0x00 */
+
+#define SSPPCellID_0	SHIFT_U32(0xFF, 0) /* 0x0D */
+#define SSPPCellID_1	SHIFT_U32(0xFF, 0) /* 0xF0 */
+#define SSPPPCellID_2	SHIFT_U32(0xFF, 0) /* 0x05 */
+#define SSPPPCellID_3	SHIFT_U32(0xFF, 0) /* 0xB1 */
+
+#define MASK_32 0xFFFFFFFF
+#define MASK_28 0xFFFFFFF
+#define MASK_24 0xFFFFFF
+#define MASK_20 0xFFFFF
+#define MASK_16 0xFFFF
+#define MASK_12 0xFFF
+#define MASK_8 0xFF
+#define MASK_4 0xF
+/* SPI register masks */
+
+#define SSP_CPSDVR_MAX		254
+#define SSP_CPSDVR_MIN		2
+#define SSP_SCR_MAX		255
+#define SSP_SCR_MIN		0
+#define SSP_DATASIZE_MAX	16
+
+static enum spi_result pl022_txrx8(struct spi_chip *chip, uint8_t *wdat,
+	uint8_t *rdat, size_t num_pkts)
+{
+	size_t i = 0;
+	size_t j = 0;
+	struct pl022_data *pd = container_of(chip, struct pl022_data, chip);
+
+
+	if (pd->data_size_bits != 8) {
+		EMSG("data_size_bits should be 8, not %u",
+			pd->data_size_bits);
+		return SPI_ERR_CFG;
+	}
+
+	if (wdat)
+		while (i < num_pkts) {
+			if (read8(pd->base + SSPSR) & SSPSR_TNF) {
+				/* tx 1 packet */
+				write8(wdat[i++], pd->base + SSPDR);
+			}
+
+			if (rdat)
+				if (read8(pd->base + SSPSR) & SSPSR_RNE) {
+					/* rx 1 packet */
+					rdat[j++] = read8(pd->base + SSPDR);
+				}
+		}
+
+	/* Capture remaining rdat not read above */
+	if (rdat) {
+		while ((j < num_pkts) &&
+			(read8(pd->base + SSPSR) & SSPSR_BSY))
+			if (read8(pd->base + SSPSR) & SSPSR_RNE) {
+				/* rx 1 packet */
+				rdat[j++] = read8(pd->base + SSPDR);
+			}
+
+		if (j < num_pkts) {
+			EMSG("Packets requested %zu, received %zu",
+				num_pkts, j);
+			return SPI_ERR_PKTCNT;
+		}
+	}
+
+	return SPI_OK;
+}
+
+static enum spi_result pl022_txrx16(struct spi_chip *chip, uint16_t *wdat,
+	uint16_t *rdat, size_t num_pkts)
+{
+	size_t i = 0;
+	size_t j = 0;
+	struct pl022_data *pd = container_of(chip, struct pl022_data, chip);
+
+	if (pd->data_size_bits != 16) {
+		EMSG("data_size_bits should be 16, not %u",
+			pd->data_size_bits);
+		return SPI_ERR_CFG;
+	}
+
+	if (wdat)
+		while (i < num_pkts) {
+			if (read8(pd->base + SSPSR) & SSPSR_TNF) {
+				/* tx 1 packet */
+				write16(wdat[i++], pd->base + SSPDR);
+			}
+
+			if (rdat)
+				if (read8(pd->base + SSPSR) & SSPSR_RNE) {
+					/* rx 1 packet */
+					rdat[j++] = read16(pd->base + SSPDR);
+				}
+		}
+
+	/* Capture remaining rdat not read above */
+	if (rdat) {
+		while ((j < num_pkts) &&
+			(read8(pd->base + SSPSR) & SSPSR_BSY))
+			if (read8(pd->base + SSPSR) & SSPSR_RNE) {
+				/* rx 1 packet */
+				rdat[j++] = read16(pd->base + SSPDR);
+			}
+
+		if (j < num_pkts) {
+			EMSG("Packets requested %zu, received %zu",
+				num_pkts, j);
+			return SPI_ERR_PKTCNT;
+		}
+	}
+
+	return SPI_OK;
+}
+
+static void pl022_print_peri_id(struct pl022_data *pd __maybe_unused)
+{
+	DMSG("Expected: 0x 22 10 ?4 00");
+	DMSG("Read: 0x %02x %02x %02x %02x",
+		read32(pd->base + SSPPeriphID0),
+		read32(pd->base + SSPPeriphID1),
+		read32(pd->base + SSPPeriphID2),
+		read32(pd->base + SSPPeriphID3));
+}
+
+static void pl022_print_cell_id(struct pl022_data *pd __maybe_unused)
+{
+	DMSG("Expected: 0x 0d f0 05 b1");
+	DMSG("Read: 0x %02x %02x %02x %02x",
+		read32(pd->base + SSPPCellID0),
+		read32(pd->base + SSPPCellID1),
+		read32(pd->base + SSPPCellID2),
+		read32(pd->base + SSPPCellID3));
+}
+
+static void pl022_sanity_check(struct pl022_data *pd)
+{
+	assert(pd);
+	assert(pd->chip.ops);
+	assert(pd->cs_control <= PL022_CS_CTRL_MANUAL);
+	switch (pd->cs_control) {
+	case PL022_CS_CTRL_AUTO_GPIO:
+		assert(pd->cs_data.gpio_data.chip);
+		assert(pd->cs_data.gpio_data.chip->ops);
+		break;
+	case PL022_CS_CTRL_CB:
+		assert(pd->cs_data.cs_cb);
+		break;
+	default:
+		break;
+	}
+	assert(pd->clk_hz);
+	assert(pd->speed_hz && pd->speed_hz <= pd->clk_hz/2);
+	assert(pd->mode <= SPI_MODE3);
+	assert(pd->data_size_bits == 8 || pd->data_size_bits == 16);
+
+	#ifdef PLATFORM_hikey
+	DMSG("SSPB2BTRANS: Expected: 0x2. Read: 0x%x",
+		read32(pd->base + SSPB2BTRANS));
+	#endif
+	pl022_print_peri_id(pd);
+	pl022_print_cell_id(pd);
+}
+
+static inline uint32_t pl022_calc_freq(struct pl022_data *pd,
+	uint8_t cpsdvr, uint8_t scr)
+{
+	return pd->clk_hz / (cpsdvr * (1 + scr));
+}
+
+static void pl022_control_cs(struct spi_chip *chip, enum gpio_level value)
+{
+	struct pl022_data *pd = container_of(chip, struct pl022_data, chip);
+
+	switch (pd->cs_control) {
+	case PL022_CS_CTRL_AUTO_GPIO:
+		if (read8(pd->base + SSPSR) & SSPSR_BSY)
+			DMSG("pl022 busy - do NOT set CS!");
+		while (read8(pd->base + SSPSR) & SSPSR_BSY)
+			;
+		DMSG("pl022 done - set CS!");
+
+		pd->cs_data.gpio_data.chip->ops->set_value(
+			pd->cs_data.gpio_data.pin_num, value);
+		break;
+	case PL022_CS_CTRL_CB:
+		pd->cs_data.cs_cb(value);
+		break;
+	default:
+		break;
+	}
+}
+
+static void pl022_calc_clk_divisors(struct pl022_data *pd,
+	uint8_t *cpsdvr, uint8_t *scr)
+{
+	unsigned int freq1 = 0;
+	unsigned int freq2 = 0;
+	uint8_t tmp_cpsdvr1;
+	uint8_t tmp_scr1;
+	uint8_t tmp_cpsdvr2 = 0;
+	uint8_t tmp_scr2 = 0;
+
+	for (tmp_scr1 = SSP_SCR_MIN; tmp_scr1 < SSP_SCR_MAX; tmp_scr1++) {
+		for (tmp_cpsdvr1 = SSP_CPSDVR_MIN; tmp_cpsdvr1 < SSP_CPSDVR_MAX;
+			tmp_cpsdvr1++) {
+			freq1 = pl022_calc_freq(pd, tmp_cpsdvr1, tmp_scr1);
+			if (freq1 == pd->speed_hz)
+				goto done;
+			else if (freq1 < pd->speed_hz)
+				goto stage2;
+		}
+	}
+
+stage2:
+	for (tmp_cpsdvr2 = SSP_CPSDVR_MIN; tmp_cpsdvr2 < SSP_CPSDVR_MAX;
+		tmp_cpsdvr2++) {
+		for (tmp_scr2 = SSP_SCR_MIN; tmp_scr2 < SSP_SCR_MAX;
+			tmp_scr2++) {
+			freq2 = pl022_calc_freq(pd, tmp_cpsdvr2, tmp_scr2);
+			if (freq2 <= pd->speed_hz)
+				goto done;
+		}
+	}
+
+done:
+	if (freq1 >= freq2) {
+		*cpsdvr = tmp_cpsdvr1;
+		*scr = tmp_scr1;
+		DMSG("speed: requested: %u, closest1: %u",
+			pd->speed_hz, freq1);
+	} else {
+		*cpsdvr = tmp_cpsdvr2;
+		*scr = tmp_scr2;
+		DMSG("speed: requested: %u, closest2: %u",
+			pd->speed_hz, freq2);
+	}
+	DMSG("CPSDVR: %u (0x%x), SCR: %u (0x%x)",
+		*cpsdvr, *cpsdvr, *scr, *scr);
+}
+
+static void pl022_flush_fifo(struct pl022_data *pd)
+{
+	uint32_t __maybe_unused rdat;
+
+	do {
+		while (read32(pd->base + SSPSR) & SSPSR_RNE) {
+			rdat = read32(pd->base + SSPDR);
+			DMSG("rdat: 0x%x", rdat);
+		}
+	} while (read32(pd->base + SSPSR) & SSPSR_BSY);
+}
+
+static void pl022_configure(struct spi_chip *chip)
+{
+	uint16_t mode;
+	uint16_t data_size;
+	uint8_t cpsdvr;
+	uint8_t scr;
+	uint8_t lbm;
+	struct pl022_data *pd = container_of(chip, struct pl022_data, chip);
+
+	pl022_sanity_check(pd);
+
+	switch (pd->cs_control) {
+	case PL022_CS_CTRL_AUTO_GPIO:
+		DMSG("Use auto GPIO CS control");
+		DMSG("Mask/disable interrupt for CS GPIO");
+		pd->cs_data.gpio_data.chip->ops->set_interrupt(
+			pd->cs_data.gpio_data.pin_num,
+			GPIO_INTERRUPT_DISABLE);
+		DMSG("Set CS GPIO dir to out");
+		pd->cs_data.gpio_data.chip->ops->set_direction(
+			pd->cs_data.gpio_data.pin_num,
+			GPIO_DIR_OUT);
+		break;
+	case PL022_CS_CTRL_CB:
+		DMSG("Use registered CS callback");
+		break;
+	case PL022_CS_CTRL_MANUAL:
+		DMSG("Use manual CS control");
+		break;
+	default:
+		EMSG("Invalid CS control type: %d", pd->cs_control);
+		panic();
+	}
+
+	DMSG("Pull CS high");
+	pl022_control_cs(chip, GPIO_LEVEL_HIGH);
+
+	pl022_calc_clk_divisors(pd, &cpsdvr, &scr);
+
+	/* configure ssp based on platform settings */
+	switch (pd->mode) {
+	case SPI_MODE0:
+		DMSG("SPI mode 0");
+		mode = SSPCR0_SPO0 | SSPCR0_SPH0;
+		break;
+	case SPI_MODE1:
+		DMSG("SPI mode 1");
+		mode = SSPCR0_SPO0 | SSPCR0_SPH1;
+		break;
+	case SPI_MODE2:
+		DMSG("SPI mode 2");
+		mode = SSPCR0_SPO1 | SSPCR0_SPH0;
+		break;
+	case SPI_MODE3:
+		DMSG("SPI mode 3");
+		mode = SSPCR0_SPO1 | SSPCR0_SPH1;
+		break;
+	default:
+		EMSG("Invalid SPI mode: %u", pd->mode);
+		panic();
+	}
+
+	switch (pd->data_size_bits) {
+	case 8:
+		DMSG("Data size: 8");
+		data_size = SSPCR0_DSS_8BIT;
+		break;
+	case 16:
+		DMSG("Data size: 16");
+		data_size = SSPCR0_DSS_16BIT;
+		break;
+	default:
+		EMSG("Unsupported data size: %u bits", pd->data_size_bits);
+		panic();
+	}
+
+	if (pd->loopback) {
+		DMSG("Starting in loopback mode!");
+		lbm = SSPCR1_LBM_YES;
+	} else {
+		DMSG("Starting in regular (non-loopback) mode!");
+		lbm = SSPCR1_LBM_NO;
+	}
+
+	DMSG("Set Serial Clock Rate (SCR), SPI mode (phase and clock)");
+	DMSG("Set frame format (SPI) and data size (8- or 16-bit)");
+	io_mask16(pd->base + SSPCR0, SHIFT_U32(scr, 8) | mode | SSPCR0_FRF_SPI |
+		data_size, MASK_16);
+
+	DMSG("Set master mode, disable SSP, set loopback mode");
+	io_mask8(pd->base + SSPCR1, SSPCR1_SOD_DISABLE | SSPCR1_MS_MASTER |
+		SSPCR1_SSE_DISABLE | lbm, MASK_4);
+
+	DMSG("Set clock prescale");
+	io_mask8(pd->base + SSPCPSR, cpsdvr, SSPCPSR_CPSDVR);
+
+	DMSG("Disable interrupts");
+	io_mask8(pd->base + SSPIMSC, 0, MASK_4);
+
+	DMSG("Clear interrupts");
+	io_mask8(pd->base + SSPICR, SSPICR_RORIC | SSPICR_RTIC,
+		SSPICR_RORIC | SSPICR_RTIC);
+
+	DMSG("Empty FIFO before starting");
+	pl022_flush_fifo(pd);
+}
+
+static void pl022_start(struct spi_chip *chip)
+{
+	struct pl022_data *pd = container_of(chip, struct pl022_data, chip);
+
+	DMSG("Enable SSP");
+	io_mask8(pd->base + SSPCR1, SSPCR1_SSE_ENABLE, SSPCR1_SSE);
+
+	pl022_control_cs(chip, GPIO_LEVEL_LOW);
+}
+
+static void pl022_end(struct spi_chip *chip)
+{
+	struct pl022_data *pd = container_of(chip, struct pl022_data, chip);
+
+	pl022_control_cs(chip, GPIO_LEVEL_HIGH);
+
+	DMSG("Disable SSP");
+	io_mask8(pd->base + SSPCR1, SSPCR1_SSE_DISABLE, SSPCR1_SSE);
+}
+
+static const struct spi_ops pl022_ops = {
+	.configure = pl022_configure,
+	.start = pl022_start,
+	.txrx8 = pl022_txrx8,
+	.txrx16 = pl022_txrx16,
+	.end = pl022_end,
+};
+
+void pl022_init(struct pl022_data *pd)
+{
+	assert(pd);
+	pd->chip.ops = &pl022_ops;
+}
diff --git a/core/drivers/pl050.c b/core/drivers/pl050.c
new file mode 100644
index 0000000..fa5feab
--- /dev/null
+++ b/core/drivers/pl050.c
@@ -0,0 +1,102 @@
+/*
+ * Copyright (c) 2016, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+#include <compiler.h>
+#include <drivers/pl050.h>
+#include <util.h>
+#include <io.h>
+
+#define KMI_ICR		0x00
+#define KMI_STAT	0x04
+#define KMI_DATA	0x08
+#define KMI_CLKDIV	0x0c
+#define KMI_IR		0x10
+
+#define KMI_ICR_TYPE		(1 << 5)
+#define KMI_ICR_RXINTERN	(1 << 4)
+#define KMI_ICR_TXINTERN	(1 << 3)
+#define KMI_ICR_EN		(1 << 2)
+#define KMI_ICR_FKMID		(1 << 1)
+#define KMI_ICR_FKMIC		(1 << 0)
+
+#define KMI_STAT_TXEMPTY	(1 << 6)
+#define KMI_STAT_TXBUSY		(1 << 5)
+#define KMI_STAT_RXFULL		(1 << 4)
+#define KMI_STAT_RXBUSY		(1 << 3)
+#define KMI_STAT_RXPARITY	(1 << 2)
+#define KMI_STAT_KMIC		(1 << 1)
+#define KMI_STAT_KMID		(1 << 0)
+
+#define KMI_IR_TXINTR		(1 << 1)
+#define KMI_IR_RXINTR		(1 << 0)
+
+static bool pl050_have_rx_data(struct serial_chip *chip)
+{
+	struct pl050_data *pd = container_of(chip, struct pl050_data, chip);
+
+	return !!(read8(pd->base + KMI_STAT) & KMI_STAT_RXFULL);
+}
+
+static int pl050_getchar(struct serial_chip *chip)
+{
+	struct pl050_data *pd = container_of(chip, struct pl050_data, chip);
+
+	while (!pl050_have_rx_data(chip))
+		;
+	return read8(pd->base + KMI_DATA);
+}
+
+static void pl050_flush(struct serial_chip *chip)
+{
+	struct pl050_data *pd = container_of(chip, struct pl050_data, chip);
+
+	while (!(read8(pd->base + KMI_STAT) & KMI_STAT_TXEMPTY))
+		;
+}
+
+static void pl050_putc(struct serial_chip *chip, int ch)
+{
+	struct pl050_data *pd = container_of(chip, struct pl050_data, chip);
+
+	pl050_flush(chip);
+	write8(ch, pd->base + KMI_DATA);
+}
+
+static const struct serial_ops pl050_ops = {
+	.putc = pl050_putc,
+	.flush = pl050_flush,
+	.have_rx_data = pl050_have_rx_data,
+	.getchar = pl050_getchar,
+};
+
+void pl050_init(struct pl050_data *pd, vaddr_t base, uint32_t clk)
+{
+	pd->base = base;
+	pd->chip.ops = &pl050_ops;
+
+	write8(KMI_ICR_RXINTERN | KMI_ICR_EN, pd->base + KMI_ICR);
+	write8(clk, base + KMI_CLKDIV);
+}
diff --git a/core/drivers/pl061_gpio.c b/core/drivers/pl061_gpio.c
new file mode 100644
index 0000000..d670c44
--- /dev/null
+++ b/core/drivers/pl061_gpio.c
@@ -0,0 +1,243 @@
+/*
+ * Copyright (c) 2016, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <assert.h>
+#include <drivers/pl061_gpio.h>
+#include <io.h>
+#include <trace.h>
+#include <util.h>
+
+#ifndef PLAT_PL061_MAX_GPIOS
+# define PLAT_PL061_MAX_GPIOS	32
+#endif	/* PLAT_PL061_MAX_GPIOS */
+
+#define MAX_GPIO_DEVICES	((PLAT_PL061_MAX_GPIOS + \
+	(GPIOS_PER_PL061 - 1)) / GPIOS_PER_PL061)
+
+#define GPIOS_PER_PL061		8
+
+/* gpio register offsets */
+#define GPIODIR		0x400
+#define GPIOIS		0x404
+#define GPIOIBE		0x408
+#define GPIOIEV		0x40C
+#define GPIOIE		0x410
+#define GPIORIS		0x414
+#define GPIOMIS		0x418
+#define GPIOIC		0x41C
+#define GPIOAFSEL	0x420
+
+/* gpio register masks */
+#define GPIOIE_ENABLED		SHIFT_U32(1, 0)
+#define GPIOIE_MASKED		SHIFT_U32(0, 0)
+#define GPIOAFSEL_HW		SHIFT_U32(1, 0)
+#define GPIOAFSEL_SW		SHIFT_U32(0, 0)
+#define GPIODIR_OUT			SHIFT_U32(1, 0)
+#define GPIODIR_IN			SHIFT_U32(0, 0)
+
+static vaddr_t pl061_reg_base[MAX_GPIO_DEVICES];
+
+static enum gpio_dir pl061_get_direction(unsigned int gpio_pin)
+{
+	vaddr_t base_addr;
+	uint8_t data;
+	unsigned int offset;
+
+	assert(gpio_pin < PLAT_PL061_MAX_GPIOS);
+
+	base_addr = pl061_reg_base[gpio_pin / GPIOS_PER_PL061];
+	offset = gpio_pin % GPIOS_PER_PL061;
+	data = read8(base_addr + GPIODIR);
+	if (data & BIT(offset))
+		return GPIO_DIR_OUT;
+	return GPIO_DIR_IN;
+}
+
+static void pl061_set_direction(unsigned int gpio_pin, enum gpio_dir direction)
+{
+	vaddr_t base_addr;
+	uint8_t data;
+	unsigned int offset;
+
+	assert(gpio_pin < PLAT_PL061_MAX_GPIOS);
+
+	base_addr = pl061_reg_base[gpio_pin / GPIOS_PER_PL061];
+	offset = gpio_pin % GPIOS_PER_PL061;
+	if (direction == GPIO_DIR_OUT) {
+		data = read8(base_addr + GPIODIR) | BIT(offset);
+		write8(data, base_addr + GPIODIR);
+	} else {
+		data = read8(base_addr + GPIODIR) & ~BIT(offset);
+		write8(data, base_addr + GPIODIR);
+	}
+}
+
+/*
+ * The offset of GPIODATA register is 0.
+ * The values read from GPIODATA are determined for each bit, by the mask bit
+ * derived from the address used to access the data register, PADDR[9:2].
+ * Bits that are 1 in the address mask cause the corresponding bits in GPIODATA
+ * to be read, and bits that are 0 in the address mask cause the corresponding
+ * bits in GPIODATA to be read as 0, regardless of their value.
+ */
+static enum gpio_level pl061_get_value(unsigned int gpio_pin)
+{
+	vaddr_t base_addr;
+	unsigned int offset;
+
+	assert(gpio_pin < PLAT_PL061_MAX_GPIOS);
+
+	base_addr = pl061_reg_base[gpio_pin / GPIOS_PER_PL061];
+	offset = gpio_pin % GPIOS_PER_PL061;
+	if (read8(base_addr + BIT(offset + 2)))
+		return GPIO_LEVEL_HIGH;
+	return GPIO_LEVEL_LOW;
+}
+
+/*
+ * In order to write GPIODATA, the corresponding bits in the mask, resulting
+ * from the address bus, PADDR[9:2], must be HIGH. Otherwise the bit values
+ * remain unchanged by the write.
+ */
+static void pl061_set_value(unsigned int gpio_pin, enum gpio_level value)
+{
+	vaddr_t base_addr;
+	unsigned int offset;
+
+	assert(gpio_pin < PLAT_PL061_MAX_GPIOS);
+
+	base_addr = pl061_reg_base[gpio_pin / GPIOS_PER_PL061];
+	offset = gpio_pin % GPIOS_PER_PL061;
+	if (value == GPIO_LEVEL_HIGH)
+		write8(BIT(offset), base_addr + BIT(offset + 2));
+	else
+		write8(0, base_addr + BIT(offset + 2));
+}
+
+static enum gpio_interrupt pl061_get_interrupt(unsigned int gpio_pin)
+{
+	vaddr_t base_addr;
+	uint8_t data;
+	unsigned int offset;
+
+	assert(gpio_pin < PLAT_PL061_MAX_GPIOS);
+
+	base_addr = pl061_reg_base[gpio_pin / GPIOS_PER_PL061];
+	offset = gpio_pin % GPIOS_PER_PL061;
+	data = read8(base_addr + GPIOIE);
+	if (data & BIT(offset))
+		return GPIO_INTERRUPT_ENABLE;
+	return GPIO_INTERRUPT_DISABLE;
+}
+
+static void pl061_set_interrupt(unsigned int gpio_pin,
+	enum gpio_interrupt ena_dis)
+{
+	vaddr_t base_addr;
+	uint8_t data;
+	unsigned int offset;
+
+	assert(gpio_pin < PLAT_PL061_MAX_GPIOS);
+
+	base_addr = pl061_reg_base[gpio_pin / GPIOS_PER_PL061];
+	offset = gpio_pin % GPIOS_PER_PL061;
+	if (ena_dis == GPIO_INTERRUPT_ENABLE) {
+		data = read8(base_addr + GPIOIE) | BIT(offset);
+		write8(data, base_addr + GPIOIE);
+	} else {
+		data = read8(base_addr + GPIOIE) & ~BIT(offset);
+		write8(data, base_addr + GPIOIE);
+	}
+}
+
+/*
+ * Register the PL061 GPIO controller with a base address and the offset
+ * of start pin in this GPIO controller.
+ * This function is called after pl061_init().
+ */
+void pl061_register(vaddr_t base_addr, unsigned int gpio_dev)
+{
+	assert(gpio_dev < MAX_GPIO_DEVICES);
+
+	pl061_reg_base[gpio_dev] = base_addr;
+}
+
+static const struct gpio_ops pl061_ops = {
+	.get_direction = pl061_get_direction,
+	.set_direction = pl061_set_direction,
+	.get_value = pl061_get_value,
+	.set_value = pl061_set_value,
+	.get_interrupt = pl061_get_interrupt,
+	.set_interrupt = pl061_set_interrupt,
+};
+
+/*
+ * Initialize PL061 GPIO controller
+ */
+void pl061_init(struct pl061_data *pd)
+{
+	COMPILE_TIME_ASSERT(PLAT_PL061_MAX_GPIOS > 0);
+
+	assert(pd);
+	pd->chip.ops = &pl061_ops;
+}
+
+enum pl061_mode_control pl061_get_mode_control(unsigned int gpio_pin)
+{
+	vaddr_t base_addr;
+	uint8_t data;
+	unsigned int offset;
+
+	assert(gpio_pin < PLAT_PL061_MAX_GPIOS);
+
+	base_addr = pl061_reg_base[gpio_pin / GPIOS_PER_PL061];
+	offset = gpio_pin % GPIOS_PER_PL061;
+	data = read8(base_addr + GPIOAFSEL);
+	if (data & BIT(offset))
+		return PL061_MC_HW;
+	return PL061_MC_SW;
+}
+
+void pl061_set_mode_control(unsigned int gpio_pin,
+	enum pl061_mode_control hw_sw)
+{
+	vaddr_t base_addr;
+	uint8_t data;
+	unsigned int offset;
+
+	assert(gpio_pin < PLAT_PL061_MAX_GPIOS);
+
+	base_addr = pl061_reg_base[gpio_pin / GPIOS_PER_PL061];
+	offset = gpio_pin % GPIOS_PER_PL061;
+	if (hw_sw == PL061_MC_HW) {
+		data = read8(base_addr + GPIOAFSEL) | BIT(offset);
+		write8(data, base_addr + GPIOAFSEL);
+	} else {
+		data = read8(base_addr + GPIOAFSEL) & ~BIT(offset);
+		write8(data, base_addr + GPIOAFSEL);
+	}
+}
diff --git a/core/drivers/pl111.c b/core/drivers/pl111.c
new file mode 100644
index 0000000..de27624
--- /dev/null
+++ b/core/drivers/pl111.c
@@ -0,0 +1,126 @@
+/*
+ * Copyright (c) 2016, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <compiler.h>
+#include <drivers/pl111.h>
+#include <io.h>
+#include <string.h>
+#include <types_ext.h>
+#include <util.h>
+
+#define CLCD_TIM0		0x000
+#define CLCD_TIM1		0x004
+#define CLCD_TIM2		0x008
+#define CLCD_TIM3		0x00c
+#define CLCD_UBASE		0x010
+#define CLCD_LBASE		0x014
+#define CLCD_CNTL		0x018
+#define CLCD_IMSC		0x01c
+#define CLCD_CRSRIMAGE0		0x800
+#define CLCD_CRSRCTRL		0xc00
+#define CLCD_CRSRCFG		0xc04
+#define CLCD_CRSRPLT0		0xc08
+#define CLCD_CRSRXY		0xc10
+
+#define CLCD_TIM0_HBP_SHIFT	24
+#define CLCD_TIM0_HBP_MASK	0x7f
+#define CLCD_TIM0_HFP_SHIFT	16
+#define CLCD_TIM0_HFP_MASK	0x7f
+#define CLCD_TIM0_HSW_SHIFT	8
+#define CLCD_TIM0_HSW_MASK	0x7f
+#define CLCD_TIM0_PPL_SHIFT	2
+#define CLCD_TIM0_PPL_MASK	0x1f
+
+#define CLCD_TIM1_VBP_SHIFT	24
+#define CLCD_TIM1_VBP_MASK	0x7f
+#define CLCD_TIM1_VFP_SHIFT	16
+#define CLCD_TIM1_VFP_MASK	0x7f
+#define CLCD_TIM1_VSW_SHIFT	10
+#define CLCD_TIM1_VSW_MASK	0x1f
+#define CLCD_TIM1_LPP_SHIFT	0
+#define CLCD_TIM1_LPP_MASK	0x1ff
+
+#define CLCD_TIM2_CPL_SHIFT	16
+#define CLCD_TIM2_CPL_MASK	0x1ff
+#define CLCD_TIM2_BCD		(1 << 26)
+
+#define CLCD_CNTL_EN		(1 << 0)
+#define CLCD_CNTL_BPP_SHIFT	1
+#define CLCD_CNTL_BPP_MASK	0x7
+#define CLCD_CNTL_BPP_24BPP	5
+#define CLCD_CNTL_TFT		(1 << 5)
+#define CLCD_CNTL_PWR		(1 << 11)
+
+#define CLCD_CRSRXY_X_SHIFT	0
+#define CLCD_CRSRXY_X_MASK	0x1ff
+#define CLCD_CRSRXY_Y_SHIFT	16
+#define CLCD_CRSRXY_Y_MASK	0x1ff
+
+
+void pl111_init(vaddr_t base, paddr_t frame_base,
+		const struct pl111_videomode *m)
+{
+	uint32_t v;
+
+	v = SHIFT_U32((m->hactive / 16) - 1, CLCD_TIM0_PPL_SHIFT) |
+	    SHIFT_U32(m->hback_porch - 1, CLCD_TIM0_HBP_SHIFT) |
+	    SHIFT_U32(m->hfront_porch - 1, CLCD_TIM0_HFP_SHIFT) |
+	    SHIFT_U32(m->hsync_len - 1, CLCD_TIM0_HSW_SHIFT);
+	write32(v, base + CLCD_TIM0);
+
+	v = SHIFT_U32(m->vactive - 1, CLCD_TIM1_LPP_SHIFT) |
+	    SHIFT_U32(m->vsync_len, CLCD_TIM1_VSW_SHIFT) |
+	    SHIFT_U32(m->vfront_porch, CLCD_TIM1_VFP_SHIFT) |
+	    SHIFT_U32(m->vback_porch, CLCD_TIM1_VBP_SHIFT);
+	write32(v, base + CLCD_TIM1);
+
+	v = SHIFT_U32(m->hactive - 1, CLCD_TIM2_CPL_SHIFT) | CLCD_TIM2_BCD;
+	write32(v, base + CLCD_TIM2);
+
+	write32(0, base + CLCD_TIM3);
+	write32(frame_base, base + CLCD_UBASE);
+	write32(0, base + CLCD_LBASE);
+	write32(0, base + CLCD_IMSC);
+
+	write32(0, base + CLCD_CRSRCFG);
+
+	v = CLCD_CNTL_EN | SHIFT_U32(CLCD_CNTL_BPP_24BPP, CLCD_CNTL_BPP_SHIFT) |
+	    CLCD_CNTL_TFT | CLCD_CNTL_PWR;
+	write32(v, base + CLCD_CNTL);
+}
+
+void pl111_cursor(vaddr_t base, bool on)
+{
+	write32(on ? 1 : 0, base + CLCD_CRSRCTRL);
+}
+
+void pl111_set_cursor_xy(vaddr_t base, size_t x, size_t y)
+{
+	write32(SHIFT_U32(x, CLCD_CRSRXY_X_SHIFT) |
+		SHIFT_U32(y, CLCD_CRSRXY_Y_SHIFT),
+		base + CLCD_CRSRXY);
+}
diff --git a/core/drivers/ps2mouse.c b/core/drivers/ps2mouse.c
new file mode 100644
index 0000000..893b247
--- /dev/null
+++ b/core/drivers/ps2mouse.c
@@ -0,0 +1,160 @@
+/*
+ * Copyright (c) 2016, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <types_ext.h>
+#include <drivers/ps2mouse.h>
+#include <drivers/serial.h>
+#include <string.h>
+#include <keep.h>
+#include <trace.h>
+
+#define PS2_CMD_RESET			0xff
+#define PS2_CMD_ACK			0xfa
+#define PS2_CMD_ENABLE_DATA_REPORTING	0xf4
+#define PS2_BAT_OK			0xaa
+#define PS2_MOUSE_ID			0x00
+
+#define PS2_BYTE0_Y_OVERFLOW		(1 << 7)
+#define PS2_BYTE0_X_OVERFLOW		(1 << 6)
+#define PS2_BYTE0_Y_SIGN		(1 << 5)
+#define PS2_BYTE0_X_SIGN		(1 << 4)
+#define PS2_BYTE0_ALWAYS_ONE		(1 << 3)
+#define PS2_BYTE0_MIDDLE_DOWN		(1 << 2)
+#define PS2_BYTE0_RIGHT_DOWN		(1 << 1)
+#define PS2_BYTE0_LEFT_DOWN		(1 << 0)
+
+static void call_callback(struct ps2mouse_data *d, uint8_t byte1,
+			  uint8_t byte2, uint8_t byte3)
+{
+	uint8_t button;
+	int16_t xdelta;
+	int16_t ydelta;
+
+	button = byte1 & (PS2_BYTE0_MIDDLE_DOWN | PS2_BYTE0_RIGHT_DOWN |
+			  PS2_BYTE0_LEFT_DOWN);
+
+	if (byte1 & PS2_BYTE0_X_OVERFLOW) {
+		xdelta = byte1 & PS2_BYTE0_X_SIGN ? -255 : 255;
+	} else {
+		xdelta = byte2;
+		if (byte1 & PS2_BYTE0_X_SIGN)
+			xdelta |= 0xff00; /* sign extend */
+	}
+
+	if (byte1 & PS2_BYTE0_Y_OVERFLOW) {
+		ydelta = byte1 & PS2_BYTE0_Y_SIGN ? -255 : 255;
+	} else {
+		ydelta = byte3;
+		if (byte1 & PS2_BYTE0_Y_SIGN)
+			ydelta |= 0xff00; /* sign extend */
+	}
+
+	d->callback(d->callback_data, button, xdelta, -ydelta);
+}
+
+static void psm_consume(struct ps2mouse_data *d, uint8_t b)
+{
+	switch (d->state) {
+	case PS2MS_RESET:
+		if (b != PS2_CMD_ACK)
+			goto reset;
+		d->state = PS2MS_INIT;
+		return;
+	case PS2MS_INIT:
+		if (b != PS2_BAT_OK)
+			goto reset;
+		d->state = PS2MS_INIT2;
+		return;
+	case PS2MS_INIT2:
+		if (b != PS2_MOUSE_ID) {
+			EMSG("Unexpected byte 0x%x in state %d", b, d->state);
+			d->state = PS2MS_INACTIVE;
+			return;
+		}
+		d->state = PS2MS_INIT3;
+		d->serial->ops->putc(d->serial, PS2_CMD_ENABLE_DATA_REPORTING);
+		return;
+	case PS2MS_INIT3:
+		d->state = PS2MS_ACTIVE1;
+		return;
+	case PS2MS_ACTIVE1:
+		if (!(b & PS2_BYTE0_ALWAYS_ONE))
+			goto reset;
+		d->bytes[0] = b;
+		d->state = PS2MS_ACTIVE2;
+		return;
+	case PS2MS_ACTIVE2:
+		d->bytes[1] = b;
+		d->state = PS2MS_ACTIVE3;
+		return;
+	case PS2MS_ACTIVE3:
+		d->state = PS2MS_ACTIVE1;
+		call_callback(d, d->bytes[0], d->bytes[1], b);
+		return;
+	default:
+		EMSG("Unexpected byte 0x%x in state %d", b, d->state);
+		return;
+	}
+
+reset:
+	EMSG("Unexpected byte 0x%x in state %d, resetting", b, d->state);
+	d->state = PS2MS_RESET;
+	d->serial->ops->putc(d->serial, PS2_CMD_RESET);
+}
+
+static enum itr_return ps2mouse_itr_cb(struct itr_handler *h)
+{
+	struct ps2mouse_data *d = h->data;
+
+	if (!d->serial->ops->have_rx_data(d->serial))
+		return ITRR_NONE;
+
+	while (true) {
+		psm_consume(d, d->serial->ops->getchar(d->serial));
+		if (!d->serial->ops->have_rx_data(d->serial))
+			return ITRR_HANDLED;
+	}
+}
+KEEP_PAGER(ps2mouse_itr_cb);
+
+void ps2mouse_init(struct ps2mouse_data *d, struct serial_chip *serial,
+		   size_t serial_it, ps2mouse_callback cb, void *cb_data)
+{
+	memset(d, 0, sizeof(*d));
+	d->serial = serial;
+	d->state = PS2MS_RESET;
+	d->itr_handler.it = serial_it;
+	d->itr_handler.flags = ITRF_TRIGGER_LEVEL;
+	d->itr_handler.handler = ps2mouse_itr_cb;
+	d->itr_handler.data = d;
+	d->callback = cb;
+	d->callback_data = cb_data;
+
+	itr_add(&d->itr_handler);
+	itr_enable(serial_it);
+	d->serial->ops->putc(d->serial, PS2_CMD_RESET);
+}
diff --git a/core/drivers/scif.c b/core/drivers/scif.c
new file mode 100644
index 0000000..02fd49d
--- /dev/null
+++ b/core/drivers/scif.c
@@ -0,0 +1,65 @@
+/*
+ * Copyright (c) 2016, GlobalLogic
+ * 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 <compiler.h>
+#include <io.h>
+#include <util.h>
+#include <drivers/scif.h>
+
+#define SCIF_SCFSR		(0x10)
+#define SCIF_SCFTDR		(0x0C)
+#define SCIF_SCFCR		(0x18)
+#define SCIF_SCFDR		(0x1C)
+
+#define SCFSR_TDFE		BIT(5)
+#define SCFSR_TEND		BIT(6)
+
+#define SCFDR_T_SHIFT		8
+
+#define SCIF_TX_FIFO_SIZE	16
+
+void scif_uart_flush(vaddr_t base)
+{
+	while (!(read16(base + SCIF_SCFSR) & SCFSR_TEND))
+		;
+}
+
+void scif_uart_init(vaddr_t base)
+{
+	/* Bootloader should initialize device for us */
+	scif_uart_flush(base);
+}
+
+void scif_uart_putc(int ch, vaddr_t base)
+{
+	/* Wait until there is space in the FIFO */
+	while ((read16(base + SCIF_SCFDR) >> SCFDR_T_SHIFT) >=
+		SCIF_TX_FIFO_SIZE)
+		;
+	write8(ch, base + SCIF_SCFTDR);
+	write16(read16(base + SCIF_SCFSR) & ~(SCFSR_TEND | SCFSR_TDFE),
+		base + SCIF_SCFSR);
+}
diff --git a/core/drivers/serial8250_uart.c b/core/drivers/serial8250_uart.c
new file mode 100644
index 0000000..1dd21de
--- /dev/null
+++ b/core/drivers/serial8250_uart.c
@@ -0,0 +1,93 @@
+/*
+ * 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 <platform_config.h>
+
+#include <drivers/serial8250_uart.h>
+#include <console.h>
+#include <io.h>
+#include <compiler.h>
+
+/* uart register defines */
+#define UART_RHR	0x0
+#define UART_THR	0x0
+#define UART_IER	0x4
+#define UART_ISR	0x8
+#define UART_FCR	0x8
+#define UART_LCR	0xc
+#define UART_MCR	0x10
+#define UART_LSR	0x14
+#define UART_MSR	0x18
+#define UART_SPR	0x1c
+
+/* uart status register bits */
+#define LSR_TEMT	0x40 /* Transmitter empty */
+#define LSR_THRE	0x20 /* Transmit-hold-register empty */
+#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)
+{
+	/*
+	 * do nothing, debug uart(uart0) share with normal world,
+	 * everything for uart0 is ready now.
+	 */
+}
+
+void serial8250_uart_flush_tx_fifo(vaddr_t base)
+{
+	while (1) {
+		uint8_t state = read8(base + UART_LSR);
+
+		/* waiting transmit fifo empty */
+		if ((state & LSR_EMPTY) == LSR_EMPTY)
+			break;
+	}
+}
+
+bool serial8250_uart_have_rx_data(vaddr_t base)
+{
+	return (read32(base + UART_LSR) & LSR_DR);
+}
+
+void serial8250_uart_putc(int ch, vaddr_t base)
+{
+	serial8250_uart_flush_tx_fifo(base);
+
+	/* 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. */
+		;
+	}
+	return read8(base + UART_RHR);
+}
+
diff --git a/core/drivers/sprd_uart.c b/core/drivers/sprd_uart.c
new file mode 100644
index 0000000..fdaa1b6
--- /dev/null
+++ b/core/drivers/sprd_uart.c
@@ -0,0 +1,78 @@
+/*
+ * Copyright (c) 2016, Spreadtrum Communications Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+#include <io.h>
+#include <drivers/sprd_uart.h>
+
+/* Register definitions */
+#define UART_TXD		0x0000
+#define UART_RXD		0x0004
+#define UART_STS1		0x000C /* data number in TX and RX fifo */
+
+/* Register Bit Fields*/
+#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)
+{
+	return read32(base + reg);
+}
+
+static void sprd_uart_write(vaddr_t base, uint32_t reg, uint32_t value)
+{
+	write32(value, base + reg);
+}
+
+static void sprd_uart_wait_xmit_done(vaddr_t base)
+{
+	while (sprd_uart_read(base, UART_STS1) & STS1_TXF_CNT_MASK)
+		;
+}
+
+static void sprd_uart_wait_rx_data(vaddr_t base)
+{
+	while (!(sprd_uart_read(base, UART_STS1) & STS1_RXF_CNT_MASK))
+		;
+}
+
+void sprd_uart_flush(vaddr_t base)
+{
+	sprd_uart_wait_xmit_done(base);
+}
+
+void sprd_uart_putc(vaddr_t base, unsigned char ch)
+{
+	sprd_uart_wait_xmit_done(base);
+
+	sprd_uart_write(base, UART_TXD, (uint32_t)ch);
+}
+
+unsigned char sprd_uart_getc(vaddr_t base)
+{
+	sprd_uart_wait_rx_data(base);
+
+	return sprd_uart_read(base, UART_RXD) & 0xff;
+}
diff --git a/core/drivers/sub.mk b/core/drivers/sub.mk
new file mode 100644
index 0000000..609b378
--- /dev/null
+++ b/core/drivers/sub.mk
@@ -0,0 +1,19 @@
+srcs-$(CFG_CDNS_UART) += cdns_uart.c
+srcs-$(CFG_PL011) += pl011.c
+srcs-$(CFG_PL050) += pl050.c
+srcs-$(CFG_TZC400) += tzc400.c
+srcs-$(CFG_PS2MOUSE) += ps2mouse.c
+srcs-$(CFG_PL111) += pl111.c
+srcs-$(CFG_FRAME_BUFFER) += frame_buffer.c
+srcs-$(CFG_GIC) += gic.c
+srcs-$(CFG_PL061) += pl061_gpio.c
+srcs-$(CFG_PL022) += pl022_spi.c
+srcs-$(CFG_SUNXI_UART) += sunxi_uart.c
+srcs-$(CFG_8250_UART) += serial8250_uart.c
+srcs-$(CFG_16550_UART) += ns16550.c
+srcs-$(CFG_IMX_UART) += imx_uart.c
+srcs-$(CFG_SPRD_UART) += sprd_uart.c
+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
diff --git a/core/drivers/sunxi_uart.c b/core/drivers/sunxi_uart.c
new file mode 100644
index 0000000..433c423
--- /dev/null
+++ b/core/drivers/sunxi_uart.c
@@ -0,0 +1,97 @@
+/*
+ * Copyright (c) 2014, Allwinner Technology Co., Ltd.
+ * 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 <drivers/sunxi_uart.h>
+#include <io.h>
+#include <compiler.h>
+
+/* uart register defines */
+#define UART_REG_RBR 	(0x00)
+#define UART_REG_THR 	(0x00)
+#define UART_REG_DLL 	(0x00)
+#define UART_REG_DLH 	(0x04)
+#define UART_REG_IER 	(0x04)
+#define UART_REG_IIR 	(0x08)
+#define UART_REG_FCR 	(0x08)
+#define UART_REG_LCR 	(0x0c)
+#define UART_REG_MCR 	(0x10)
+#define UART_REG_LSR 	(0x14)
+#define UART_REG_MSR 	(0x18)
+#define UART_REG_SCH 	(0x1c)
+#define UART_REG_USR 	(0x7c)
+#define UART_REG_TFL 	(0x80)
+#define UART_REG_RFL 	(0x84)
+#define UART_REG_HALT	(0xa4)
+
+/* uart status register bits */
+#define UART_REG_USR_BUSY (0x1 << 0x0)
+#define UART_REG_USR_TFNF (0x1 << 0x1)
+#define UART_REG_USR_TFE  (0x1 << 0x2)
+#define UART_REG_USR_RFNE (0x1 << 0x3)
+#define UART_REG_USR_RFF  (0x1 << 0x4)
+
+void sunxi_uart_init(vaddr_t __unused base)
+{
+	/* do nothing, debug uart(uart0) share with normal world,
+	 * everything for uart0 is ready now.
+	 */
+}
+
+void sunxi_uart_flush(vaddr_t base)
+{
+	while (read32(base + UART_REG_TFL)) {
+		/* waiting transmit fifo empty */
+		;
+	}
+}
+
+bool sunxi_uart_have_rx_data(vaddr_t base)
+{
+	return read32(base + UART_REG_RFL);
+}
+
+void sunxi_uart_putc(int ch, vaddr_t base)
+{
+	while (!(read32(base + UART_REG_USR) & UART_REG_USR_TFNF)) {
+		/* transmit fifo is full, waiting again. */
+		;
+	}
+
+	/* write out charset to transmit fifo */
+	write8(ch, base + UART_REG_THR);
+}
+
+int sunxi_uart_getchar(vaddr_t base)
+{
+	while (!sunxi_uart_have_rx_data(base)) {
+		/* transmit fifo is empty, waiting again. */
+		;
+	}
+	return read32(base + UART_REG_RBR) & 0xff;
+}
+
diff --git a/core/drivers/tzc400.c b/core/drivers/tzc400.c
new file mode 100644
index 0000000..9fee0b4
--- /dev/null
+++ b/core/drivers/tzc400.c
@@ -0,0 +1,417 @@
+/*
+ * 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.
+ */
+ /*
+ * Copyright (c) 2014, ARM Limited and Contributors. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * 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.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE 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 <drivers/tzc400.h>
+#include <io.h>
+#include <kernel/panic.h>
+#include <stddef.h>
+#include <trace.h>
+#include <util.h>
+
+/*
+ * Implementation defined values used to validate inputs later.
+ * Filters : max of 4 ; 0 to 3
+ * Regions : max of 9 ; 0 to 8
+ * Address width : Values between 32 to 64
+ */
+struct tzc_instance {
+	vaddr_t base;
+	uint8_t addr_width;
+	uint8_t num_filters;
+	uint8_t num_regions;
+};
+
+static struct tzc_instance tzc;
+
+
+static uint32_t tzc_read_build_config(vaddr_t base)
+{
+	return read32(base + BUILD_CONFIG_OFF);
+}
+
+static uint32_t tzc_read_gate_keeper(vaddr_t base)
+{
+	return read32(base + GATE_KEEPER_OFF);
+}
+
+static void tzc_write_gate_keeper(vaddr_t base, uint32_t val)
+{
+	write32(val, base + GATE_KEEPER_OFF);
+}
+
+static void tzc_write_action(vaddr_t base, enum tzc_action action)
+{
+	write32(action, base + ACTION_OFF);
+}
+
+static void tzc_write_region_base_low(vaddr_t base, uint32_t region,
+				      uint32_t val)
+{
+	write32(val, base + REGION_BASE_LOW_OFF +
+		REGION_NUM_OFF(region));
+}
+
+static void tzc_write_region_base_high(vaddr_t base, uint32_t region,
+				       uint32_t val)
+{
+	write32(val, base + REGION_BASE_HIGH_OFF +
+		REGION_NUM_OFF(region));
+}
+
+static void tzc_write_region_top_low(vaddr_t base, uint32_t region,
+				     uint32_t val)
+{
+	write32(val, base + REGION_TOP_LOW_OFF +
+		REGION_NUM_OFF(region));
+}
+
+static void tzc_write_region_top_high(vaddr_t base, uint32_t region,
+				      uint32_t val)
+{
+	write32(val, base + REGION_TOP_HIGH_OFF +
+		REGION_NUM_OFF(region));
+}
+
+static void tzc_write_region_attributes(vaddr_t base, uint32_t region,
+					uint32_t val)
+{
+	write32(val, base + REGION_ATTRIBUTES_OFF +
+		REGION_NUM_OFF(region));
+}
+
+static void tzc_write_region_id_access(vaddr_t base, uint32_t region,
+				       uint32_t val)
+{
+	write32(val, base + REGION_ID_ACCESS_OFF +
+		REGION_NUM_OFF(region));
+}
+
+static uint32_t tzc_read_component_id(vaddr_t base)
+{
+	uint32_t id;
+
+	id = read8(base + CID0_OFF);
+	id |= SHIFT_U32(read8(base + CID1_OFF), 8);
+	id |= SHIFT_U32(read8(base + CID2_OFF), 16);
+	id |= SHIFT_U32(read8(base + CID3_OFF), 24);
+
+	return id;
+}
+
+static uint32_t tzc_get_gate_keeper(vaddr_t base, uint8_t filter)
+{
+	uint32_t tmp;
+
+	tmp = (tzc_read_gate_keeper(base) >> GATE_KEEPER_OS_SHIFT) &
+		GATE_KEEPER_OS_MASK;
+
+	return (tmp >> filter) & GATE_KEEPER_FILTER_MASK;
+}
+
+/* This function is not MP safe. */
+static void tzc_set_gate_keeper(vaddr_t base, uint8_t filter, uint32_t val)
+{
+	uint32_t tmp;
+
+	/* Upper half is current state. Lower half is requested state. */
+	tmp = (tzc_read_gate_keeper(base) >> GATE_KEEPER_OS_SHIFT) &
+		GATE_KEEPER_OS_MASK;
+
+	if (val)
+		tmp |=  (1 << filter);
+	else
+		tmp &= ~(1 << filter);
+
+	tzc_write_gate_keeper(base, (tmp & GATE_KEEPER_OR_MASK) <<
+			      GATE_KEEPER_OR_SHIFT);
+
+	/* Wait here until we see the change reflected in the TZC status. */
+	while (((tzc_read_gate_keeper(base) >> GATE_KEEPER_OS_SHIFT) &
+		GATE_KEEPER_OS_MASK) != tmp)
+		;
+}
+
+
+void tzc_init(vaddr_t base)
+{
+	uint32_t tzc_id, tzc_build;
+
+	assert(base);
+	tzc.base = base;
+
+	/*
+	 * We expect to see a tzc400. Check component ID. The TZC-400 TRM shows
+	 * component ID is expected to be "0xB105F00D".
+	 */
+	tzc_id = tzc_read_component_id(tzc.base);
+	if (tzc_id != TZC400_COMPONENT_ID) {
+		EMSG("TZC : Wrong device ID (0x%x).\n", tzc_id);
+		panic();
+	}
+
+	/* Save values we will use later. */
+	tzc_build = tzc_read_build_config(tzc.base);
+	tzc.num_filters = ((tzc_build >> BUILD_CONFIG_NF_SHIFT) &
+			   BUILD_CONFIG_NF_MASK) + 1;
+	tzc.addr_width  = ((tzc_build >> BUILD_CONFIG_AW_SHIFT) &
+			   BUILD_CONFIG_AW_MASK) + 1;
+	tzc.num_regions = ((tzc_build >> BUILD_CONFIG_NR_SHIFT) &
+			   BUILD_CONFIG_NR_MASK) + 1;
+}
+
+static uint32_t addr_low(vaddr_t addr)
+{
+	return (uint32_t)addr;
+}
+
+static uint32_t addr_high(vaddr_t addr __unused)
+{
+#if (UINTPTR_MAX == UINT64_MAX)
+	return (addr >> 32);
+#else
+	return 0;
+#endif
+}
+
+
+/*
+ * `tzc_configure_region` is used to program regions into the TrustZone
+ * controller. A region can be associated with more than one filter. The
+ * associated filters are passed in as a bitmap (bit0 = filter0).
+ * NOTE:
+ * The region 0 covers the whole address space and is enabled on all filters,
+ * this cannot be changed. It is, however, possible to change some region 0
+ * permissions.
+ */
+void tzc_configure_region(uint32_t filters,
+			  uint8_t  region,
+			  vaddr_t  region_base,
+			  vaddr_t  region_top,
+			  enum tzc_region_attributes sec_attr,
+			  uint32_t ns_device_access)
+{
+	assert(tzc.base);
+
+	/* Do range checks on filters and regions. */
+	assert(((filters >> tzc.num_filters) == 0) &&
+	       (region < tzc.num_regions));
+
+	/*
+	 * Do address range check based on TZC configuration. A 64bit address is
+	 * the max and expected case.
+	 */
+#if (UINTPTR_MAX == UINT64_MAX)
+	assert(((region_top <= (UINT64_MAX >> (64 - tzc.addr_width))) &&
+		(region_base < region_top)));
+#endif
+	/* region_base and (region_top + 1) must be 4KB aligned */
+	assert(((region_base | (region_top + 1)) & (4096 - 1)) == 0);
+
+	assert(sec_attr <= TZC_REGION_S_RDWR);
+
+	/*
+	 * Inputs look ok, start programming registers.
+	 * All the address registers are 32 bits wide and have a LOW and HIGH
+	 * component used to construct a up to a 64bit address.
+	 */
+	tzc_write_region_base_low(tzc.base, region,
+				  addr_low(region_base));
+	tzc_write_region_base_high(tzc.base, region,
+				   addr_high(region_base));
+
+	tzc_write_region_top_low(tzc.base, region,
+				addr_low(region_top));
+	tzc_write_region_top_high(tzc.base, region,
+				addr_high(region_top));
+
+	/* Assign the region to a filter and set secure attributes */
+	tzc_write_region_attributes(tzc.base, region,
+		(sec_attr << REG_ATTR_SEC_SHIFT) | filters);
+
+	/*
+	 * Specify which non-secure devices have permission to access this
+	 * region.
+	 */
+	tzc_write_region_id_access(tzc.base, region, ns_device_access);
+}
+
+
+void tzc_set_action(enum tzc_action action)
+{
+	assert(tzc.base);
+
+	/*
+	 * - Currently no handler is provided to trap an error via interrupt
+	 *   or exception.
+	 * - The interrupt action has not been tested.
+	 */
+	tzc_write_action(tzc.base, action);
+}
+
+
+void tzc_enable_filters(void)
+{
+	uint32_t state;
+	uint32_t filter;
+
+	assert(tzc.base);
+
+	for (filter = 0; filter < tzc.num_filters; filter++) {
+		state = tzc_get_gate_keeper(tzc.base, filter);
+		if (state) {
+			/*
+			 * The TZC filter is already configured. Changing the
+			 * programmer's view in an active system can cause
+			 * unpredictable behavior therefore panic for now rather
+			 * than try to determine whether this is safe in this
+			 * instance. See:
+			 * http://infocenter.arm.com/help/index.jsp?\
+			 * topic=/com.arm.doc.ddi0504c/CJHHECBF.html
+			 */
+			EMSG("TZC : Filter %d Gatekeeper already enabled.\n",
+				filter);
+			panic();
+		}
+		tzc_set_gate_keeper(tzc.base, filter, 1);
+	}
+}
+
+
+void tzc_disable_filters(void)
+{
+	uint32_t filter;
+
+	assert(tzc.base);
+
+	/*
+	 * We don't do the same state check as above as the Gatekeepers are
+	 * disabled after reset.
+	 */
+	for (filter = 0; filter < tzc.num_filters; filter++)
+		tzc_set_gate_keeper(tzc.base, filter, 0);
+}
+
+#if TRACE_LEVEL >= TRACE_DEBUG
+
+static uint32_t tzc_read_region_attributes(vaddr_t base, uint32_t region)
+{
+	return read32(base + REGION_ATTRIBUTES_OFF + REGION_NUM_OFF(region));
+}
+
+static uint32_t tzc_read_region_base_low(vaddr_t base, uint32_t region)
+{
+	return read32(base + REGION_BASE_LOW_OFF + REGION_NUM_OFF(region));
+}
+
+static uint32_t tzc_read_region_base_high(vaddr_t base, uint32_t region)
+{
+	return read32(base + REGION_BASE_HIGH_OFF + REGION_NUM_OFF(region));
+}
+
+static uint32_t tzc_read_region_top_low(vaddr_t base, uint32_t region)
+{
+	return read32(base + REGION_TOP_LOW_OFF + REGION_NUM_OFF(region));
+}
+
+static uint32_t tzc_read_region_top_high(vaddr_t base, uint32_t region)
+{
+	return read32(base + REGION_TOP_HIGH_OFF + REGION_NUM_OFF(region));
+}
+
+#define	REGION_MAX		8
+static const __maybe_unused char * const tzc_attr_msg[] = {
+	"TZC_REGION_S_NONE",
+	"TZC_REGION_S_RD",
+	"TZC_REGION_S_WR",
+	"TZC_REGION_S_RDWR"
+};
+
+void tzc_dump_state(void)
+{
+	uint32_t n;
+	uint32_t temp_32reg, temp_32reg_h;
+
+	DMSG("enter");
+	for (n = 0; n <= REGION_MAX; n++) {
+		temp_32reg = tzc_read_region_attributes(tzc.base, n);
+		if (!(temp_32reg & REG_ATTR_F_EN_MASK))
+			continue;
+
+		DMSG("\n");
+		DMSG("region %d", n);
+		temp_32reg = tzc_read_region_base_low(tzc.base, n);
+		temp_32reg_h = tzc_read_region_base_high(tzc.base, n);
+		DMSG("region_base: 0x%08x%08x", temp_32reg_h, temp_32reg);
+		temp_32reg = tzc_read_region_top_low(tzc.base, n);
+		temp_32reg_h = tzc_read_region_top_high(tzc.base, n);
+		DMSG("region_top: 0x%08x%08x", temp_32reg_h, temp_32reg);
+		temp_32reg = tzc_read_region_attributes(tzc.base, n);
+		DMSG("secure rw: %s",
+		     tzc_attr_msg[temp_32reg >> REG_ATTR_SEC_SHIFT]);
+		if (temp_32reg & (1 << 0))
+			DMSG("filter 0 enable");
+		if (temp_32reg & (1 << 1))
+			DMSG("filter 1 enable");
+		if (temp_32reg & (1 << 2))
+			DMSG("filter 2 enable");
+		if (temp_32reg & (1 << 3))
+			DMSG("filter 3 enable");
+	}
+	DMSG("exit");
+}
+
+#endif /* CFG_TRACE_LEVEL >= TRACE_DEBUG */
diff --git a/core/include/console.h b/core/include/console.h
new file mode 100644
index 0000000..0fe8e49
--- /dev/null
+++ b/core/include/console.h
@@ -0,0 +1,36 @@
+/*
+ * Copyright (c) 2014, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef CONSOLE_H
+#define CONSOLE_H
+
+void console_init(void);
+void console_putc(int ch);
+void console_flush(void);
+
+#endif /* CONSOLE_H */
+
diff --git a/core/include/display.h b/core/include/display.h
new file mode 100644
index 0000000..7d433d5
--- /dev/null
+++ b/core/include/display.h
@@ -0,0 +1,38 @@
+/*
+ * Copyright (c) 2016, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef __DISPLAY_H
+#define __DISPLAY_H
+
+#include <drivers/frame_buffer.h>
+
+void display_init(void);
+struct frame_buffer *display_get_frame_buffer(void);
+void display_final(void);
+
+#endif /* __DISPLAY_H */
+
diff --git a/core/include/drivers/cdns_uart.h b/core/include/drivers/cdns_uart.h
new file mode 100644
index 0000000..6688fd3
--- /dev/null
+++ b/core/include/drivers/cdns_uart.h
@@ -0,0 +1,42 @@
+/*
+ * Copyright (c) 2016, Xilinx Inc
+ * 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 CDNS_UART_H
+#define CDNS_UART_H
+
+#include <types_ext.h>
+
+void cdns_uart_init(vaddr_t base, uint32_t uart_clk, uint32_t baud_rate);
+
+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);
+
+#endif /* CDNS_UART_H */
diff --git a/core/include/drivers/frame_buffer.h b/core/include/drivers/frame_buffer.h
new file mode 100644
index 0000000..aabbb84
--- /dev/null
+++ b/core/include/drivers/frame_buffer.h
@@ -0,0 +1,52 @@
+/*
+ * Copyright (c) 2016, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef __DRIVERS__FRAME_BUFFER_H
+#define __DRIVERS__FRAME_BUFFER_H
+
+#include <types_ext.h>
+
+enum frame_buffer_bpp {
+	FB_24BPP,
+};
+
+struct frame_buffer {
+	size_t width;
+	size_t height;
+	size_t width_dpi;
+	size_t height_dpi;
+	enum frame_buffer_bpp bpp;
+	void *base;
+};
+
+size_t frame_buffer_get_image_size(struct frame_buffer *fb, size_t width,
+			size_t height);
+void frame_buffer_clear(struct frame_buffer *fb, uint32_t color);
+void frame_buffer_set_image(struct frame_buffer *fb, size_t xpos, size_t ypos,
+			size_t width, size_t height, const void *image);
+
+#endif /*__DRIVERS__FRAME_BUFFER_H*/
diff --git a/core/include/drivers/gic.h b/core/include/drivers/gic.h
new file mode 100644
index 0000000..2032e06
--- /dev/null
+++ b/core/include/drivers/gic.h
@@ -0,0 +1,59 @@
+/*
+ * Copyright (c) 2016, Linaro Limited
+ * 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.
+ */
+
+#ifndef __DRIVERS_GIC_H
+#define __DRIVERS_GIC_H
+#include <types_ext.h>
+#include <kernel/interrupt.h>
+
+#define GIC_DIST_REG_SIZE	0x10000
+#define GIC_CPU_REG_SIZE	0x10000
+
+struct gic_data {
+	vaddr_t gicc_base;
+	vaddr_t gicd_base;
+	size_t max_it;
+	struct itr_chip chip;
+};
+
+/*
+ * The two gic_init_* functions initializes the struct gic_data which is
+ * then used by the other functions.
+ */
+
+void gic_init(struct gic_data *gd, paddr_t gicc_base, paddr_t gicd_base);
+/* initial base address only */
+void gic_init_base_addr(struct gic_data *gd, vaddr_t gicc_base,
+			vaddr_t gicd_base);
+/* initial cpu if only, mainly use for secondary cpu setup cpu interface */
+void gic_cpu_init(struct gic_data *gd);
+
+void gic_it_handle(struct gic_data *gd);
+
+void gic_dump_state(struct gic_data *gd);
+#endif /*__DRIVERS_GIC_H*/
diff --git a/core/include/drivers/hi16xx_uart.h b/core/include/drivers/hi16xx_uart.h
new file mode 100644
index 0000000..a7d4f0c
--- /dev/null
+++ b/core/include/drivers/hi16xx_uart.h
@@ -0,0 +1,50 @@
+/*
+ * Copyright (c) 2016, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/*
+ * UART driver for Hisilicon Hi16xx and Phosphor V660 (hip05) SoCs
+ */
+
+#ifndef HI16XX_UART_H
+#define HI16XX_UART_H
+
+#include <types_ext.h>
+
+#define HI16XX_UART_REG_SIZE 0xF8
+
+void hi16xx_uart_init(vaddr_t base, uint32_t uart_clk, uint32_t baud_rate);
+
+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);
+
+#endif /* HI16XX_UART_H */
+
diff --git a/core/include/drivers/imx_uart.h b/core/include/drivers/imx_uart.h
new file mode 100644
index 0000000..db63227
--- /dev/null
+++ b/core/include/drivers/imx_uart.h
@@ -0,0 +1,42 @@
+/*
+ * Copyright (C) 2015 Freescale Semiconductor, Inc.
+ * 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 IMX_UART_H
+#define IMX_UART_H
+
+#include <types_ext.h>
+
+void imx_uart_init(vaddr_t base);
+
+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);
+
+#endif /* IMX_UART_H */
diff --git a/core/include/drivers/ns16550.h b/core/include/drivers/ns16550.h
new file mode 100644
index 0000000..e865871
--- /dev/null
+++ b/core/include/drivers/ns16550.h
@@ -0,0 +1,36 @@
+/*
+ * Copyright (C) 2015 Freescale Semiconductor, Inc.
+ * 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 NS16550_H
+#define NS16550_H
+
+#include <types_ext.h>
+
+void ns16550_putc(int ch, vaddr_t base);
+
+void ns16550_flush(vaddr_t base);
+
+#endif /* NS16550_H */
diff --git a/core/include/drivers/pl011.h b/core/include/drivers/pl011.h
new file mode 100644
index 0000000..b83f2b2
--- /dev/null
+++ b/core/include/drivers/pl011.h
@@ -0,0 +1,45 @@
+/*
+ * Copyright (c) 2014, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+#ifndef PL011_H
+#define PL011_H
+
+#include <types_ext.h>
+
+#define PL011_REG_SIZE	0x1000
+
+void pl011_init(vaddr_t base, uint32_t uart_clk, uint32_t baud_rate);
+
+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);
+
+#endif /* PL011_H */
+
diff --git a/core/include/drivers/pl022_spi.h b/core/include/drivers/pl022_spi.h
new file mode 100644
index 0000000..079cf81
--- /dev/null
+++ b/core/include/drivers/pl022_spi.h
@@ -0,0 +1,68 @@
+/*
+ * Copyright (c) 2016, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef __PL022_SPI_H__
+#define __PL022_SPI_H__
+
+#include <gpio.h>
+#include <spi.h>
+
+#define PL022_REG_SIZE	0x1000
+
+enum pl022_cs_control {
+	PL022_CS_CTRL_AUTO_GPIO,
+	PL022_CS_CTRL_CB,
+	PL022_CS_CTRL_MANUAL
+};
+
+struct pl022_cs_gpio_data {
+	struct gpio_chip	*chip;
+	unsigned int		pin_num;
+};
+
+union pl022_cs_data {
+	struct pl022_cs_gpio_data	gpio_data;
+	void				(*cs_cb)(enum gpio_level value);
+};
+
+struct pl022_data {
+	union pl022_cs_data	cs_data;
+	struct spi_chip		chip;
+	vaddr_t			base;
+	enum spi_mode		mode;
+	enum pl022_cs_control	cs_control;
+	unsigned int		clk_hz;
+	unsigned int		speed_hz;
+	unsigned int		data_size_bits;
+	bool			loopback;
+};
+
+void pl022_init(struct pl022_data *pd);
+
+#endif	/* __PL022_SPI_H__ */
+
diff --git a/core/include/drivers/pl050.h b/core/include/drivers/pl050.h
new file mode 100644
index 0000000..56bd772
--- /dev/null
+++ b/core/include/drivers/pl050.h
@@ -0,0 +1,44 @@
+/*
+ * Copyright (c) 2016, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef __DRIVERS_PL050_H
+#define __DRIVERS_PL050_H
+
+#include <types_ext.h>
+#include <drivers/serial.h>
+
+#define PL050_REG_SIZE	0x1000
+
+struct pl050_data {
+	vaddr_t base;
+	struct serial_chip chip;
+};
+
+void pl050_init(struct pl050_data *pd, vaddr_t base, uint32_t clk);
+
+#endif /*__DRIVERS_PL050_H*/
+
diff --git a/core/include/drivers/pl061_gpio.h b/core/include/drivers/pl061_gpio.h
new file mode 100644
index 0000000..c9fb1cc
--- /dev/null
+++ b/core/include/drivers/pl061_gpio.h
@@ -0,0 +1,51 @@
+/*
+ * Copyright (c) 2016, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef __PL061_GPIO_H__
+#define __PL061_GPIO_H__
+
+#include <gpio.h>
+#include <types_ext.h>
+
+#define PL061_REG_SIZE	0x1000
+
+enum pl061_mode_control {
+	PL061_MC_SW,
+	PL061_MC_HW
+};
+
+struct pl061_data {
+	struct gpio_chip chip;
+};
+
+void pl061_register(vaddr_t base_addr, unsigned int gpio_dev);
+void pl061_init(struct pl061_data *pd);
+enum pl061_mode_control pl061_get_mode_control(unsigned int gpio_pin);
+void pl061_set_mode_control(unsigned int gpio_pin,
+	enum pl061_mode_control hw_sw);
+
+#endif	/* __PL061_GPIO_H__ */
diff --git a/core/include/drivers/pl111.h b/core/include/drivers/pl111.h
new file mode 100644
index 0000000..fb5425a
--- /dev/null
+++ b/core/include/drivers/pl111.h
@@ -0,0 +1,53 @@
+/*
+ * 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.
+ */
+
+#ifndef PL111_H
+#define PL111_H
+
+#include <types_ext.h>
+
+#define PL111_REG_SIZE	0x1000
+
+struct pl111_videomode {
+	uint16_t hactive;
+	uint16_t hback_porch;
+	uint16_t hfront_porch;
+	uint16_t hsync_len;
+	uint16_t vactive;
+	uint16_t vback_porch;
+	uint16_t vfront_porch;
+	uint16_t vsync_len;
+};
+
+void pl111_init(vaddr_t base, paddr_t frame_base,
+		const struct pl111_videomode *m);
+
+void pl111_cursor(vaddr_t base, bool on);
+
+void pl111_set_cursor_xy(vaddr_t base, size_t x, size_t y);
+
+#endif /* PL111_H */
diff --git a/core/include/drivers/ps2mouse.h b/core/include/drivers/ps2mouse.h
new file mode 100644
index 0000000..80bee80
--- /dev/null
+++ b/core/include/drivers/ps2mouse.h
@@ -0,0 +1,67 @@
+/*
+ * Copyright (c) 2016, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+#ifndef __DRIVERS_PS2MOUSE_H
+#define __DRIVERS_PS2MOUSE_H
+
+#include <types_ext.h>
+#include <kernel/interrupt.h>
+#include <drivers/serial.h>
+
+enum ps2mouse_state {
+	PS2MS_RESET,
+	PS2MS_INIT,
+	PS2MS_INIT2,
+	PS2MS_INIT3,
+	PS2MS_ACTIVE1,
+	PS2MS_ACTIVE2,
+	PS2MS_ACTIVE3,
+	PS2MS_INACTIVE,
+};
+
+#define PS2MOUSE_BUTTON_LEFT_DOWN	(1 << 0)
+#define PS2MOUSE_BUTTON_RIGHT_DOWN	(1 << 1)
+#define PS2MOUSE_BUTTON_MIDDLE_DOWN	(1 << 2)
+
+typedef void (*ps2mouse_callback)(void *data, uint8_t button, int16_t xdelta,
+				  int16_t ydelta);
+
+struct ps2mouse_data {
+	struct serial_chip *serial;
+	enum ps2mouse_state state;
+	uint8_t bytes[2];
+	struct itr_handler itr_handler;
+	ps2mouse_callback callback;
+	void *callback_data;
+};
+
+
+void ps2mouse_init(struct ps2mouse_data *d, struct serial_chip *serial,
+		   size_t serial_it, ps2mouse_callback cb, void *cb_data);
+
+
+#endif /*__DRIVERS_PS2MOUSE_H*/
+
diff --git a/core/include/drivers/scif.h b/core/include/drivers/scif.h
new file mode 100644
index 0000000..d9d1055
--- /dev/null
+++ b/core/include/drivers/scif.h
@@ -0,0 +1,40 @@
+/*
+ * Copyright (c) 2016, GlobalLogic
+ * 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 SCIF_H
+#define SCIF_H
+
+#include <types_ext.h>
+
+#define SCIF_REG_SIZE	0x1000
+
+void scif_uart_flush(vaddr_t base);
+
+void scif_uart_init(vaddr_t base);
+
+void scif_uart_putc(int ch, vaddr_t base);
+
+#endif /* SCIF */
diff --git a/core/include/drivers/serial.h b/core/include/drivers/serial.h
new file mode 100644
index 0000000..b8f00df
--- /dev/null
+++ b/core/include/drivers/serial.h
@@ -0,0 +1,41 @@
+/*
+ * Copyright (c) 2016, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+#ifndef __DRIVERS_SERIAL_H
+#define __DRIVERS_SERIAL_H
+
+struct serial_chip {
+	const struct serial_ops *ops;
+};
+
+struct serial_ops {
+	void (*putc)(struct serial_chip *chip, int ch);
+	void (*flush)(struct serial_chip *chip);
+	bool (*have_rx_data)(struct serial_chip *chip);
+	int (*getchar)(struct serial_chip *chip);
+};
+
+#endif /*__DRIVERS_SERIASERIAL_H*/
diff --git a/core/include/drivers/serial8250_uart.h b/core/include/drivers/serial8250_uart.h
new file mode 100644
index 0000000..5b8985d
--- /dev/null
+++ b/core/include/drivers/serial8250_uart.h
@@ -0,0 +1,46 @@
+/*
+ * 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.
+ */
+#ifndef SERIAL8250_UART_H
+#define SERIAL8250_UART_H
+
+#include <types_ext.h>
+
+#define SERIAL8250_UART_REG_SIZE 0x20
+
+void serial8250_uart_init(vaddr_t base,
+		uint32_t uart_clk, uint32_t baud_rate);
+
+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);
+
+#endif /* SERIAL8250_UART_H */
+
diff --git a/core/include/drivers/sprd_uart.h b/core/include/drivers/sprd_uart.h
new file mode 100644
index 0000000..c70bfb9
--- /dev/null
+++ b/core/include/drivers/sprd_uart.h
@@ -0,0 +1,39 @@
+/*
+ * Copyright (c) 2016, Spreadtrum Communications Inc.
+ * 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 SPRD_UART_H
+#define SPRD_UART_H
+
+#include <types_ext.h>
+
+void sprd_uart_flush(vaddr_t base);
+
+void sprd_uart_putc(vaddr_t base, unsigned char ch);
+
+unsigned char sprd_uart_getc(vaddr_t base);
+
+#endif /* SPRD_UART_H */
+
diff --git a/core/include/drivers/sunxi_uart.h b/core/include/drivers/sunxi_uart.h
new file mode 100644
index 0000000..2b33641
--- /dev/null
+++ b/core/include/drivers/sunxi_uart.h
@@ -0,0 +1,43 @@
+/*
+ * Copyright (c) 2014, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+#ifndef SUNXI_UART_H
+#define SUNXI_UART_H
+
+#include <types_ext.h>
+
+void sunxi_uart_init(vaddr_t base);
+
+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);
+
+#endif /*SUNXI_UART_H*/
+
diff --git a/core/include/drivers/tzc400.h b/core/include/drivers/tzc400.h
new file mode 100644
index 0000000..fb3142e
--- /dev/null
+++ b/core/include/drivers/tzc400.h
@@ -0,0 +1,236 @@
+/*
+ * 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.
+ */
+/*
+ * Copyright (c) 2014, ARM Limited and Contributors. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * 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.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE 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 __DRIVERS_TZC400_H
+#define __DRIVERS_TZC400_H
+
+#include <stdint.h>
+#include <types_ext.h>
+#include <trace_levels.h>
+
+#define TZC400_REG_SIZE		0x1000
+
+#define BUILD_CONFIG_OFF	0x000
+#define ACTION_OFF		0x004
+#define GATE_KEEPER_OFF		0x008
+#define SPECULATION_CTRL_OFF	0x00c
+#define INT_STATUS		0x010
+#define INT_CLEAR		0x014
+
+#define FAIL_ADDRESS_LOW_OFF	0x020
+#define FAIL_ADDRESS_HIGH_OFF	0x024
+#define FAIL_CONTROL_OFF	0x028
+#define FAIL_ID			0x02c
+
+#define REGION_BASE_LOW_OFF	0x100
+#define REGION_BASE_HIGH_OFF	0x104
+#define REGION_TOP_LOW_OFF	0x108
+#define REGION_TOP_HIGH_OFF	0x10c
+#define REGION_ATTRIBUTES_OFF	0x110
+#define REGION_ID_ACCESS_OFF	0x114
+#define REGION_NUM_OFF(region)  (0x20 * region)
+
+/* ID Registers */
+#define PID0_OFF		0xfe0
+#define PID1_OFF		0xfe4
+#define PID2_OFF		0xfe8
+#define PID3_OFF		0xfec
+#define PID4_OFF		0xfd0
+#define PID5_OFF		0xfd4
+#define PID6_OFF		0xfd8
+#define PID7_OFF		0xfdc
+#define CID0_OFF		0xff0
+#define CID1_OFF		0xff4
+#define CID2_OFF		0xff8
+#define CID3_OFF		0xffc
+
+#define BUILD_CONFIG_NF_SHIFT	24
+#define BUILD_CONFIG_NF_MASK	0x3
+#define BUILD_CONFIG_AW_SHIFT	8
+#define BUILD_CONFIG_AW_MASK	0x3f
+#define BUILD_CONFIG_NR_SHIFT	0
+#define BUILD_CONFIG_NR_MASK	0x1f
+
+/* Not describing the case where regions 1 to 8 overlap */
+#define ACTION_RV_SHIFT		0
+#define ACTION_RV_MASK		0x3
+#define  ACTION_RV_LOWOK	0x0
+#define  ACTION_RV_LOWERR	0x1
+#define  ACTION_RV_HIGHOK	0x2
+#define  ACTION_RV_HIGHERR	0x3
+
+/*
+ * Number of gate keepers is implementation defined. But we know the max for
+ * this device is 4. Get implementation details from BUILD_CONFIG.
+ */
+#define GATE_KEEPER_OS_SHIFT	16
+#define GATE_KEEPER_OS_MASK	0xf
+#define GATE_KEEPER_OR_SHIFT	0
+#define GATE_KEEPER_OR_MASK	0xf
+#define GATE_KEEPER_FILTER_MASK	0x1
+
+/* Speculation is enabled by default. */
+#define SPECULATION_CTRL_WRITE_DISABLE	(1 << 1)
+#define SPECULATION_CTRL_READ_DISABLE	(1 << 0)
+
+/* Max number of filters allowed is 4. */
+#define INT_STATUS_OVERLAP_SHIFT	16
+#define INT_STATUS_OVERLAP_MASK		0xf
+#define INT_STATUS_OVERRUN_SHIFT	8
+#define INT_STATUS_OVERRUN_MASK		0xf
+#define INT_STATUS_STATUS_SHIFT		0
+#define INT_STATUS_STATUS_MASK		0xf
+
+#define INT_CLEAR_CLEAR_SHIFT		0
+#define INT_CLEAR_CLEAR_MASK		0xf
+
+#define FAIL_CONTROL_DIR_SHIFT		(1 << 24)
+#define  FAIL_CONTROL_DIR_READ		0x0
+#define  FAIL_CONTROL_DIR_WRITE		0x1
+#define FAIL_CONTROL_NS_SHIFT		(1 << 21)
+#define  FAIL_CONTROL_NS_SECURE		0x0
+#define  FAIL_CONTROL_NS_NONSECURE	0x1
+#define FAIL_CONTROL_PRIV_SHIFT		(1 << 20)
+#define  FAIL_CONTROL_PRIV_PRIV		0x0
+#define  FAIL_CONTROL_PRIV_UNPRIV	0x1
+
+/*
+ * FAIL_ID_ID_MASK depends on AID_WIDTH which is platform specific.
+ * Platform should provide the value on initialisation.
+ */
+#define FAIL_ID_VNET_SHIFT		24
+#define FAIL_ID_VNET_MASK		0xf
+#define FAIL_ID_ID_SHIFT		0
+
+/* Used along with 'enum tzc_region_attributes' below */
+#define REG_ATTR_SEC_SHIFT		30
+#define REG_ATTR_F_EN_SHIFT		0
+#define REG_ATTR_F_EN_MASK		0xf
+#define REG_ATTR_FILTER_BIT(x)		((1 << x) << REG_ATTR_F_EN_SHIFT)
+#define REG_ATTR_FILTER_BIT_ALL		(REG_ATTR_F_EN_MASK << \
+					REG_ATTR_F_EN_SHIFT)
+
+#define REGION_ID_ACCESS_NSAID_WR_EN_SHIFT	16
+#define REGION_ID_ACCESS_NSAID_RD_EN_SHIFT	0
+#define REGION_ID_ACCESS_NSAID_ID_MASK		0xf
+
+
+/* Macros for setting Region ID access permissions based on NSAID */
+#define TZC_REGION_ACCESS_RD(id)					\
+		((1 << (id & REGION_ID_ACCESS_NSAID_ID_MASK)) <<	\
+		 REGION_ID_ACCESS_NSAID_RD_EN_SHIFT)
+#define TZC_REGION_ACCESS_WR(id)					\
+		((1 << (id & REGION_ID_ACCESS_NSAID_ID_MASK)) <<	\
+		 REGION_ID_ACCESS_NSAID_WR_EN_SHIFT)
+#define TZC_REGION_ACCESS_RDWR(id)					\
+		(TZC_REGION_ACCESS_RD(id) | TZC_REGION_ACCESS_WR(id))
+
+/* Filters are bit mapped 0 to 3. */
+#define TZC400_COMPONENT_ID	0xb105f00d
+
+/*******************************************************************************
+ * Function & variable prototypes
+ ******************************************************************************/
+
+/*
+ * What type of action is expected when an access violation occurs.
+ * The memory requested is zeroed. But we can also raise and event to
+ * let the system know it happened.
+ * We can raise an interrupt(INT) and/or cause an exception(ERR).
+ *  TZC_ACTION_NONE    - No interrupt, no Exception
+ *  TZC_ACTION_ERR     - No interrupt, raise exception -> sync external
+ *                       data abort
+ *  TZC_ACTION_INT     - Raise interrupt, no exception
+ *  TZC_ACTION_ERR_INT - Raise interrupt, raise exception -> sync
+ *                       external data abort
+ */
+enum tzc_action {
+	TZC_ACTION_NONE = 0,
+	TZC_ACTION_ERR = 1,
+	TZC_ACTION_INT = 2,
+	TZC_ACTION_ERR_INT = (TZC_ACTION_ERR | TZC_ACTION_INT)
+};
+
+/*
+ * Controls secure access to a region. If not enabled secure access is not
+ * allowed to region.
+ */
+enum tzc_region_attributes {
+	TZC_REGION_S_NONE = 0,
+	TZC_REGION_S_RD = 1,
+	TZC_REGION_S_WR = 2,
+	TZC_REGION_S_RDWR = (TZC_REGION_S_RD | TZC_REGION_S_WR)
+};
+
+
+void tzc_init(vaddr_t base);
+void tzc_configure_region(uint32_t filters, uint8_t region,
+			  vaddr_t region_base, vaddr_t region_top,
+			  enum tzc_region_attributes sec_attr,
+			  uint32_t ns_device_access);
+void tzc_enable_filters(void);
+void tzc_disable_filters(void);
+void tzc_set_action(enum tzc_action action);
+
+#if TRACE_LEVEL >= TRACE_DEBUG
+void tzc_dump_state(void);
+#else
+static inline void tzc_dump_state(void)
+{
+}
+#endif
+
+#endif /* __DRIVERS_TZC400_H */
diff --git a/core/include/gpio.h b/core/include/gpio.h
new file mode 100644
index 0000000..6105521
--- /dev/null
+++ b/core/include/gpio.h
@@ -0,0 +1,60 @@
+/*
+ * Copyright (c) 2016, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef __GPIO_H__
+#define __GPIO_H__
+
+enum gpio_dir {
+	GPIO_DIR_OUT,
+	GPIO_DIR_IN
+};
+
+enum gpio_level {
+	GPIO_LEVEL_LOW,
+	GPIO_LEVEL_HIGH
+};
+
+enum gpio_interrupt {
+	GPIO_INTERRUPT_DISABLE,
+	GPIO_INTERRUPT_ENABLE
+};
+
+struct gpio_chip {
+	const struct gpio_ops *ops;
+};
+
+struct gpio_ops {
+	enum gpio_dir (*get_direction)(unsigned int gpio_pin);
+	void (*set_direction)(unsigned int gpio_pin, enum gpio_dir direction);
+	enum gpio_level (*get_value)(unsigned int gpio_pin);
+	void (*set_value)(unsigned int gpio_pin, enum gpio_level value);
+	enum gpio_interrupt (*get_interrupt)(unsigned int gpio_pin);
+	void (*set_interrupt)(unsigned int gpio_pin,
+		enum gpio_interrupt ena_dis);
+};
+
+#endif	/* __GPIO_H__ */
diff --git a/core/include/initcall.h b/core/include/initcall.h
new file mode 100644
index 0000000..dcbec5d
--- /dev/null
+++ b/core/include/initcall.h
@@ -0,0 +1,44 @@
+/*
+ * Copyright (c) 2014, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef INITCALL_H
+#define INITCALL_H
+
+#include <tee_api_types.h>
+
+typedef TEE_Result (*initcall_t)(void);
+
+#define __define_initcall(level, fn) \
+	static initcall_t __initcall_##fn __attribute__((used)) \
+	__attribute__((__section__(".initcall" level))) = fn
+
+#define service_init(fn)	__define_initcall("1", fn)
+#define service_init_late(fn)	__define_initcall("2", fn)
+#define driver_init(fn)		__define_initcall("3", fn)
+#define driver_init_late(fn)	__define_initcall("4", fn)
+
+#endif
diff --git a/core/include/io.h b/core/include/io.h
new file mode 100644
index 0000000..510bf33
--- /dev/null
+++ b/core/include/io.h
@@ -0,0 +1,84 @@
+/*
+ * Copyright (c) 2014, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+#ifndef IO_H
+#define IO_H
+
+#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;
+}
+
+static inline void write16(uint16_t val, vaddr_t addr)
+{
+	*(volatile uint16_t *)addr = val;
+}
+
+static inline void write32(uint32_t val, vaddr_t addr)
+{
+	*(volatile uint32_t *)addr = val;
+}
+
+static inline uint8_t read8(vaddr_t addr)
+{
+	return *(volatile uint8_t *)addr;
+}
+
+static inline uint16_t read16(vaddr_t addr)
+{
+	return *(volatile uint16_t *)addr;
+}
+
+static inline uint32_t read32(vaddr_t addr)
+{
+	return *(volatile uint32_t *)addr;
+}
+
+static inline void io_mask8(vaddr_t addr, uint8_t val, uint8_t mask)
+{
+	write8((read8(addr) & ~mask) | (val & mask), addr);
+}
+
+static inline void io_mask16(vaddr_t addr, uint16_t val, uint16_t mask)
+{
+	write16((read16(addr) & ~mask) | (val & mask), addr);
+}
+
+static inline void io_mask32(vaddr_t addr, uint32_t val, uint32_t mask)
+{
+	write32((read32(addr) & ~mask) | (val & mask), addr);
+}
+
+#endif /*IO_H*/
diff --git a/core/include/keep.h b/core/include/keep.h
new file mode 100644
index 0000000..81e4917
--- /dev/null
+++ b/core/include/keep.h
@@ -0,0 +1,60 @@
+/*
+ * 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.
+ */
+#ifndef KEEP_H
+#define KEEP_H
+
+#ifdef ASM
+
+	.macro KEEP_PAGER sym
+	.pushsection __keep_meta_vars_pager
+	___keep_pager_\sym:
+	.long	\sym
+	.popsection
+	.endm
+
+	.macro KEEP_INIT sym
+	.pushsection __keep_meta_vars_init
+	___keep_init_\sym:
+	.long	\sym
+	.popsection
+	.endm
+
+#else
+
+#include <compiler.h>
+
+#define KEEP_PAGER(sym) \
+	const unsigned long ____keep_pager_##sym  \
+		__section("__keep_meta_vars_pager") = (unsigned long)&sym
+
+#define KEEP_INIT(sym) \
+	const unsigned long ____keep_init_##sym  \
+		__section("__keep_meta_vars_init") = (unsigned long)&sym
+
+#endif /* ASM */
+
+#endif /*KEEP_H*/
diff --git a/core/include/kernel/asan.h b/core/include/kernel/asan.h
new file mode 100644
index 0000000..6eaa119
--- /dev/null
+++ b/core/include/kernel/asan.h
@@ -0,0 +1,61 @@
+/*
+ * Copyright (c) 2016, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+#ifndef __KERNEL_ASAN_H
+#define __KERNEL_ASAN_H
+
+
+#define ASAN_DATA_RED_ZONE	-1
+#define ASAN_HEAP_RED_ZONE	-2
+
+#define ASAN_BLOCK_SIZE		8
+#define ASAN_BLOCK_SHIFT	3
+#define ASAN_BLOCK_MASK		(ASAN_BLOCK_SIZE - 1)
+
+#ifndef ASM
+#include <types_ext.h>
+
+void asan_set_shadowed(void *va_begin, void *va_end);
+void asan_start(void);
+
+#ifdef CFG_CORE_SANITIZE_KADDRESS
+void asan_tag_no_access(void *begin, void *end);
+void asan_tag_access(void *begin, void *end);
+void asan_tag_heap_free(void *begin, void *end);
+#else
+static inline void asan_tag_no_access(void *begin __unused, void *end __unused)
+{
+}
+static inline void asan_tag_access(void *begin __unused, void *end __unused)
+{
+}
+static inline void asan_tag_heap_free(void *begin __unused, void *end __unused)
+{
+}
+#endif
+
+#endif /*ASM*/
+#endif /*__KERNEL_ASAN_H*/
diff --git a/core/include/kernel/chip_services.h b/core/include/kernel/chip_services.h
new file mode 100644
index 0000000..8e08c7d
--- /dev/null
+++ b/core/include/kernel/chip_services.h
@@ -0,0 +1,34 @@
+/*
+ * 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.
+ */
+
+#ifndef CHIP_SERVICES_H
+#define CHIP_SERVICES_H
+
+/* Die ID */
+#define FVR_DIE_ID_NUM_REGS                 3
+
+#endif
diff --git a/core/include/kernel/handle.h b/core/include/kernel/handle.h
new file mode 100644
index 0000000..2cfd2e6
--- /dev/null
+++ b/core/include/kernel/handle.h
@@ -0,0 +1,68 @@
+/*
+ * Copyright (c) 2014, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+#ifndef KERNEL_HANDLE_H
+#define KERNEL_HANDLE_H
+
+#include <stdint.h>
+
+struct handle_db {
+	void **ptrs;
+	size_t max_ptrs;
+};
+
+#define HANDLE_DB_INITIALIZER { NULL, 0 }
+
+/*
+ * Frees all internal data structures of the database, but does not free
+ * the db pointer. The database is safe to reuse after it's destroyed, it
+ * just be empty again.
+ */
+void handle_db_destroy(struct handle_db *db);
+
+/*
+ * Allocates a new handle and assigns the supplied pointer to it,
+ * ptr must not be NULL.
+ * The function returns
+ * >= 0 on success and
+ * -1 on failure
+ */
+int handle_get(struct handle_db *db, void *ptr);
+
+/*
+ * Deallocates a handle. Returns the assiciated pointer of the handle
+ * the the handle was valid or NULL if it's invalid.
+ */
+void *handle_put(struct handle_db *db, int handle);
+
+/*
+ * Returns the assiciated pointer of the handle if the handle is a valid
+ * handle.
+ * Returns NULL on failure.
+ */
+void *handle_lookup(struct handle_db *db, int handle);
+
+#endif /*KERNEL_HANDLE_H*/
diff --git a/core/include/kernel/interrupt.h b/core/include/kernel/interrupt.h
new file mode 100644
index 0000000..7732380
--- /dev/null
+++ b/core/include/kernel/interrupt.h
@@ -0,0 +1,82 @@
+/*
+ * Copyright (c) 2016, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+#ifndef __KERNEL_INTERRUPT_H
+#define __KERNEL_INTERRUPT_H
+
+#include <types_ext.h>
+#include <sys/queue.h>
+
+#define ITRF_TRIGGER_LEVEL	(1 << 0)
+
+struct itr_chip {
+	const struct itr_ops *ops;
+};
+
+struct itr_ops {
+	void (*add)(struct itr_chip *chip, size_t it, uint32_t flags);
+	void (*enable)(struct itr_chip *chip, size_t it);
+	void (*disable)(struct itr_chip *chip, size_t it);
+	void (*raise_pi)(struct itr_chip *chip, size_t it);
+	void (*raise_sgi)(struct itr_chip *chip, size_t it,
+		uint8_t cpu_mask);
+	void (*set_affinity)(struct itr_chip *chip, size_t it,
+		uint8_t cpu_mask);
+};
+
+enum itr_return {
+	ITRR_NONE,
+	ITRR_HANDLED,
+};
+
+struct itr_handler {
+	size_t it;
+	uint32_t flags;
+	enum itr_return (*handler)(struct itr_handler *h);
+	void *data;
+	SLIST_ENTRY(itr_handler) link;
+};
+
+void itr_init(struct itr_chip *data);
+void itr_handle(size_t it);
+
+void itr_add(struct itr_handler *handler);
+void itr_enable(size_t it);
+void itr_disable(size_t it);
+/* raise the Peripheral Interrupt corresponding to the interrupt ID */
+void itr_raise_pi(size_t it);
+/*
+ * raise the Software Generated Interrupt corresponding to the interrupt ID,
+ * the cpu_mask represents which cpu interface to forward.
+ */
+void itr_raise_sgi(size_t it, uint8_t cpu_mask);
+/*
+ * let corresponding interrupt forward to the cpu interface
+ * according to the cpu_mask.
+ */
+void itr_set_affinity(size_t it, uint8_t cpu_mask);
+
+#endif /*__KERNEL_INTERRUPT_H*/
diff --git a/core/include/kernel/panic.h b/core/include/kernel/panic.h
new file mode 100644
index 0000000..02ff6f8
--- /dev/null
+++ b/core/include/kernel/panic.h
@@ -0,0 +1,58 @@
+/*
+ * 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.
+ */
+
+#ifndef KERNEL_PANIC_H
+#define KERNEL_PANIC_H
+
+#include <compiler.h>
+
+/* debug disabled => __FILE__, ... and panic message are not built. */
+#if defined(CFG_TEE_CORE_DEBUG)
+#define __panic(str)	__do_panic(__FILE__, __LINE__, __func__, str)
+#else
+#define __panic(str)	__do_panic((void *)0, 0, (void *)0, (void *)0)
+#endif
+
+void __do_panic(const char *file, const int line, const char *func,
+		const char *msg) __noreturn;
+
+/*
+ * Suppress GCC warning on expansion of the panic() macro with no argument:
+ *  'ISO C99 requires at least one argument for the "..." in a variadic macro'
+ * Occurs when '-pedantic' is combined with '-std=gnu99'.
+ * Suppression applies only to this file and the expansion of macros defined in
+ * this file.
+ */
+#pragma GCC system_header
+
+/* panic() can get a string or no argument */
+#define _panic0()	__panic((void *)0)
+#define _panic1(s)	__panic(s)
+#define _panic_fn(a, b, name, ...) name
+#define panic(...) _panic_fn("", ##__VA_ARGS__, _panic1, _panic0)(__VA_ARGS__)
+
+#endif /*KERNEL_PANIC_H*/
diff --git a/core/include/kernel/tee_common.h b/core/include/kernel/tee_common.h
new file mode 100644
index 0000000..74354b3
--- /dev/null
+++ b/core/include/kernel/tee_common.h
@@ -0,0 +1,60 @@
+/*
+ * 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.
+ */
+#ifndef TEE_COMMON_H
+#define TEE_COMMON_H
+
+#include <stdlib.h>
+
+#ifdef MEASURE_TIME
+/*
+ * Initializes mesaure time. Initializes RTT0 to highest possible
+ * resolution.
+ */
+void tee_mtime_init(void);
+
+/*
+ * Adds a time stamp together the description. Note that only the pointer
+ * is copied, not the contents to minimize impact.
+ */
+void tee_mtime_stamp(const char *descr);
+
+/*
+ * Prints a report of measured times and reinitializes clears the table of
+ * saved time stamps.
+ */
+void tee_mtime_report(void);
+
+void tee_mtime_perftest(void);
+#else
+/* Empty macros to not have any impact on code when not meassuring time */
+#define tee_mtime_init() do { } while (0)
+#define tee_mtime_stamp(descr) do { } while (0)
+#define tee_mtime_report() do { } while (0)
+#define tee_mtime_perftest()  do { } while (0)
+#endif
+
+#endif /* TEE_COMMON_H */
diff --git a/core/include/kernel/tee_common_otp.h b/core/include/kernel/tee_common_otp.h
new file mode 100644
index 0000000..c04b7be
--- /dev/null
+++ b/core/include/kernel/tee_common_otp.h
@@ -0,0 +1,64 @@
+/*
+ * 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.
+ */
+#ifndef TEE_COMMON_OTP_H
+#define TEE_COMMON_OTP_H
+
+#include <stddef.h>
+#include <stdint.h>
+#include <string.h>
+#include <utee_defines.h>
+
+struct tee_hw_unique_key {
+	uint8_t data[HW_UNIQUE_KEY_LENGTH];
+};
+
+#if defined(CFG_OTP_SUPPORT)
+
+void tee_otp_get_hw_unique_key(struct tee_hw_unique_key *hwkey);
+int tee_otp_get_die_id(uint8_t *buffer, size_t len);
+
+#else
+
+static inline void tee_otp_get_hw_unique_key(struct tee_hw_unique_key *hwkey)
+{
+	memset(&hwkey->data[0], 0, sizeof(hwkey->data));
+}
+
+static inline int tee_otp_get_die_id(uint8_t *buffer, size_t len)
+{
+	size_t i;
+
+	char pattern[4] = { 'B', 'E', 'E', 'F' };
+	for (i = 0; i < len; i++)
+		buffer[i] = pattern[i % 4];
+
+	return 0;
+}
+
+#endif /* !defined(CFG_OTP_SUPPORT) */
+
+#endif /* TEE_COMMON_OTP_H */
diff --git a/core/include/kernel/tee_customer_part.h b/core/include/kernel/tee_customer_part.h
new file mode 100644
index 0000000..c38fddc
--- /dev/null
+++ b/core/include/kernel/tee_customer_part.h
@@ -0,0 +1,41 @@
+/*
+ * 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.
+ */
+
+#ifndef TEE_CUSTOMER_PART_H
+#define TEE_CUSTOMER_PART_H
+
+#include "stdint.h"
+#include "stdbool.h"
+#include "tee_api_types.h"
+
+#define TEE_SIGNING_CONSTRAINTS_LEN         64
+
+TEE_Result tee_cust_part_import(const uint32_t cust_part,
+				const uint32_t cust_size,
+				const uint8_t *hash, const uint32_t hash_type);
+
+#endif /* TEE_CUSTOMER_PART_H */
diff --git a/core/include/kernel/tee_kta_trace.h b/core/include/kernel/tee_kta_trace.h
new file mode 100644
index 0000000..3caa9de
--- /dev/null
+++ b/core/include/kernel/tee_kta_trace.h
@@ -0,0 +1,52 @@
+/*
+ * 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.
+ */
+
+#ifndef KERNEL_TEE_KTA_TRACE_H
+#define KERNEL_TEE_KTA_TRACE_H
+
+#include <trace.h>
+
+/*
+ * This file provides legacy support for static TAs which hasn't been
+ * converted yet.
+ */
+
+#define ETAMSG		EMSG
+#define ITAMSG		IMSG
+#define DTAMSG		DMSG
+#define FTAMSG		FMSG
+
+#define TAINMSG		INMSG
+#define TAOUTMSG	OUTMSG
+#define TAOUTRMSG	OUTRMSG
+
+#define ETAMSG_RAW	EMSG_RAW
+#define ITAMSG_RAW	IMSG_RAW
+#define DTAMSG_RAW	DMSG_RAW
+#define FTAMSG_RAW	FMSG_RAW
+
+#endif /*KERNEL_TEE_KTA_TRACE_H*/
diff --git a/core/include/kernel/tee_misc.h b/core/include/kernel/tee_misc.h
new file mode 100644
index 0000000..14c6ddb
--- /dev/null
+++ b/core/include/kernel/tee_misc.h
@@ -0,0 +1,83 @@
+/*
+ * 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.
+ */
+#ifndef TEE_MISC_H
+#define TEE_MISC_H
+
+#include <types_ext.h>
+
+/*
+ * Macro to derive hex string buffer size from binary buffer size & the
+ * reverse
+ */
+#define TEE_B2HS_HSBUF_SIZE(x) ((x) * 2 + 1)
+#define TEE_HS2B_BBUF_SIZE(x) ((x + 1) >> 1)
+
+/*
+ * binary to hex string buffer
+ * Returns the number of data bytes written to the hex string
+ */
+uint32_t tee_b2hs(uint8_t *b, uint8_t *hs, uint32_t blen, uint32_t hslen);
+
+/*
+ * hex string to binary buffer
+ * Returns the number of data bytes written to the bin buffer
+ */
+uint32_t tee_hs2b(uint8_t *hs, uint8_t *b, uint32_t hslen, uint32_t blen);
+
+/*
+ * Is buffer 'b' inside/outside/overlapping area 'a'?
+ *
+ * core_is_buffer_inside() - return true if buffer is inside memory area
+ * core_is_buffer_outside() - return true if buffer is outside area
+ * core_is_buffer_intersect() - return true if buffer overlaps area
+ *
+ * Warning: core_is_buffer_inside(x,x,x,x)==false does NOT mean
+ * core_is_buffer_outside(x,x,x,x)==true.
+ *
+ * Arguments use by each of these routines:
+ * @b - buffer start address (handled has an unsigned offset)
+ * @bl - length (in bytes) of the target buffer
+ * @a - memory area start address (handled has an unsigned offset)
+ * @al - memory area length (in byte)
+ */
+#define core_is_buffer_inside(b, bl, a, al) \
+	_core_is_buffer_inside((vaddr_t)(b), (size_t)(bl), \
+				(vaddr_t)(a), (size_t)(al))
+
+#define core_is_buffer_outside(b, bl, a, al) \
+	_core_is_buffer_outside((vaddr_t)(b), (size_t)(bl), \
+				(vaddr_t)(a), (size_t)(al))
+
+#define core_is_buffer_intersect(b, bl, a, al) \
+	_core_is_buffer_intersect((vaddr_t)(b), (size_t)(bl), \
+				(vaddr_t)(a), (size_t)(al))
+
+bool _core_is_buffer_inside(vaddr_t b, size_t bl, vaddr_t a, size_t al);
+bool _core_is_buffer_outside(vaddr_t b, size_t bl, vaddr_t a, size_t al);
+bool _core_is_buffer_intersect(vaddr_t b, size_t bl, vaddr_t a, size_t al);
+
+#endif /* TEE_MISC_H */
diff --git a/core/include/kernel/tee_ta_manager.h b/core/include/kernel/tee_ta_manager.h
new file mode 100644
index 0000000..d055f9d
--- /dev/null
+++ b/core/include/kernel/tee_ta_manager.h
@@ -0,0 +1,193 @@
+/*
+ * 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
+ * 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_TA_MANAGER_H
+#define TEE_TA_MANAGER_H
+
+#include <types_ext.h>
+#include <sys/queue.h>
+#include <tee_api_types.h>
+#include <utee_types.h>
+#include <kernel/tee_common.h>
+#include <kernel/mutex.h>
+#include <tee_api_types.h>
+#include <user_ta_header.h>
+
+/* Magic TEE identity pointer: set when teecore requests a TA close */
+#define KERN_IDENTITY	((TEE_Identity *)-1)
+/* Operation is initiated by a client (non-secure) app */
+#define NSAPP_IDENTITY	(NULL)
+
+TAILQ_HEAD(tee_ta_session_head, tee_ta_session);
+TAILQ_HEAD(tee_ta_ctx_head, tee_ta_ctx);
+
+struct mobj;
+
+struct param_val {
+	uint32_t a;
+	uint32_t b;
+};
+
+struct param_mem {
+	struct mobj *mobj;
+	size_t size;
+	size_t offs;
+};
+
+struct tee_ta_param {
+	uint32_t types;
+	union {
+		struct param_val val;
+		struct param_mem mem;
+	} u[TEE_NUM_PARAMS];
+};
+
+struct tee_ta_ctx;
+struct user_ta_ctx;
+struct pseudo_ta_ctx;
+
+struct tee_ta_ops {
+	TEE_Result (*enter_open_session)(struct tee_ta_session *s,
+			struct tee_ta_param *param, TEE_ErrorOrigin *eo);
+	TEE_Result (*enter_invoke_cmd)(struct tee_ta_session *s, uint32_t cmd,
+			struct tee_ta_param *param, TEE_ErrorOrigin *eo);
+	void (*enter_close_session)(struct tee_ta_session *s);
+	void (*dump_state)(struct tee_ta_ctx *ctx);
+	void (*destroy)(struct tee_ta_ctx *ctx);
+	uint32_t (*get_instance_id)(struct tee_ta_ctx *ctx);
+};
+
+#if defined(CFG_TA_GPROF_SUPPORT)
+struct sample_buf {
+	uint32_t nsamples;	/* Size of @samples array in uint16_t */
+	uint32_t offset;	/* Passed from user mode */
+	uint32_t scale;		/* Passed from user mode */
+	uint32_t count;		/* Number of samples taken */
+	bool enabled;		/* Sampling enabled? */
+	uint16_t *samples;
+	uint64_t usr;		/* Total user CPU time for this session */
+	uint64_t usr_entered;	/* When this session last entered user mode */
+	uint32_t freq;		/* @usr divided by @freq is in seconds */
+};
+#endif
+
+/* Context of a loaded TA */
+struct tee_ta_ctx {
+	TEE_UUID uuid;
+	const struct tee_ta_ops *ops;
+	uint32_t flags;		/* TA_FLAGS from TA header */
+	TAILQ_ENTRY(tee_ta_ctx) link;
+	uint32_t panicked;	/* True if TA has panicked, written from asm */
+	uint32_t panic_code;	/* Code supplied for panic */
+	uint32_t ref_count;	/* Reference counter for multi session TA */
+	bool busy;		/* context is busy and cannot be entered */
+	struct condvar busy_cv;	/* CV used when context is busy */
+};
+
+struct tee_ta_session {
+	TAILQ_ENTRY(tee_ta_session) link;
+	TAILQ_ENTRY(tee_ta_session) link_tsd;
+	struct tee_ta_ctx *ctx;	/* TA context */
+	TEE_Identity clnt_id;	/* Identify of client */
+	bool cancel;		/* True if TAF is cancelled */
+	bool cancel_mask;	/* True if cancel is masked */
+	TEE_Time cancel_time;	/* Time when to cancel the TAF */
+	void *user_ctx;		/* ??? */
+	uint32_t ref_count;	/* reference counter */
+	struct condvar refc_cv;	/* CV used to wait for ref_count to be 0 */
+	struct condvar lock_cv;	/* CV used to wait for lock */
+	int lock_thread;	/* Id of thread holding the lock */
+	bool unlink;		/* True if session is to be unlinked */
+#if defined(CFG_TA_GPROF_SUPPORT)
+	struct sample_buf *sbuf; /* Profiling data (PC sampling) */
+#endif
+};
+
+/* Registered contexts */
+extern struct tee_ta_ctx_head tee_ctxes;
+
+extern struct mutex tee_ta_mutex;
+
+TEE_Result tee_ta_open_session(TEE_ErrorOrigin *err,
+			       struct tee_ta_session **sess,
+			       struct tee_ta_session_head *open_sessions,
+			       const TEE_UUID *uuid,
+			       const TEE_Identity *clnt_id,
+			       uint32_t cancel_req_to,
+			       struct tee_ta_param *param);
+
+TEE_Result tee_ta_invoke_command(TEE_ErrorOrigin *err,
+				 struct tee_ta_session *sess,
+				 const TEE_Identity *clnt_id,
+				 uint32_t cancel_req_to, uint32_t cmd,
+				 struct tee_ta_param *param);
+
+TEE_Result tee_ta_cancel_command(TEE_ErrorOrigin *err,
+				 struct tee_ta_session *sess,
+				 const TEE_Identity *clnt_id);
+
+bool tee_ta_session_is_cancelled(struct tee_ta_session *s, TEE_Time *curr_time);
+
+/*-----------------------------------------------------------------------------
+ * Function called to close a TA.
+ * Parameters:
+ * id   - The session id (in)
+ * Returns:
+ *        TEE_Result
+ *---------------------------------------------------------------------------*/
+TEE_Result tee_ta_close_session(struct tee_ta_session *sess,
+				struct tee_ta_session_head *open_sessions,
+				const TEE_Identity *clnt_id);
+
+TEE_Result tee_ta_get_current_session(struct tee_ta_session **sess);
+
+void tee_ta_push_current_session(struct tee_ta_session *sess);
+struct tee_ta_session *tee_ta_pop_current_session(void);
+
+struct tee_ta_session *tee_ta_get_calling_session(void);
+
+TEE_Result tee_ta_get_client_id(TEE_Identity *id);
+
+struct tee_ta_session *tee_ta_get_session(uint32_t id, bool exclusive,
+			struct tee_ta_session_head *open_sessions);
+
+void tee_ta_put_session(struct tee_ta_session *sess);
+
+void tee_ta_dump_current(void);
+
+#if defined(CFG_TA_GPROF_SUPPORT)
+void tee_ta_gprof_sample_pc(vaddr_t pc);
+void tee_ta_update_session_utime_suspend(void);
+void tee_ta_update_session_utime_resume(void);
+#else
+static inline void tee_ta_gprof_sample_pc(vaddr_t pc __unused) {}
+static inline void tee_ta_update_session_utime_suspend(void) {}
+static inline void tee_ta_update_session_utime_resume(void) {}
+#endif
+
+#endif
diff --git a/core/include/kernel/tee_time.h b/core/include/kernel/tee_time.h
new file mode 100644
index 0000000..58c8438
--- /dev/null
+++ b/core/include/kernel/tee_time.h
@@ -0,0 +1,45 @@
+/*
+ * 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.
+ */
+
+#ifndef TEE_TIME_H
+#define TEE_TIME_H
+
+#include "tee_api_types.h"
+
+#define TEE_TIME_BOOT_TICKS_HZ  10UL
+
+TEE_Result tee_time_get_sys_time(TEE_Time *time);
+uint32_t tee_time_get_sys_time_protection_level(void);
+TEE_Result tee_time_get_ta_time(const TEE_UUID *uuid, TEE_Time *time);
+TEE_Result tee_time_get_ree_time(TEE_Time *time);
+TEE_Result tee_time_set_ta_time(const TEE_UUID *uuid, const TEE_Time *time);
+/* Releases CPU through OP-TEE RPC which switches to Normal World */
+void tee_time_wait(uint32_t milliseconds_delay);
+/* Busy wait */
+void tee_time_busy_wait(uint32_t milliseconds_delay);
+
+#endif
diff --git a/core/include/kernel/trace_ta.h b/core/include/kernel/trace_ta.h
new file mode 100644
index 0000000..91652fd
--- /dev/null
+++ b/core/include/kernel/trace_ta.h
@@ -0,0 +1,42 @@
+/*
+ * Copyright (c) 2014, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+#ifndef TRACE_TA_H
+#define TRACE_TA_H
+
+#include <trace.h>
+
+/* Macros to trace TA related events, logs, TA crash info etc */
+#ifdef CFG_TEE_CORE_TA_TRACE
+#define TAMSG(...)	EMSG(__VA_ARGS__)
+#define TAMSG_RAW(...)	EMSG_RAW(__VA_ARGS__)
+#else
+#define TAMSG(...)
+#define TAMSG_RAW(...)
+#endif
+
+#endif /*TRACE_TA_H*/
+
diff --git a/core/include/mm/tee_mm.h b/core/include/mm/tee_mm.h
new file mode 100644
index 0000000..b9915f3
--- /dev/null
+++ b/core/include/mm/tee_mm.h
@@ -0,0 +1,135 @@
+/*
+ * 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.
+ */
+
+#ifndef TEE_MM_H
+#define TEE_MM_H
+
+#include <malloc.h>
+#include <types_ext.h>
+
+/* Define to indicate default pool initiation */
+#define TEE_MM_POOL_NO_FLAGS            0
+/* Flag to indicate that memory is allocated from hi address to low address */
+#define TEE_MM_POOL_HI_ALLOC            (1u << 0)
+
+struct _tee_mm_entry_t {
+	struct _tee_mm_pool_t *pool;
+	struct _tee_mm_entry_t *next;
+	uint32_t offset;	/* offset in pages/sections */
+	uint32_t size;		/* size in pages/sections */
+};
+typedef struct _tee_mm_entry_t tee_mm_entry_t;
+
+struct _tee_mm_pool_t {
+	tee_mm_entry_t *entry;
+	paddr_t lo;		/* low boundary of the pool */
+	paddr_t hi;		/* high boundary of the pool */
+	uint32_t flags;		/* Config flags for the pool */
+	uint8_t shift;		/* size shift */
+#ifdef CFG_WITH_STATS
+	size_t max_allocated;
+#endif
+};
+typedef struct _tee_mm_pool_t tee_mm_pool_t;
+
+/* Physical Secure DDR pool */
+extern tee_mm_pool_t tee_mm_sec_ddr;
+
+/* Virtual eSRAM pool */
+extern tee_mm_pool_t tee_mm_vcore;
+
+/*
+ * Returns a pointer to the mm covering the supplied address,
+ * if no mm is found NULL is returned.
+ */
+tee_mm_entry_t *tee_mm_find(const tee_mm_pool_t *pool, paddr_t addr);
+
+/*
+ * Validates that an address (addr) is part of the secure virtual memory
+ * Returns false if not valid, true if valid
+ * NOTE: This function is executed in abort mode.
+ *       Please take care of stack usage
+ */
+static inline bool tee_mm_validate(const tee_mm_pool_t *pool, paddr_t addr)
+{
+	return tee_mm_find(pool, addr) != 0;
+}
+
+/*
+ * Returns a virtual address to the start of the allocated memory
+ * for the mm entry.
+ */
+uintptr_t tee_mm_get_smem(const tee_mm_entry_t *mm);
+
+/* Init managed memory area */
+bool tee_mm_init(tee_mm_pool_t *pool, paddr_t lo, paddr_t hi, uint8_t shift,
+		 uint32_t flags);
+
+/* Kill managed memory area*/
+void tee_mm_final(tee_mm_pool_t *pool);
+
+/*
+ * Allocates size number of bytes in the paged virtual address space
+ * Returns a handle to the memory. The handle is used as an input to
+ * the tee_mm_free function.
+ */
+tee_mm_entry_t *tee_mm_alloc(tee_mm_pool_t *pool, size_t size);
+
+/* Allocate supplied memory range if it's free */
+tee_mm_entry_t *tee_mm_alloc2(tee_mm_pool_t *pool, paddr_t base, size_t size);
+
+/*
+ * Frees the entry in the paged virtual address space pointed to by the
+ * input parameter p
+ */
+void tee_mm_free(tee_mm_entry_t *p);
+
+/* Returns size in sections or pages */
+static inline size_t tee_mm_get_size(tee_mm_entry_t *p)
+{
+	return p->size;
+}
+
+/* Returns offset from start of area in sections or pages */
+static inline uint32_t tee_mm_get_offset(tee_mm_entry_t *p)
+{
+	return p->offset;
+}
+
+/* Return size of the mm entry in bytes */
+size_t tee_mm_get_bytes(const tee_mm_entry_t *mm);
+
+bool tee_mm_addr_is_within_range(tee_mm_pool_t *pool, paddr_t addr);
+
+bool tee_mm_is_empty(tee_mm_pool_t *pool);
+
+#ifdef CFG_WITH_STATS
+void tee_mm_get_pool_stats(tee_mm_pool_t *pool, struct malloc_stats *stats,
+			   bool reset);
+#endif
+
+#endif
diff --git a/core/include/mm/tee_mmu.h b/core/include/mm/tee_mmu.h
new file mode 100644
index 0000000..062196d
--- /dev/null
+++ b/core/include/mm/tee_mmu.h
@@ -0,0 +1,123 @@
+/*
+ * Copyright (c) 2014, STMicroelectronics International N.V.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+#ifndef TEE_MMU_H
+#define TEE_MMU_H
+
+#include <tee_api_types.h>
+#include <kernel/tee_ta_manager.h>
+#include <kernel/user_ta.h>
+
+/*-----------------------------------------------------------------------------
+ * Allocate context resources like ASID and MMU table information
+ *---------------------------------------------------------------------------*/
+TEE_Result tee_mmu_init(struct user_ta_ctx *utc);
+
+/*-----------------------------------------------------------------------------
+ * tee_mmu_final - Release context resources like ASID
+ *---------------------------------------------------------------------------*/
+void tee_mmu_final(struct user_ta_ctx *utc);
+
+/* Map stack of a user TA.  */
+void tee_mmu_map_stack(struct user_ta_ctx *utc, struct mobj *mobj);
+/*
+ * Map a code segment of a user TA, this function may be called multiple
+ * times if there's several segments.
+ */
+TEE_Result tee_mmu_map_add_segment(struct user_ta_ctx *utc, struct mobj *mobj,
+				   size_t offs, size_t size, uint32_t prot);
+
+void tee_mmu_map_clear(struct user_ta_ctx *utc);
+
+/* Map parameters for a user TA */
+TEE_Result tee_mmu_map_param(struct user_ta_ctx *utc,
+		struct tee_ta_param *param, void *param_va[TEE_NUM_PARAMS]);
+
+/*
+ * If the rwmem area covers more than one page directory @pgdir_offset has
+ * to be honoured unless it's -1.
+ */
+TEE_Result tee_mmu_add_rwmem(struct user_ta_ctx *utc, struct mobj *mobj,
+			     int pgdir_offset, vaddr_t *va);
+void tee_mmu_rem_rwmem(struct user_ta_ctx *utc, struct mobj *mobj, vaddr_t va);
+
+/*
+ * TA private memory is defined as TA image static segment (code, ro/rw static
+ * data, heap, stack). The sole other virtual memory mapped to TA are memref
+ * parameters. These later are considered outside TA private memory as it
+ * might be accessed by the TA and its client(s).
+ */
+bool tee_mmu_is_vbuf_inside_ta_private(const struct user_ta_ctx *utc,
+				       const void *va, size_t size);
+
+bool tee_mmu_is_vbuf_intersect_ta_private(const struct user_ta_ctx *utc,
+					  const void *va, size_t size);
+
+TEE_Result tee_mmu_vbuf_to_mobj_offs(const struct user_ta_ctx *utc,
+				     const void *va, size_t size,
+				     struct mobj **mobj, size_t *offs);
+
+/*-----------------------------------------------------------------------------
+ * tee_mmu_user_va2pa - Translate virtual user address to physical address
+ * given the user context.
+ * Interface is deprecated, use virt_to_phys() instead.
+ *---------------------------------------------------------------------------*/
+TEE_Result tee_mmu_user_va2pa_helper(const struct user_ta_ctx *utc, void *ua,
+				     paddr_t *pa);
+
+/*-----------------------------------------------------------------------------
+ * tee_mmu_user_va2pa - Translate physical address to virtual user address
+ * given the user context.
+ * Interface is deprecated, use phys_to_virt() instead.
+ *---------------------------------------------------------------------------*/
+TEE_Result tee_mmu_user_pa2va_helper(const struct user_ta_ctx *utc,
+				     paddr_t pa, void **va);
+
+/*-----------------------------------------------------------------------------
+ * tee_mmu_check_access_rights -
+ *---------------------------------------------------------------------------*/
+TEE_Result tee_mmu_check_access_rights(const struct user_ta_ctx *utc,
+				       uint32_t flags, uaddr_t uaddr,
+				       size_t len);
+
+/*-----------------------------------------------------------------------------
+ * If ctx is NULL user mapping is removed and ASID set to 0
+ *---------------------------------------------------------------------------*/
+void tee_mmu_set_ctx(struct tee_ta_ctx *ctx);
+struct tee_ta_ctx *tee_mmu_get_ctx(void);
+
+/* Returns virtual address to which TA is loaded */
+uintptr_t tee_mmu_get_load_addr(const struct tee_ta_ctx *const ctx);
+
+/* init some allocation pools */
+void teecore_init_ta_ram(void);
+void teecore_init_pub_ram(void);
+
+uint32_t tee_mmu_user_get_cache_attr(struct user_ta_ctx *utc, void *va);
+
+TEE_Result tee_mmu_register_shm(paddr_t pa, size_t len, uint32_t attr);
+
+#endif
diff --git a/core/include/mm/tee_mmu_types.h b/core/include/mm/tee_mmu_types.h
new file mode 100644
index 0000000..891f254
--- /dev/null
+++ b/core/include/mm/tee_mmu_types.h
@@ -0,0 +1,92 @@
+/*
+ * 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.
+ */
+#ifndef TEE_MMU_TYPES_H
+#define TEE_MMU_TYPES_H
+
+#include <stdint.h>
+
+#define TEE_MATTR_VALID_BLOCK		(1 << 0)
+#define TEE_MATTR_HIDDEN_BLOCK		(1 << 1)
+#define TEE_MATTR_HIDDEN_DIRTY_BLOCK	(1 << 2)
+#define	TEE_MATTR_TABLE			(1 << 3)
+#define	TEE_MATTR_PR			(1 << 4)
+#define	TEE_MATTR_PW			(1 << 5)
+#define	TEE_MATTR_PX			(1 << 6)
+#define	TEE_MATTR_PRW			(TEE_MATTR_PR | TEE_MATTR_PW)
+#define	TEE_MATTR_PRX			(TEE_MATTR_PR | TEE_MATTR_PX)
+#define	TEE_MATTR_PRWX			(TEE_MATTR_PRW | TEE_MATTR_PX)
+#define	TEE_MATTR_UR			(1 << 7)
+#define	TEE_MATTR_UW			(1 << 8)
+#define	TEE_MATTR_UX			(1 << 9)
+#define	TEE_MATTR_URW			(TEE_MATTR_UR | TEE_MATTR_UW)
+#define	TEE_MATTR_URX			(TEE_MATTR_UR | TEE_MATTR_UX)
+#define	TEE_MATTR_URWX			(TEE_MATTR_URW | TEE_MATTR_UX)
+
+#define TEE_MATTR_GLOBAL		(1 << 10)
+#define	TEE_MATTR_SECURE		(1 << 11)
+
+#define TEE_MATTR_CACHE_MASK	0x7
+#define TEE_MATTR_CACHE_SHIFT	12
+/* These are shifted TEE_MATTR_CACHE_SHIFT */
+#define TEE_MATTR_CACHE_NONCACHE 0
+#define TEE_MATTR_CACHE_CACHED	1
+
+#define TEE_MATTR_LOCKED		(1 << 15)
+
+#define TEE_MMU_UMAP_STACK_IDX	0
+#define TEE_MMU_UMAP_CODE_IDX	1
+#define TEE_MMU_UMAP_NUM_CODE_SEGMENTS	3
+
+#define TEE_MMU_UMAP_PARAM_IDX		(TEE_MMU_UMAP_CODE_IDX + \
+					 TEE_MMU_UMAP_NUM_CODE_SEGMENTS)
+#define TEE_MMU_UMAP_MAX_ENTRIES	(TEE_MMU_UMAP_PARAM_IDX + \
+					 TEE_NUM_PARAMS)
+
+struct tee_mmap_region {
+	unsigned int type; /* enum teecore_memtypes */
+	unsigned int region_size;
+	paddr_t pa;
+	vaddr_t va;
+	size_t size;
+	uint32_t attr; /* TEE_MATTR_* above */
+};
+
+struct tee_ta_region {
+	struct mobj *mobj;
+	size_t offset;
+	vaddr_t va;
+	size_t size;
+	uint32_t attr; /* TEE_MATTR_* above */
+};
+
+struct tee_mmu_info {
+	struct tee_ta_region regions[TEE_MMU_UMAP_MAX_ENTRIES];
+	vaddr_t ta_private_vmem_start;
+	vaddr_t ta_private_vmem_end;
+};
+
+#endif
diff --git a/core/include/optee_msg.h b/core/include/optee_msg.h
new file mode 100644
index 0000000..09be461
--- /dev/null
+++ b/core/include/optee_msg.h
@@ -0,0 +1,422 @@
+/*
+ * 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.
+ */
+#ifndef _OPTEE_MSG_H
+#define _OPTEE_MSG_H
+
+#include <compiler.h>
+#include <types_ext.h>
+
+/*
+ * This file defines the OP-TEE message protocol used to communicate
+ * with an instance of OP-TEE running in secure world.
+ *
+ * This file is divided into three sections.
+ * 1. Formatting of messages.
+ * 2. Requests from normal world
+ * 3. Requests from secure world, Remote Procedure Call (RPC), handled by
+ *    tee-supplicant.
+ */
+
+/*****************************************************************************
+ * Part 1 - formatting of messages
+ *****************************************************************************/
+
+#define OPTEE_MSG_ATTR_TYPE_NONE		0x0
+#define OPTEE_MSG_ATTR_TYPE_VALUE_INPUT		0x1
+#define OPTEE_MSG_ATTR_TYPE_VALUE_OUTPUT	0x2
+#define OPTEE_MSG_ATTR_TYPE_VALUE_INOUT		0x3
+#define OPTEE_MSG_ATTR_TYPE_RMEM_INPUT		0x5
+#define OPTEE_MSG_ATTR_TYPE_RMEM_OUTPUT		0x6
+#define OPTEE_MSG_ATTR_TYPE_RMEM_INOUT		0x7
+#define OPTEE_MSG_ATTR_TYPE_TMEM_INPUT		0x9
+#define OPTEE_MSG_ATTR_TYPE_TMEM_OUTPUT		0xa
+#define OPTEE_MSG_ATTR_TYPE_TMEM_INOUT		0xb
+
+#define OPTEE_MSG_ATTR_TYPE_MASK		0xff
+
+/*
+ * Meta parameter to be absorbed by the Secure OS and not passed
+ * to the Trusted Application.
+ *
+ * Currently only used with OPTEE_MSG_CMD_OPEN_SESSION.
+ */
+#define OPTEE_MSG_ATTR_META			(1 << 8)
+
+/*
+ * The temporary shared memory object is not physically contiguous and this
+ * temp memref is followed by another fragment until the last temp memref
+ * that doesn't have this bit set.
+ */
+#define OPTEE_MSG_ATTR_FRAGMENT			(1 << 9)
+
+/*
+ * Memory attributes for caching passed with temp memrefs. The actual value
+ * used is defined outside the message protocol with the exception of
+ * OPTEE_MSG_ATTR_CACHE_PREDEFINED which means the attributes already
+ * defined for the memory range should be used. If optee_smc.h is used as
+ * bearer of this protocol OPTEE_SMC_SHM_* is used for values.
+ */
+#define OPTEE_MSG_ATTR_CACHE_SHIFT		16
+#define OPTEE_MSG_ATTR_CACHE_MASK		0x7
+#define OPTEE_MSG_ATTR_CACHE_PREDEFINED		0
+
+/*
+ * Same values as TEE_LOGIN_* from TEE Internal API
+ */
+#define OPTEE_MSG_LOGIN_PUBLIC			0x00000000
+#define OPTEE_MSG_LOGIN_USER			0x00000001
+#define OPTEE_MSG_LOGIN_GROUP			0x00000002
+#define OPTEE_MSG_LOGIN_APPLICATION		0x00000004
+#define OPTEE_MSG_LOGIN_APPLICATION_USER	0x00000005
+#define OPTEE_MSG_LOGIN_APPLICATION_GROUP	0x00000006
+
+#ifndef ASM
+/**
+ * struct optee_msg_param_tmem - temporary memory reference
+ * @buf_ptr:	Address of the buffer
+ * @size:	Size of the buffer
+ * @shm_ref:	Temporary shared memory reference, pointer to a struct tee_shm
+ *
+ * Secure and normal world communicates pointers as physical address
+ * instead of the virtual address. This is because secure and normal world
+ * have completely independent memory mapping. Normal world can even have a
+ * hypervisor which need to translate the guest physical address (AKA IPA
+ * in ARM documentation) to a real physical address before passing the
+ * structure to secure world.
+ */
+struct optee_msg_param_tmem {
+	uint64_t buf_ptr;
+	uint64_t size;
+	uint64_t shm_ref;
+};
+
+/**
+ * struct optee_msg_param_rmem - registered memory reference
+ * @offs:	Offset into shared memory reference
+ * @size:	Size of the buffer
+ * @shm_ref:	Shared memory reference, pointer to a struct tee_shm
+ */
+struct optee_msg_param_rmem {
+	uint64_t offs;
+	uint64_t size;
+	uint64_t shm_ref;
+};
+
+/**
+ * struct optee_msg_param_value - values
+ * @a: first value
+ * @b: second value
+ * @c: third value
+ */
+struct optee_msg_param_value {
+	uint64_t a;
+	uint64_t b;
+	uint64_t c;
+};
+
+/**
+ * struct optee_msg_param - parameter
+ * @attr: attributes
+ * @memref: a memory reference
+ * @value: a value
+ *
+ * @attr & OPTEE_MSG_ATTR_TYPE_MASK indicates if tmem, rmem or value is used in
+ * the union. OPTEE_MSG_ATTR_TYPE_VALUE_* indicates value,
+ * OPTEE_MSG_ATTR_TYPE_TMEM_* indicates tmem and
+ * OPTEE_MSG_ATTR_TYPE_RMEM_* indicates rmem.
+ * OPTEE_MSG_ATTR_TYPE_NONE indicates that none of the members are used.
+ */
+struct optee_msg_param {
+	uint64_t attr;
+	union {
+		struct optee_msg_param_tmem tmem;
+		struct optee_msg_param_rmem rmem;
+		struct optee_msg_param_value value;
+	} u;
+};
+
+/**
+ * struct optee_msg_arg - call argument
+ * @cmd: Command, one of OPTEE_MSG_CMD_* or OPTEE_MSG_RPC_CMD_*
+ * @func: Trusted Application function, specific to the Trusted Application,
+ *	     used if cmd == OPTEE_MSG_CMD_INVOKE_COMMAND
+ * @session: In parameter for all OPTEE_MSG_CMD_* except
+ *	     OPTEE_MSG_CMD_OPEN_SESSION where it's an output parameter instead
+ * @cancel_id: Cancellation id, a unique value to identify this request
+ * @ret: return value
+ * @ret_origin: origin of the return value
+ * @num_params: number of parameters supplied to the OS Command
+ * @params: the parameters supplied to the OS Command
+ *
+ * All normal calls to Trusted OS uses this struct. If cmd requires further
+ * information than what these fields hold it can be passed as a parameter
+ * tagged as meta (setting the OPTEE_MSG_ATTR_META bit in corresponding
+ * attrs field). All parameters tagged as meta have to come first.
+ *
+ * Temp memref parameters can be fragmented if supported by the Trusted OS
+ * (when optee_smc.h is bearer of this protocol this is indicated with
+ * OPTEE_SMC_SEC_CAP_UNREGISTERED_SHM). If a logical memref parameter is
+ * fragmented then all but the last fragment have the
+ * OPTEE_MSG_ATTR_FRAGMENT bit set in attrs. Even if a memref is fragmented
+ * it will still be presented as a single logical memref to the Trusted
+ * Application.
+ */
+struct optee_msg_arg {
+	uint32_t cmd;
+	uint32_t func;
+	uint32_t session;
+	uint32_t cancel_id;
+	uint32_t pad;
+	uint32_t ret;
+	uint32_t ret_origin;
+	uint32_t num_params;
+
+	/*
+	 * this struct is 8 byte aligned since the 'struct optee_msg_param'
+	 * which follows requires 8 byte alignment.
+	 *
+	 * Commented out element used to visualize the layout dynamic part
+	 * of the struct. This field is not available at all if
+	 * num_params == 0.
+	 *
+	 * params is accessed through the macro OPTEE_MSG_GET_PARAMS
+	 *
+	 * struct optee_msg_param params[num_params];
+	 */
+} __aligned(8);
+
+/**
+ * OPTEE_MSG_GET_PARAMS - return pointer to struct optee_msg_param *
+ *
+ * @x: Pointer to a struct optee_msg_arg
+ *
+ * Returns a pointer to the params[] inside a struct optee_msg_arg.
+ */
+#define OPTEE_MSG_GET_PARAMS(x) \
+	(struct optee_msg_param *)(((struct optee_msg_arg *)(x)) + 1)
+
+/**
+ * OPTEE_MSG_GET_ARG_SIZE - return size of struct optee_msg_arg
+ *
+ * @num_params: Number of parameters embedded in the struct optee_msg_arg
+ *
+ * Returns the size of the struct optee_msg_arg together with the number
+ * of embedded parameters.
+ */
+#define OPTEE_MSG_GET_ARG_SIZE(num_params) \
+	(sizeof(struct optee_msg_arg) + \
+	 sizeof(struct optee_msg_param) * (num_params))
+#endif /*ASM*/
+
+/*****************************************************************************
+ * Part 2 - requests from normal world
+ *****************************************************************************/
+
+/*
+ * Return the following UID if using API specified in this file without
+ * further extensions:
+ * 384fb3e0-e7f8-11e3-af63-0002a5d5c51b.
+ * Represented in 4 32-bit words in OPTEE_MSG_UID_0, OPTEE_MSG_UID_1,
+ * OPTEE_MSG_UID_2, OPTEE_MSG_UID_3.
+ */
+#define OPTEE_MSG_UID_0			0x384fb3e0
+#define OPTEE_MSG_UID_1			0xe7f811e3
+#define OPTEE_MSG_UID_2			0xaf630002
+#define OPTEE_MSG_UID_3			0xa5d5c51b
+#define OPTEE_MSG_FUNCID_CALLS_UID	0xFF01
+
+/*
+ * Returns 2.0 if using API specified in this file without further
+ * extensions. Represented in 2 32-bit words in OPTEE_MSG_REVISION_MAJOR
+ * and OPTEE_MSG_REVISION_MINOR
+ */
+#define OPTEE_MSG_REVISION_MAJOR	2
+#define OPTEE_MSG_REVISION_MINOR	0
+#define OPTEE_MSG_FUNCID_CALLS_REVISION	0xFF03
+
+/*
+ * Get UUID of Trusted OS.
+ *
+ * Used by non-secure world to figure out which Trusted OS is installed.
+ * Note that returned UUID is the UUID of the Trusted OS, not of the API.
+ *
+ * Returns UUID in 4 32-bit words in the same way as
+ * OPTEE_MSG_FUNCID_CALLS_UID described above.
+ */
+#define OPTEE_MSG_OS_OPTEE_UUID_0	0x486178e0
+#define OPTEE_MSG_OS_OPTEE_UUID_1	0xe7f811e3
+#define OPTEE_MSG_OS_OPTEE_UUID_2	0xbc5e0002
+#define OPTEE_MSG_OS_OPTEE_UUID_3	0xa5d5c51b
+#define OPTEE_MSG_FUNCID_GET_OS_UUID	0x0000
+
+/*
+ * Get revision of Trusted OS.
+ *
+ * Used by non-secure world to figure out which version of the Trusted OS
+ * is installed. Note that the returned revision is the revision of the
+ * Trusted OS, not of the API.
+ *
+ * Returns revision in 2 32-bit words in the same way as
+ * OPTEE_MSG_CALLS_REVISION described above.
+ */
+#define OPTEE_MSG_FUNCID_GET_OS_REVISION	0x0001
+
+/*
+ * Do a secure call with struct optee_msg_arg as argument
+ * The OPTEE_MSG_CMD_* below defines what goes in struct optee_msg_arg::cmd
+ *
+ * OPTEE_MSG_CMD_OPEN_SESSION opens a session to a Trusted Application.
+ * The first two parameters are tagged as meta, holding two value
+ * parameters to pass the following information:
+ * param[0].u.value.a-b uuid of Trusted Application
+ * param[1].u.value.a-b uuid of Client
+ * param[1].u.value.c Login class of client OPTEE_MSG_LOGIN_*
+ *
+ * OPTEE_MSG_CMD_INVOKE_COMMAND invokes a command a previously opened
+ * session to a Trusted Application.  struct optee_msg_arg::func is Trusted
+ * Application function, specific to the Trusted Application.
+ *
+ * OPTEE_MSG_CMD_CLOSE_SESSION closes a previously opened session to
+ * Trusted Application.
+ *
+ * OPTEE_MSG_CMD_CANCEL cancels a currently invoked command.
+ *
+ * OPTEE_MSG_CMD_REGISTER_SHM registers a shared memory reference. The
+ * information is passed as:
+ * [in] param[0].attr			OPTEE_MSG_ATTR_TYPE_TMEM_INPUT
+ *					[| OPTEE_MSG_ATTR_FRAGMENT]
+ * [in] param[0].u.tmem.buf_ptr		physical address (of first fragment)
+ * [in] param[0].u.tmem.size		size (of first fragment)
+ * [in] param[0].u.tmem.shm_ref		holds shared memory reference
+ * ...
+ * The shared memory can optionally be fragmented, temp memrefs can follow
+ * each other with all but the last with the OPTEE_MSG_ATTR_FRAGMENT bit set.
+ *
+ * OPTEE_MSG_CMD_UNREGISTER_SHM unregisteres a previously registered shared
+ * memory reference. The information is passed as:
+ * [in] param[0].attr			OPTEE_MSG_ATTR_TYPE_RMEM_INPUT
+ * [in] param[0].u.rmem.shm_ref		holds shared memory reference
+ * [in] param[0].u.rmem.offs		0
+ * [in] param[0].u.rmem.size		0
+ */
+#define OPTEE_MSG_CMD_OPEN_SESSION	0
+#define OPTEE_MSG_CMD_INVOKE_COMMAND	1
+#define OPTEE_MSG_CMD_CLOSE_SESSION	2
+#define OPTEE_MSG_CMD_CANCEL		3
+#define OPTEE_MSG_CMD_REGISTER_SHM	4
+#define OPTEE_MSG_CMD_UNREGISTER_SHM	5
+#define OPTEE_MSG_FUNCID_CALL_WITH_ARG	0x0004
+
+/*****************************************************************************
+ * Part 3 - Requests from secure world, RPC
+ *****************************************************************************/
+
+/*
+ * All RPC is done with a struct optee_msg_arg as bearer of information,
+ * struct optee_msg_arg::arg holds values defined by OPTEE_MSG_RPC_CMD_* below.
+ * Only the commands handled by the kernel driver are defined here.
+ *
+ * RPC communication with tee-supplicant is reversed compared to normal
+ * client communication described above. The supplicant receives requests
+ * and sends responses. The command codes and exact protocol are defined in an
+ * external header file.
+ */
+
+/*
+ * Get time
+ *
+ * Returns number of seconds and nano seconds since the Epoch,
+ * 1970-01-01 00:00:00 +0000 (UTC).
+ *
+ * [out] param[0].u.value.a	Number of seconds
+ * [out] param[0].u.value.b	Number of nano seconds.
+ */
+#define OPTEE_MSG_RPC_CMD_GET_TIME	3
+
+/*
+ * Wait queue primitive, helper for secure world to implement a wait queue
+ *
+ * Waiting on a key
+ * [in] param[0].u.value.a OPTEE_MSG_RPC_WAIT_QUEUE_SLEEP
+ * [in] param[0].u.value.b wait key
+ *
+ * Waking up a key
+ * [in] param[0].u.value.a OPTEE_MSG_RPC_WAIT_QUEUE_WAKEUP
+ * [in] param[0].u.value.b wakeup key
+ */
+#define OPTEE_MSG_RPC_CMD_WAIT_QUEUE	4
+#define OPTEE_MSG_RPC_WAIT_QUEUE_SLEEP	0
+#define OPTEE_MSG_RPC_WAIT_QUEUE_WAKEUP	1
+
+/*
+ * Suspend execution
+ *
+ * [in] param[0].value	.a number of milliseconds to suspend
+ */
+#define OPTEE_MSG_RPC_CMD_SUSPEND	5
+
+/*
+ * Allocate a piece of shared memory
+ *
+ * Shared memory can optionally be fragmented, to support that additional
+ * spare param entries are allocated to make room for eventual fragments.
+ * The spare param entries has .attr = OPTEE_MSG_ATTR_TYPE_NONE when
+ * unused. All returned temp memrefs except the last should have the
+ * OPTEE_MSG_ATTR_FRAGMENT bit set in the attr field.
+ *
+ * [in]  param[0].u.value.a		type of memory one of
+ *					OPTEE_MSG_RPC_SHM_TYPE_* below
+ * [in]  param[0].u.value.b		requested size
+ * [in]  param[0].u.value.c		required alignment
+ *
+ * [out] param[0].u.tmem.buf_ptr	physical address (of first fragment)
+ * [out] param[0].u.tmem.size		size (of first fragment)
+ * [out] param[0].u.tmem.shm_ref	shared memory reference
+ * ...
+ * [out] param[n].u.tmem.buf_ptr	physical address
+ * [out] param[n].u.tmem.size		size
+ * [out] param[n].u.tmem.shm_ref	shared memory reference (same value
+ *					as in param[n-1].u.tmem.shm_ref)
+ */
+#define OPTEE_MSG_RPC_CMD_SHM_ALLOC	6
+/* Memory that can be shared with a non-secure user space application */
+#define OPTEE_MSG_RPC_SHM_TYPE_APPL	0
+/* Memory only shared with non-secure kernel */
+#define OPTEE_MSG_RPC_SHM_TYPE_KERNEL	1
+
+/*
+ * Free shared memory previously allocated with OPTEE_MSG_RPC_CMD_SHM_ALLOC
+ *
+ * [in]  param[0].u.value.a		type of memory one of
+ *					OPTEE_MSG_RPC_SHM_TYPE_* above
+ * [in]  param[0].u.value.b		value of shared memory reference
+ *					returned in param[0].u.tmem.shm_ref
+ *					above
+ */
+#define OPTEE_MSG_RPC_CMD_SHM_FREE	7
+
+#endif /* _OPTEE_MSG_H */
diff --git a/core/include/optee_msg_supplicant.h b/core/include/optee_msg_supplicant.h
new file mode 100644
index 0000000..e54dac0
--- /dev/null
+++ b/core/include/optee_msg_supplicant.h
@@ -0,0 +1,279 @@
+/*
+ * Copyright (c) 2016-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 __OPTEE_MSG_SUPPLICANT_H
+#define __OPTEE_MSG_SUPPLICANT_H
+
+/*
+ * Load a TA into memory
+ */
+#define OPTEE_MSG_RPC_CMD_LOAD_TA	0
+
+/*
+ * Replay Protected Memory Block access
+ */
+#define OPTEE_MSG_RPC_CMD_RPMB		1
+
+/*
+ * File system access
+ */
+#define OPTEE_MSG_RPC_CMD_FS		2
+
+/*
+ * Values 3-7 are reserved in optee_msg.h for use by the kernel driver
+ */
+
+/*
+ * SQLite file system access
+ */
+#define OPTEE_MSG_RPC_CMD_SQL_FS	8
+
+/*
+ * Define protocol for messages with .cmd == OPTEE_MSG_RPC_CMD_FS or
+ * .cmd == OPTEE_MSG_RPC_CMD_SQL_FS and first parameter has the attribute
+ * OPTEE_MSG_ATTR_TYPE_VALUE_INPUT.
+ */
+
+/*
+ * Open a file
+ *
+ * [in]     param[0].u.value.a	OPTEE_MRF_OPEN
+ * [in]     param[1].u.tmem	a string holding the file name
+ * [out]    param[2].u.value.a	file descriptor of open file
+ */
+#define OPTEE_MRF_OPEN			0
+
+/*
+ * Create a file
+ *
+ * [in]     param[0].u.value.a	OPTEE_MRF_CREATE
+ * [in]     param[1].u.tmem	a string holding the file name
+ * [out]    param[2].u.value.a	file descriptor of open file
+ */
+#define OPTEE_MRF_CREATE		1
+
+/*
+ * Close a file
+ *
+ * [in]     param[0].u.value.a	OPTEE_MRF_CLOSE
+ * [in]     param[0].u.value.b	file descriptor of open file.
+ */
+#define OPTEE_MRF_CLOSE			2
+
+/*
+ * Read from a file
+ *
+ * [in]     param[0].u.value.a	OPTEE_MRF_READ
+ * [in]     param[0].u.value.b	file descriptor of open file
+ * [in]     param[0].u.value.c	offset into file
+ * [out]    param[1].u.tmem	buffer to hold returned data
+ */
+#define OPTEE_MRF_READ			3
+
+/*
+ * Write to a file
+ *
+ * [in]     param[0].u.value.a	OPTEE_MRF_WRITE
+ * [in]     param[0].u.value.b	file descriptor of open file
+ * [in]     param[0].u.value.c	offset into file
+ * [in]     param[1].u.tmem	buffer holding data to be written
+ */
+#define OPTEE_MRF_WRITE			4
+
+/*
+ * Truncate a file
+ *
+ * [in]     param[0].u.value.a	OPTEE_MRF_TRUNCATE
+ * [in]     param[0].u.value.b	file descriptor of open file
+ * [in]     param[0].u.value.c	length of file.
+ */
+#define OPTEE_MRF_TRUNCATE		5
+
+/*
+ * Remove a file
+ *
+ * [in]  param[0].u.value.a	OPTEE_MRF_REMOVE
+ * [in]  param[1].u.tmem	a string holding the file name
+ */
+#define OPTEE_MRF_REMOVE		6
+
+/*
+ * Rename a file
+ *
+ * [in]  param[0].u.value.a	OPTEE_MRF_RENAME
+ * [in]  param[0].u.value.b	true if existing target should be removed
+ * [in]  param[1].u.tmem	a string holding the old file name
+ * [in]  param[2].u.tmem	a string holding the new file name
+ */
+#define OPTEE_MRF_RENAME		7
+
+/*
+ * Opens a directory for file listing
+ *
+ * [in]  param[0].u.value.a	OPTEE_MRF_OPENDIR
+ * [in]  param[1].u.tmem	a string holding the name of the directory
+ * [out] param[2].u.value.a	handle to open directory
+ */
+#define OPTEE_MRF_OPENDIR		8
+
+/*
+ * Closes a directory handle
+ *
+ * [in]  param[0].u.value.a	OPTEE_MRF_CLOSEDIR
+ * [in]  param[0].u.value.b	handle to open directory
+ */
+#define OPTEE_MRF_CLOSEDIR		9
+
+/*
+ * Read next file name of directory
+ *
+ *
+ * [in]  param[0].u.value.a	OPTEE_MRF_READDIR
+ * [in]  param[0].u.value.b	handle to open directory
+ * [out] param[1].u.tmem	a string holding the file name
+ */
+#define OPTEE_MRF_READDIR		10
+
+/*
+ * Begins a new transaction (only valid for SQL FS)
+ *
+ * [in]  param[0].u.value.a	OPTEE_MRF_BEGIN_TRANSACTION
+ */
+#define OPTEE_MRF_BEGIN_TRANSACTION	11
+
+/*
+ * Ends a transaction (only valid for SQL FS)
+ *
+ * [in]  param[0].u.value.a	OPTEE_MRF_END_TRANSACTION
+ * [in]  param[0].u.value.b	true if rolling back to previous state
+ */
+#define OPTEE_MRF_END_TRANSACTION	12
+
+/*
+ * End of definitions for messages with .cmd == OPTEE_MSG_RPC_CMD_FS or
+ * .cmd == OPTEE_MSG_RPC_CMD_SQL_FS
+ */
+/*
+ * Send TA profiling information to normal world
+ *
+ * [in/out] param[0].u.value.a		File identifier. Must be set to 0 on
+ *					first call. A value >= 1 will be
+ *					returned on success. Re-use this value
+ *					to append data to the same file.
+ *
+ * [in] param[1].u.tmem.buf_ptr		Physical address of TA UUID
+ * [in] param[1].u.tmem.size		Size of UUID
+ * [in] param[1].u.tmem.shm_ref		Shared memory reference
+ *
+ * [in] param[2].u.tmem.buf_ptr		Physical address of profile data buffer
+ * [in] param[2].u.tmem.size		Buffer size
+ * [in] param[2].u.tmem.shm_ref		Shared memory reference
+ */
+#define OPTEE_MSG_RPC_CMD_GPROF		9
+
+/*
+ * Socket commands
+ */
+#define OPTEE_MSG_RPC_CMD_SOCKET	10
+
+
+/*
+ * Define protocol for messages with .cmd == OPTEE_MSG_RPC_CMD_SOCKET
+ */
+
+#define OPTEE_MRC_SOCKET_TIMEOUT_NONBLOCKING	0
+#define OPTEE_MRC_SOCKET_TIMEOUT_BLOCKING	0xffffffff
+
+/*
+ * Open socket
+ *
+ * [in]     param[0].u.value.a	OPTEE_MRC_SOCKET_OPEN
+ * [in]     param[0].u.value.b	TA instance id
+ * [in]     param[1].u.value.a	server port number
+ * [in]     param[1].u.value.b	protocol, TEE_ISOCKET_PROTOCOLID_*
+ * [in]     param[1].u.value.c	ip version TEE_IP_VERSION_* from tee_ipsocket.h
+ * [in]     param[2].u.tmem	server address
+ * [out]    param[3].u.value.a	socket handle (32-bit)
+ */
+#define OPTEE_MRC_SOCKET_OPEN	0
+
+/*
+ * Close socket
+ *
+ * [in]     param[0].u.value.a	OPTEE_MRC_SOCKET_CLOSE
+ * [in]     param[0].u.value.b	TA instance id
+ * [in]     param[0].u.value.c	socket handle
+ */
+#define OPTEE_MRC_SOCKET_CLOSE	1
+
+/*
+ * Close all sockets
+ *
+ * [in]     param[0].u.value.a	OPTEE_MRC_SOCKET_CLOSE_ALL
+ * [in]     param[0].u.value.b	TA instance id
+ */
+#define OPTEE_MRC_SOCKET_CLOSE_ALL 2
+
+/*
+ * Send data on socket
+ *
+ * [in]     param[0].u.value.a	OPTEE_MRC_SOCKET_SEND
+ * [in]     param[0].u.value.b	TA instance id
+ * [in]     param[0].u.value.c	socket handle
+ * [in]     param[1].u.tmem	buffer to transmit
+ * [in]     param[2].u.value.a	timeout ms or OPTEE_MRC_SOCKET_TIMEOUT_*
+ * [out]    param[2].u.value.b	number of transmitted bytes
+ */
+#define OPTEE_MRC_SOCKET_SEND	3
+
+/*
+ * Receive data on socket
+ *
+ * [in]     param[0].u.value.a	OPTEE_MRC_SOCKET_RECV
+ * [in]     param[0].u.value.b	TA instance id
+ * [in]     param[0].u.value.c	socket handle
+ * [out]    param[1].u.tmem	buffer to receive
+ * [in]     param[2].u.value.a	timeout ms or OPTEE_MRC_SOCKET_TIMEOUT_*
+ */
+#define OPTEE_MRC_SOCKET_RECV	4
+
+/*
+ * Perform IOCTL on socket
+ *
+ * [in]     param[0].u.value.a	OPTEE_MRC_SOCKET_IOCTL
+ * [in]     param[0].u.value.b	TA instance id
+ * [in]     param[0].u.value.c	socket handle
+ * [in/out] param[1].u.tmem	buffer
+ * [in]     param[2].u.value.a	ioctl command
+ */
+#define OPTEE_MRC_SOCKET_IOCTL	5
+
+/*
+ * End of definitions for messages with .cmd == OPTEE_MSG_RPC_CMD_SOCKET
+ */
+
+#endif /*__OPTEE_MSG_SUPPLICANT_H*/
diff --git a/core/include/rng_support.h b/core/include/rng_support.h
new file mode 100644
index 0000000..29d3f75
--- /dev/null
+++ b/core/include/rng_support.h
@@ -0,0 +1,34 @@
+/*
+ * 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.
+ */
+#ifndef __RNG_SUPPORT_H__
+#define __RNG_SUPPORT_H__
+
+#include <stdint.h>
+
+uint8_t hw_get_random_byte(void);
+
+#endif /* __RNG_SUPPORT_H__ */
diff --git a/core/include/signed_hdr.h b/core/include/signed_hdr.h
new file mode 100644
index 0000000..0fd87c2
--- /dev/null
+++ b/core/include/signed_hdr.h
@@ -0,0 +1,75 @@
+/*
+ * 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.
+ */
+#ifndef SIGNED_HDR_H
+#define SIGNED_HDR_H
+
+#include <inttypes.h>
+
+enum shdr_img_type {
+	SHDR_TA = 0,
+};
+
+#define SHDR_MAGIC	0x4f545348
+
+/**
+ * struct shdr - signed header
+ * @magic:	magic number must match SHDR_MAGIC
+ * @img_type:	image type, values defined by enum shdr_img_type
+ * @img_size:	image size in bytes
+ * @algo:	algorithm, defined by public key algorithms TEE_ALG_*
+ *		from TEE Internal API specification
+ * @hash_size:	size of the signed hash
+ * @sig_size:	size of the signature
+ * @hash:	hash of an image
+ * @sig:	signature of @hash
+ */
+struct shdr {
+	uint32_t magic;
+	uint32_t img_type;
+	uint32_t img_size;
+	uint32_t algo;
+	uint16_t hash_size;
+	uint16_t sig_size;
+	/*
+	 * Commented out element used to visualize the layout dynamic part
+	 * of the struct.
+	 *
+	 * hash is accessed through the macro SHDR_GET_HASH and
+	 * signature is accessed through the macro SHDR_GET_SIG
+	 *
+	 * uint8_t hash[hash_size];
+	 * uint8_t sig[sig_size];
+	 */
+};
+
+#define SHDR_GET_SIZE(x)	(sizeof(struct shdr) + (x)->hash_size + \
+				 (x)->sig_size)
+#define SHDR_GET_HASH(x)	(uint8_t *)(((struct shdr *)(x)) + 1)
+#define SHDR_GET_SIG(x)		(SHDR_GET_HASH(x) + (x)->hash_size)
+
+#endif /*SIGNED_HDR_H*/
+
diff --git a/core/include/spi.h b/core/include/spi.h
new file mode 100644
index 0000000..c554d96
--- /dev/null
+++ b/core/include/spi.h
@@ -0,0 +1,62 @@
+/*
+ * Copyright (c) 2016, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef __SPI_H__
+#define __SPI_H__
+
+#include <types_ext.h>
+
+enum spi_mode {
+	SPI_MODE0,
+	SPI_MODE1,
+	SPI_MODE2,
+	SPI_MODE3
+};
+
+enum spi_result {
+	SPI_OK,
+	SPI_ERR_CFG,
+	SPI_ERR_PKTCNT,
+	SPI_ERR_GENERIC
+};
+
+struct spi_chip {
+	const struct spi_ops *ops;
+};
+
+struct spi_ops {
+	void (*configure)(struct spi_chip *chip);
+	void (*start)(struct spi_chip *chip);
+	enum spi_result (*txrx8)(struct spi_chip *chip, uint8_t *wdat,
+		uint8_t *rdat, size_t num_pkts);
+	enum spi_result (*txrx16)(struct spi_chip *chip, uint16_t *wdat,
+		uint16_t *rdat, size_t num_pkts);
+	void (*end)(struct spi_chip *chip);
+};
+
+#endif	/* __SPI_H__ */
+
diff --git a/core/include/ta_pub_key.h b/core/include/ta_pub_key.h
new file mode 100644
index 0000000..d6ad73a
--- /dev/null
+++ b/core/include/ta_pub_key.h
@@ -0,0 +1,37 @@
+/*
+ * 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.
+ */
+#ifndef KERNEL_TA_PUB_KEY_H
+#define KERNEL_TA_PUB_KEY_H
+
+#include <types_ext.h>
+
+extern const uint32_t ta_pub_key_exponent;
+extern const uint8_t ta_pub_key_modulus[];
+extern const size_t ta_pub_key_modulus_size;
+
+#endif /*KERNEL_TA_PUB_KEY_H*/
+
diff --git a/core/include/tee/se/aid.h b/core/include/tee/se/aid.h
new file mode 100644
index 0000000..ca5fb19
--- /dev/null
+++ b/core/include/tee/se/aid.h
@@ -0,0 +1,40 @@
+/*
+ * Copyright (c) 2014, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+#ifndef TEE_SE_AID
+#define TEE_SE_AID
+
+#define MAX_AID_LENGTH	16
+#define MIN_AID_LENGTH	5
+
+struct tee_se_aid;
+
+TEE_Result tee_se_aid_create_from_buffer(uint8_t *id, size_t length,
+		struct tee_se_aid **aid);
+
+void tee_se_aid_release(struct tee_se_aid *aid);
+
+#endif
diff --git a/core/include/tee/se/apdu.h b/core/include/tee/se/apdu.h
new file mode 100644
index 0000000..d923ea5
--- /dev/null
+++ b/core/include/tee/se/apdu.h
@@ -0,0 +1,60 @@
+/*
+ * Copyright (c) 2014, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+#ifndef TEE_SE_APDU
+#define TEE_SE_APDU
+
+struct cmd_apdu;
+struct resp_apdu;
+struct apdu_base;
+
+#define to_apdu_base(apdu)	((struct apdu_base *)(apdu))
+
+struct cmd_apdu *alloc_cmd_apdu(uint8_t cla, uint8_t ins, uint8_t p1,
+		uint8_t p2, uint8_t lc, uint8_t le, uint8_t *data);
+
+struct cmd_apdu *alloc_cmd_apdu_from_buf(uint8_t *buf, size_t length);
+
+struct resp_apdu *alloc_resp_apdu(uint8_t le);
+
+uint8_t *resp_apdu_get_data(struct resp_apdu *apdu);
+
+size_t resp_apdu_get_data_len(struct resp_apdu *apdu);
+
+uint8_t resp_apdu_get_sw1(struct resp_apdu *apdu);
+
+uint8_t resp_apdu_get_sw2(struct resp_apdu *apdu);
+
+uint8_t *apdu_get_data(struct apdu_base *apdu);
+
+size_t apdu_get_length(struct apdu_base *apdu);
+
+void apdu_acquire(struct apdu_base *apdu);
+
+void apdu_release(struct apdu_base *apdu);
+
+
+#endif
diff --git a/core/include/tee/se/channel.h b/core/include/tee/se/channel.h
new file mode 100644
index 0000000..be6d986
--- /dev/null
+++ b/core/include/tee/se/channel.h
@@ -0,0 +1,59 @@
+/*
+ * Copyright (c) 2014, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef TEE_SE_CHANNEL_H
+#define TEE_SE_CHANNEL_H
+
+struct tee_se_aid;
+
+/*
+ * GP Card API define the maximum logical channel number is 20,
+ * Numbered from 0 ~ 19, number 0 is basic logical channel
+ */
+#define MAX_LOGICAL_CHANNEL	20
+
+struct tee_se_session *tee_se_channel_get_session(struct tee_se_channel *c);
+
+int tee_se_channel_get_id(struct tee_se_channel *c);
+
+TEE_Result tee_se_channel_select_next(struct tee_se_channel *c);
+
+TEE_Result tee_se_channel_select(struct tee_se_channel *c,
+		struct tee_se_aid *aid);
+
+void tee_se_channel_set_aid(struct tee_se_channel *c,
+		struct tee_se_aid *aid);
+
+void tee_se_channel_set_select_response(struct tee_se_channel *c,
+		struct resp_apdu *resp);
+
+TEE_Result tee_se_channel_get_select_response(struct tee_se_channel *c,
+		struct resp_apdu **resp);
+
+TEE_Result tee_se_channel_transmit(struct tee_se_channel *c,
+		struct cmd_apdu *cmd_apdu, struct resp_apdu *resp_apdu);
+#endif
diff --git a/core/include/tee/se/iso7816.h b/core/include/tee/se/iso7816.h
new file mode 100644
index 0000000..ebd6993
--- /dev/null
+++ b/core/include/tee/se/iso7816.h
@@ -0,0 +1,78 @@
+/*
+ * Copyright (c) 2014, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef TEE_SE_PROTOCOL_H
+#define TEE_SE_PROTOCOL_H
+
+#define ISO7816_CLA	0x0
+
+#define ISO7816_CLA_OFFSET	0x0
+
+#define SELECT_CMD			0xA4
+/* P1 parameters */
+#define	SELECT_BY_AID			0x04
+/* P2 parameters */
+#define	FIRST_OR_ONLY_OCCURRENCE	0x0
+#define	NEXT_OCCURRENCE			0x2
+
+#define MANAGE_CHANNEL_CMD		0x70
+/* P1 parameters */
+#define	OPEN_CHANNEL			0x00
+#define	CLOSE_CHANNEL			0x80
+/* P2 parameters */
+#define	OPEN_NEXT_AVAILABLE		0x00
+
+
+#define CMD_OK_SW1	0x90
+#define CMD_OK_SW2	0x00
+
+struct tee_se_reader_proxy;
+struct tee_se_session;
+struct tee_se_channel;
+struct tee_se_aid;
+struct cmd_apdu;
+struct resp_apdu;
+
+/* ISO7816 protocol handlers */
+TEE_Result iso7816_exchange_apdu(struct tee_se_reader_proxy *proxy,
+		struct cmd_apdu *cmd, struct resp_apdu *resp);
+
+TEE_Result iso7816_select(struct tee_se_channel *c, struct tee_se_aid *aid);
+
+TEE_Result iso7816_select_next(struct tee_se_channel *c);
+
+TEE_Result iso7816_open_available_logical_channel(struct tee_se_session *s,
+		int *channel_id);
+
+TEE_Result iso7816_close_logical_channel(struct tee_se_session *s,
+		int channel_id);
+
+int iso7816_get_cla_channel(int channel_id);
+
+
+#endif
+
diff --git a/core/include/tee/se/manager.h b/core/include/tee/se/manager.h
new file mode 100644
index 0000000..9c35f47
--- /dev/null
+++ b/core/include/tee/se/manager.h
@@ -0,0 +1,44 @@
+/*
+ * Copyright (c) 2014, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef TEE_SE_MANAGER_H
+#define TEE_SE_MANAGER_H
+
+#include <tee_api_types.h>
+
+struct tee_se_reader_proxy;
+
+TEE_Result tee_se_manager_get_readers(
+		struct tee_se_reader_proxy **proxy_list,
+		size_t *proxy_list_size);
+
+bool tee_se_manager_is_reader_proxy_valid(
+		struct tee_se_reader_proxy *proxy);
+
+size_t tee_se_manager_get_reader_count(void);
+
+#endif
diff --git a/core/include/tee/se/reader.h b/core/include/tee/se/reader.h
new file mode 100644
index 0000000..ac361dc
--- /dev/null
+++ b/core/include/tee/se/reader.h
@@ -0,0 +1,65 @@
+/*
+ * Copyright (c) 2014, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef TEE_SE_READER_H
+#define TEE_SE_READER_H
+
+#include <tee_api_types.h>
+#include <kernel/mutex.h>
+#include <sys/queue.h>
+
+struct tee_se_reader_proxy;
+struct tee_se_session;
+
+TEE_Result tee_se_reader_get_name(struct tee_se_reader_proxy *proxy,
+		char **reader_name, size_t *reader_name_len);
+
+void tee_se_reader_get_properties(struct tee_se_reader_proxy *proxy,
+		TEE_SEReaderProperties *prop);
+
+TEE_Result tee_se_reader_attach(struct tee_se_reader_proxy *proxy);
+
+void tee_se_reader_detach(struct tee_se_reader_proxy *proxy);
+
+TEE_Result tee_se_reader_open_session(struct tee_se_reader_proxy *proxy,
+		struct tee_se_session **session);
+
+void tee_se_reader_close_sessions(struct tee_se_reader_proxy *proxy);
+
+TEE_Result tee_se_reader_get_atr(struct tee_se_reader_proxy *proxy,
+		uint8_t **atr, size_t *atr_len);
+
+TEE_Result tee_se_reader_transmit(struct tee_se_reader_proxy *proxy,
+		uint8_t *tx_buf, size_t tx_buf_len, uint8_t *rx_buf, size_t *rx_buf_len);
+
+void tee_se_reader_lock_basic_channel(struct tee_se_reader_proxy *proxy);
+
+void tee_se_reader_unlock_basic_channel(struct tee_se_reader_proxy *proxy);
+
+bool tee_se_reader_is_basic_channel_locked(struct tee_se_reader_proxy *proxy);
+
+#endif
diff --git a/core/include/tee/se/reader/interface.h b/core/include/tee/se/reader/interface.h
new file mode 100644
index 0000000..dc1f423
--- /dev/null
+++ b/core/include/tee/se/reader/interface.h
@@ -0,0 +1,66 @@
+/*
+ * Copyright (c) 2014, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef TEE_READER_INTERFACE_H
+#define TEE_READER_INTERFACE_H
+
+#include <tee_api_types.h>
+
+#include <sys/queue.h>
+
+struct tee_se_reader {
+	char name[TEE_SE_READER_NAME_MAX];
+	struct tee_se_reader_ops *ops;
+	void *private_data;
+	TEE_SEReaderProperties prop;
+};
+
+enum tee_se_reader_state {
+	READER_STATE_SE_EJECTED,
+	READER_STATE_SE_INSERTED
+};
+
+enum tee_se_reader_type {
+	READER_TYPE_ESE,
+	READER_TYPE_SD,
+	READER_TYPE_UICC,
+};
+
+struct tee_se_reader_ops {
+	TEE_Result (*open)(struct tee_se_reader *);
+	void (*close)(struct tee_se_reader *);
+	enum tee_se_reader_state (*get_state)(struct tee_se_reader *);
+	TEE_Result (*get_atr)(struct tee_se_reader *,
+			uint8_t **atr, size_t *atr_len);
+	TEE_Result (*transmit)(struct tee_se_reader *, uint8_t *tx_buf,
+			size_t tx_len, uint8_t *rx_buf, size_t *rx_len);
+};
+
+TEE_Result tee_se_manager_register_reader(struct tee_se_reader *);
+TEE_Result tee_se_manager_unregister_reader(struct tee_se_reader *);
+
+#endif
diff --git a/core/include/tee/se/service.h b/core/include/tee/se/service.h
new file mode 100644
index 0000000..49d2084
--- /dev/null
+++ b/core/include/tee/se/service.h
@@ -0,0 +1,72 @@
+/*
+ * Copyright (c) 2014, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef TEE_SE_SERVICE_H
+#define TEE_SE_SERVICE_H
+
+#include <tee_api_types.h>
+#include <kernel/mutex.h>
+
+struct tee_se_service;
+struct tee_se_session;
+struct tee_se_channel;
+struct tee_se_reader_proxy;
+
+TEE_Result tee_se_service_open(
+		struct tee_se_service **service);
+
+TEE_Result tee_se_service_add_session(
+		struct tee_se_service *service,
+		struct tee_se_session *session);
+
+void tee_se_service_close_session(
+		struct tee_se_service *service,
+		struct tee_se_session *session);
+
+void tee_se_service_close_sessions_by_reader(
+		struct tee_se_service *service,
+		struct tee_se_reader_proxy *proxy);
+
+TEE_Result tee_se_service_is_session_closed(
+		struct tee_se_service *service,
+		struct tee_se_session *session_service);
+
+TEE_Result tee_se_service_close(
+		struct tee_se_service *service);
+
+bool tee_se_service_is_valid(
+		struct tee_se_service *service);
+
+bool tee_se_service_is_session_valid(
+		struct tee_se_service *service,
+		struct tee_se_session *session_service);
+
+bool tee_se_service_is_channel_valid(
+		struct tee_se_service *service,
+		struct tee_se_channel *channel);
+
+#endif
diff --git a/core/include/tee/se/session.h b/core/include/tee/se/session.h
new file mode 100644
index 0000000..8ebc0e8
--- /dev/null
+++ b/core/include/tee/se/session.h
@@ -0,0 +1,63 @@
+/*
+ * Copyright (c) 2014, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef TEE_SE_SESSION_H
+#define TEE_SE_SESSION_H
+
+#include <tee_api_types.h>
+#include <kernel/mutex.h>
+
+#include <sys/queue.h>
+
+struct tee_se_reader_proxy;
+struct tee_se_channel;
+struct tee_se_session;
+struct tee_se_aid;
+struct cmd_apdu;
+struct resp_apdu;
+
+TEE_Result tee_se_session_open_basic_channel(struct tee_se_session *s,
+		struct tee_se_aid *aid, struct tee_se_channel **channel);
+
+TEE_Result tee_se_session_open_logical_channel(struct tee_se_session *s,
+		struct tee_se_aid *aid, struct tee_se_channel **channel);
+
+bool tee_se_session_is_channel_exist(struct tee_se_session *s,
+		struct tee_se_channel *c);
+
+void tee_se_session_close_channel(struct tee_se_session *s,
+		struct tee_se_channel *c);
+
+TEE_Result tee_se_session_get_atr(struct tee_se_session *s,
+		uint8_t **atr, size_t *atr_len);
+
+TEE_Result tee_se_session_transmit(struct tee_se_session *s,
+		struct cmd_apdu *c, struct resp_apdu *r);
+
+void tee_se_session_close(struct tee_se_session *s);
+
+#endif
diff --git a/core/include/tee/se/svc.h b/core/include/tee/se/svc.h
new file mode 100644
index 0000000..5cb8877
--- /dev/null
+++ b/core/include/tee/se/svc.h
@@ -0,0 +1,103 @@
+/*
+ * Copyright (c) 2014, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef TEE_SVC_SE_H
+#define TEE_SVC_SE_H
+
+#include <tee_api_types.h>
+
+#include <tee/se/service.h>
+#include <tee/se/manager.h>
+#include <tee/se/reader.h>
+#include <tee/se/session.h>
+#include <tee/se/channel.h>
+#include <tee/se/aid.h>
+#include <tee/se/apdu.h>
+#include <tee/se/util.h>
+
+#if defined(CFG_SE_API)
+
+TEE_Result syscall_se_service_open(uint32_t *service_handle);
+
+TEE_Result syscall_se_service_close(unsigned long service_handle);
+
+TEE_Result syscall_se_service_get_readers(unsigned long service_handle,
+			uint32_t *reader_handles, uint64_t *len);
+
+TEE_Result syscall_se_reader_get_prop(unsigned long reader_handle, uint32_t *p);
+
+TEE_Result syscall_se_reader_get_name(unsigned long reader_handle,
+			char *name, uint64_t *name_len);
+
+TEE_Result syscall_se_reader_open_session(unsigned long reader_handle,
+			uint32_t *session_handle);
+
+TEE_Result syscall_se_reader_close_sessions(unsigned long reader_handle);
+
+TEE_Result syscall_se_session_is_closed(unsigned long session_handle);
+
+TEE_Result syscall_se_session_get_atr(unsigned long session_handle,
+			void *atr, uint64_t *atr_len);
+
+TEE_Result syscall_se_session_open_channel(unsigned long session_handle,
+			unsigned long is_logical, const void *aid_buf,
+			size_t aid_buf_len, uint32_t *channel_handle);
+
+TEE_Result syscall_se_session_close(unsigned long session_handle);
+
+TEE_Result syscall_se_channel_select_next(unsigned long channel_handle);
+
+TEE_Result syscall_se_channel_get_select_resp(unsigned long channel_handle,
+			void *resp, uint64_t *resp_len);
+
+TEE_Result syscall_se_channel_transmit(unsigned long channel_handle,
+			void *cmd, unsigned long cmd_len, void *resp,
+			uint64_t *resp_len);
+
+TEE_Result syscall_se_channel_close(unsigned long channel_handle);
+
+#else
+
+#define  syscall_se_service_open		syscall_not_supported
+#define  syscall_se_service_close		syscall_not_supported
+#define  syscall_se_service_get_readers		syscall_not_supported
+#define  syscall_se_reader_get_prop		syscall_not_supported
+#define  syscall_se_reader_get_name		syscall_not_supported
+#define  syscall_se_reader_open_session		syscall_not_supported
+#define  syscall_se_reader_close_sessions	syscall_not_supported
+#define  syscall_se_session_is_closed		syscall_not_supported
+#define  syscall_se_session_get_atr		syscall_not_supported
+#define  syscall_se_session_open_channel	syscall_not_supported
+#define  syscall_se_session_close		syscall_not_supported
+#define  syscall_se_channel_select_next		syscall_not_supported
+#define  syscall_se_channel_get_select_resp	syscall_not_supported
+#define  syscall_se_channel_transmit		syscall_not_supported
+#define  syscall_se_channel_close		syscall_not_supported
+
+#endif /* if defined(CFG_SE_API) */
+
+#endif
diff --git a/core/include/tee/se/util.h b/core/include/tee/se/util.h
new file mode 100644
index 0000000..334a8df
--- /dev/null
+++ b/core/include/tee/se/util.h
@@ -0,0 +1,44 @@
+/*
+ * Copyright (c) 2014, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef TEE_SE_UTIL_H
+#define TEE_SE_UTIL_H
+
+#include <tee_api_types.h>
+
+#define DUMP_BUF_MAX	128
+char *print_buf(char *buf, size_t *remain_size, const char *fmt, ...)
+	__attribute__((__format__(__printf__, 3, 4)));
+
+void dump_hex(char *buf, size_t *remain_size, uint8_t *input_buf,
+		size_t input_size);
+
+void print_hex(uint8_t *input_buf, size_t input_size);
+
+uint8_t *hex_decode(const char *in, size_t len, uint8_t *out);
+
+#endif
diff --git a/core/include/tee/tee_authenc.h b/core/include/tee/tee_authenc.h
new file mode 100644
index 0000000..a6e4759
--- /dev/null
+++ b/core/include/tee/tee_authenc.h
@@ -0,0 +1,63 @@
+/*
+ * 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.
+ */
+#ifndef TEE_AUTHENC_H
+#define TEE_AUTHENC_H
+
+#include <tee_api_types.h>
+
+/*
+ * Algorithms in this files are as specified with the TEE_ALG_XXX from
+ * TEE Internal API.
+ */
+
+TEE_Result tee_authenc_get_ctx_size(uint32_t algo, size_t *size);
+
+TEE_Result tee_authenc_init(
+	void *ctx, uint32_t algo, TEE_OperationMode mode, const uint8_t *key,
+	size_t key_len, const uint8_t *nonce,
+	size_t nonce_len, size_t tag_len, size_t aad_len, size_t payload_len);
+
+TEE_Result tee_authenc_update_aad(
+	void *ctx, uint32_t algo, TEE_OperationMode mode,
+	  const uint8_t *data, size_t len);
+
+TEE_Result tee_authenc_update_payload(
+	void *ctx, uint32_t algo, TEE_OperationMode mode,
+	const uint8_t *src_data, size_t src_len, uint8_t *dst_data);
+
+TEE_Result tee_authenc_enc_final(
+	void *ctx, uint32_t algo, const uint8_t *src_data,
+	 size_t src_len, uint8_t *dst_data,
+	 uint8_t *dst_tag, size_t *dst_tag_len);
+
+TEE_Result tee_authenc_dec_final(
+	void *ctx, uint32_t algo, const uint8_t *src_data,
+	size_t src_len, uint8_t *dst_data, const uint8_t *tag, size_t tag_len);
+
+void tee_authenc_final(void *ctx, uint32_t algo);
+
+#endif /* TEE_AUTHENC_H */
diff --git a/core/include/tee/tee_cryp_concat_kdf.h b/core/include/tee/tee_cryp_concat_kdf.h
new file mode 100644
index 0000000..52d640b
--- /dev/null
+++ b/core/include/tee/tee_cryp_concat_kdf.h
@@ -0,0 +1,39 @@
+/*
+ * Copyright (c) 2014, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef TEE_CRYP_CONCAT_KDF_H
+#define TEE_CRYP_CONCAT_KDF_H
+
+#include <tee_api_types.h>
+
+TEE_Result tee_cryp_concat_kdf(uint32_t hash_id, const uint8_t *shared_secret,
+			       size_t shared_secret_len,
+			       const uint8_t *other_info,
+			       size_t other_info_len, uint8_t *derived_key,
+			       size_t derived_key_len);
+
+#endif /* TEE_CRYP_CONCAT_KDF_H */
diff --git a/core/include/tee/tee_cryp_hkdf.h b/core/include/tee/tee_cryp_hkdf.h
new file mode 100644
index 0000000..30ec8ad
--- /dev/null
+++ b/core/include/tee/tee_cryp_hkdf.h
@@ -0,0 +1,38 @@
+/*
+ * Copyright (c) 2014, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef TEE_CRYP_HKDF_H
+#define TEE_CRYP_HKDF_H
+
+#include <tee_api_types.h>
+
+TEE_Result tee_cryp_hkdf(uint32_t hash_id, const uint8_t *ikm, size_t ikm_len,
+			 const uint8_t *salt, size_t salt_len,
+			 const uint8_t *info, size_t info_len, uint8_t *okm,
+			 size_t okm_len);
+
+#endif /* TEE_CRYP_HKDF_H */
diff --git a/core/include/tee/tee_cryp_pbkdf2.h b/core/include/tee/tee_cryp_pbkdf2.h
new file mode 100644
index 0000000..0e2d6b9
--- /dev/null
+++ b/core/include/tee/tee_cryp_pbkdf2.h
@@ -0,0 +1,38 @@
+/*
+ * Copyright (c) 2014, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef TEE_CRYP_PBKDF2_H
+#define TEE_CRYP_PBKDF2_H
+
+#include <tee_api_types.h>
+
+TEE_Result tee_cryp_pbkdf2(uint32_t hash_id, const uint8_t *password,
+			   size_t password_len, const uint8_t *salt,
+			   size_t salt_len, uint32_t iteration_count,
+			   uint8_t *derived_key, size_t derived_key_len);
+
+#endif /* TEE_CRYP_PBKDF2_H */
diff --git a/core/include/tee/tee_cryp_provider.h b/core/include/tee/tee_cryp_provider.h
new file mode 100644
index 0000000..3927b65
--- /dev/null
+++ b/core/include/tee/tee_cryp_provider.h
@@ -0,0 +1,319 @@
+/*
+ * Copyright (c) 2014, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/*
+ * This is the Cryptographic Provider API (CP API).
+ *
+ * This defines how most crypto syscalls that implement the Cryptographic
+ * Operations API can invoke the actual providers of cryptographic algorithms
+ * (such as LibTomCrypt).
+ *
+ * To add a new provider, you need to provide an implementation of this
+ * interface.
+ *
+ * The following parameters are commonly used.
+ *
+ * @ctx: context allocated by the syscall, for later use by the algorithm
+ * @algo: algorithm identifier (TEE_ALG_*)
+ */
+
+
+#ifndef TEE_CRYP_PROVIDER_H
+#define TEE_CRYP_PROVIDER_H
+
+#include <tee_api_types.h>
+
+/* Message digest functions */
+struct hash_ops {
+	TEE_Result (*get_ctx_size)(uint32_t algo, size_t *size);
+	TEE_Result (*init)(void *ctx, uint32_t algo);
+	TEE_Result (*update)(void *ctx, uint32_t algo,
+			     const uint8_t *data, size_t len);
+	TEE_Result (*final)(void *ctx, uint32_t algo, uint8_t *digest,
+			    size_t len);
+};
+
+/* Symmetric ciphers */
+struct cipher_ops {
+	TEE_Result (*get_ctx_size)(uint32_t algo, size_t *size);
+	TEE_Result (*init)(void *ctx, uint32_t algo,
+			    TEE_OperationMode mode,
+			    const uint8_t *key1, size_t key1_len,
+			    const uint8_t *key2, size_t key2_len,
+			    const uint8_t *iv, size_t iv_len);
+	TEE_Result (*update)(void *ctx, uint32_t algo,
+			     TEE_OperationMode mode,
+			     bool last_block, const uint8_t *data,
+			     size_t len, uint8_t *dst);
+	void       (*final)(void *ctx, uint32_t algo);
+	TEE_Result (*get_block_size)(uint32_t algo, size_t *size);
+};
+
+/* Message Authentication Code functions */
+struct mac_ops {
+	TEE_Result (*get_ctx_size)(uint32_t algo, size_t *size);
+	TEE_Result (*init)(void *ctx, uint32_t algo,
+			   const uint8_t *key, size_t len);
+	TEE_Result (*update)(void *ctx, uint32_t algo,
+			     const uint8_t *data, size_t len);
+	TEE_Result (*final)(void *ctx, uint32_t algo,
+			    uint8_t *digest, size_t digest_len);
+};
+
+/* Authenticated encryption */
+struct authenc_ops {
+	TEE_Result (*get_ctx_size)(uint32_t algo, size_t *size);
+	TEE_Result (*init)(void *ctx, uint32_t algo,
+			   TEE_OperationMode mode,
+			   const uint8_t *key, size_t key_len,
+			   const uint8_t *nonce, size_t nonce_len,
+			   size_t tag_len, size_t aad_len,
+			   size_t payload_len);
+	TEE_Result (*update_aad)(void *ctx, uint32_t algo,
+				 TEE_OperationMode mode,
+				 const uint8_t *data, size_t len);
+	TEE_Result (*update_payload)(void *ctx, uint32_t algo,
+				     TEE_OperationMode mode,
+				     const uint8_t *src_data,
+				     size_t src_len,
+				     uint8_t *dst_data,
+				     size_t *dst_len);
+	TEE_Result (*enc_final)(void *ctx, uint32_t algo,
+				const uint8_t *src_data,
+				size_t src_len, uint8_t *dst_data,
+				size_t *dst_len, uint8_t *dst_tag,
+				size_t *dst_tag_len);
+	TEE_Result (*dec_final)(void *ctx, uint32_t algo,
+				const uint8_t *src_data,
+				size_t src_len, uint8_t *dst_data,
+				size_t *dst_len, const uint8_t *tag,
+				size_t tag_len);
+
+	void       (*final)(void *ctx, uint32_t algo);
+};
+
+/* Implementation-defined big numbers */
+struct bignum_ops {
+	/*
+	 * Allocate a bignum capable of holding an unsigned integer value of
+	 * up to bitsize bits
+	 */
+	struct bignum *(*allocate)(size_t size_bits);
+	TEE_Result (*bin2bn)(const uint8_t *from, size_t fromsize,
+			     struct bignum *to);
+	size_t (*num_bytes)(struct bignum *a);
+	size_t (*num_bits)(struct bignum *a);
+	void (*bn2bin)(const struct bignum *from, uint8_t *to);
+	void (*copy)(struct bignum *to, const struct bignum *from);
+	void (*free)(struct bignum *a);
+	void (*clear)(struct bignum *a);
+
+	/* return -1 if a<b, 0 if a==b, +1 if a>b */
+	int32_t (*compare)(struct bignum *a, struct bignum *b);
+};
+
+/* Asymmetric algorithms */
+
+struct rsa_keypair {
+	struct bignum *e;	/* Public exponent */
+	struct bignum *d;	/* Private exponent */
+	struct bignum *n;	/* Modulus */
+
+	/* Optional CRT parameters (all NULL if unused) */
+	struct bignum *p;	/* N = pq */
+	struct bignum *q;
+	struct bignum *qp;	/* 1/q mod p */
+	struct bignum *dp;	/* d mod (p-1) */
+	struct bignum *dq;	/* d mod (q-1) */
+};
+
+struct rsa_public_key {
+	struct bignum *e;	/* Public exponent */
+	struct bignum *n;	/* Modulus */
+};
+
+struct dsa_keypair {
+	struct bignum *g;	/* Generator of subgroup (public) */
+	struct bignum *p;	/* Prime number (public) */
+	struct bignum *q;	/* Order of subgroup (public) */
+	struct bignum *y;	/* Public key */
+	struct bignum *x;	/* Private key */
+};
+
+struct dsa_public_key {
+	struct bignum *g;	/* Generator of subgroup (public) */
+	struct bignum *p;	/* Prime number (public) */
+	struct bignum *q;	/* Order of subgroup (public) */
+	struct bignum *y;	/* Public key */
+};
+
+struct dh_keypair {
+	struct bignum *g;	/* Generator of Z_p (shared) */
+	struct bignum *p;	/* Prime modulus (shared) */
+	struct bignum *x;	/* Private key */
+	struct bignum *y;	/* Public key y = g^x */
+
+	/*
+	 * Optional parameters used by key generation.
+	 * When not used, q == NULL and xbits == 0
+	 */
+	struct bignum *q;	/* x must be in the range [2, q-2] */
+	uint32_t xbits;		/* Number of bits in the private key */
+};
+
+struct ecc_public_key {
+	struct bignum *x;	/* Public value x */
+	struct bignum *y;	/* Public value y */
+	uint32_t curve;	        /* Curve type */
+};
+
+struct ecc_keypair {
+	struct bignum *d;	/* Private value */
+	struct bignum *x;	/* Public value x */
+	struct bignum *y;	/* Public value y */
+	uint32_t curve;	        /* Curve type */
+};
+
+struct acipher_ops {
+
+	/*
+	 * Key allocation functions
+	 * Allocate the bignum's inside a key structure.
+	 * TEE core will later use bignum.free().
+	 */
+	TEE_Result (*alloc_rsa_keypair)(struct rsa_keypair *s,
+					size_t key_size_bits);
+	TEE_Result (*alloc_rsa_public_key)(struct rsa_public_key *s,
+					   size_t key_size_bits);
+	void (*free_rsa_public_key)(struct rsa_public_key *s);
+	TEE_Result (*alloc_dsa_keypair)(struct dsa_keypair *s,
+					size_t key_size_bits);
+	TEE_Result (*alloc_dsa_public_key)(struct dsa_public_key *s,
+					   size_t key_size_bits);
+	TEE_Result (*alloc_dh_keypair)(struct dh_keypair *s,
+				       size_t key_size_bits);
+	TEE_Result (*alloc_ecc_public_key)(struct ecc_public_key *s,
+					   size_t key_size_bits);
+	TEE_Result (*alloc_ecc_keypair)(struct ecc_keypair *s,
+					size_t key_size_bits);
+	void (*free_ecc_public_key)(struct ecc_public_key *s);
+
+	/*
+	 * Key generation functions
+	 */
+	TEE_Result (*gen_rsa_key)(struct rsa_keypair *key, size_t key_size);
+	TEE_Result (*gen_dsa_key)(struct dsa_keypair *key, size_t key_size);
+	TEE_Result (*gen_dh_key)(struct dh_keypair *key, struct bignum *q,
+				 size_t xbits);
+	TEE_Result (*gen_ecc_key)(struct ecc_keypair *key);
+
+	TEE_Result (*dh_shared_secret)(struct dh_keypair *private_key,
+				       struct bignum *public_key,
+				       struct bignum *secret);
+
+	TEE_Result (*rsanopad_decrypt)(struct rsa_keypair *key,
+				       const uint8_t *src, size_t src_len,
+				       uint8_t *dst, size_t *dst_len);
+	TEE_Result (*rsanopad_encrypt)(struct rsa_public_key *key,
+				       const uint8_t *src, size_t src_len,
+				       uint8_t *dst, size_t *dst_len);
+	TEE_Result (*rsaes_decrypt)(uint32_t algo, struct rsa_keypair *key,
+				    const uint8_t *label, size_t label_len,
+				    const uint8_t *src, size_t src_len,
+				    uint8_t *dst, size_t *dst_len);
+	TEE_Result (*rsaes_encrypt)(uint32_t algo,
+				    struct rsa_public_key *key,
+				    const uint8_t *label, size_t label_len,
+				    const uint8_t *src, size_t src_len,
+				    uint8_t *dst, size_t *dst_len);
+	/* RSA SSA sign/verify: if salt_len == -1, use default value */
+	TEE_Result (*rsassa_sign)(uint32_t algo, struct rsa_keypair *key,
+				  int salt_len, const uint8_t *msg,
+				  size_t msg_len, uint8_t *sig,
+				  size_t *sig_len);
+	TEE_Result (*rsassa_verify)(uint32_t algo,
+				    struct rsa_public_key *key,
+				    int salt_len, const uint8_t *msg,
+				    size_t msg_len, const uint8_t *sig,
+				    size_t sig_len);
+	TEE_Result (*dsa_sign)(uint32_t algo, struct dsa_keypair *key,
+			       const uint8_t *msg, size_t msg_len,
+			       uint8_t *sig, size_t *sig_len);
+	TEE_Result (*dsa_verify)(uint32_t algo, struct dsa_public_key *key,
+				 const uint8_t *msg, size_t msg_len,
+				 const uint8_t *sig, size_t sig_len);
+	TEE_Result (*ecc_sign)(uint32_t algo, struct ecc_keypair *key,
+			       const uint8_t *msg, size_t msg_len,
+			       uint8_t *sig, size_t *sig_len);
+	TEE_Result (*ecc_verify)(uint32_t algo, struct ecc_public_key *key,
+				 const uint8_t *msg, size_t msg_len,
+				 const uint8_t *sig, size_t sig_len);
+	TEE_Result (*ecc_shared_secret)(struct ecc_keypair *private_key,
+					struct ecc_public_key *public_key,
+					void *secret,
+					unsigned long *secret_len);
+
+};
+
+struct prng_ops {
+	/* add entropy to PRNG entropy pool */
+	TEE_Result (*add_entropy)(const uint8_t *inbuf, size_t len);
+
+	/* to read random data from PRNG implementation	 */
+	TEE_Result (*read)(void *buf, size_t blen);
+};
+
+/* Cryptographic Provider API */
+struct crypto_ops {
+	/* Human-readable provider name */
+	const char *name;
+
+	TEE_Result (*init)(void);
+	struct hash_ops hash;
+	struct cipher_ops cipher;
+	struct mac_ops mac;
+	struct authenc_ops authenc;
+	struct acipher_ops acipher;
+	struct bignum_ops bignum;
+	struct prng_ops prng;
+};
+
+extern const struct crypto_ops crypto_ops;
+
+/*
+ * Verifies a SHA-256 hash, doesn't require tee_cryp_init() to be called in
+ * advance and has as few dependencies as possible.
+ *
+ * This function is primarily used by pager and early initialization code
+ * where the complete crypto library isn't available.
+ */
+TEE_Result hash_sha256_check(const uint8_t *hash, const uint8_t *data,
+		size_t data_size);
+
+TEE_Result rng_generate(void *buffer, size_t len);
+
+#endif /* TEE_CRYP_PROVIDER_H */
diff --git a/core/include/tee/tee_cryp_utl.h b/core/include/tee/tee_cryp_utl.h
new file mode 100644
index 0000000..99304f5
--- /dev/null
+++ b/core/include/tee/tee_cryp_utl.h
@@ -0,0 +1,54 @@
+/*
+ * Copyright (c) 2014, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef TEE_CRYP_UTL_H
+#define TEE_CRYP_UTL_H
+
+#include <tee_api_types.h>
+
+#if !defined(CFG_WITH_SOFTWARE_PRNG)
+TEE_Result get_rng_array(void *buffer, int len);
+#endif
+
+TEE_Result tee_hash_get_digest_size(uint32_t algo, size_t *size);
+TEE_Result tee_hash_createdigest(uint32_t algo, const uint8_t *data,
+				 size_t datalen, uint8_t *digest,
+				 size_t digestlen);
+TEE_Result tee_mac_get_digest_size(uint32_t algo, size_t *size);
+TEE_Result tee_cipher_get_block_size(uint32_t algo, size_t *size);
+TEE_Result tee_do_cipher_update(void *ctx, uint32_t algo,
+				TEE_OperationMode mode, bool last_block,
+				const uint8_t *data, size_t len, uint8_t *dst);
+TEE_Result tee_aes_cbc_cts_update(void *cbc_ctx, void *ecb_ctx,
+				  TEE_OperationMode mode, bool last_block,
+				  const uint8_t *data, size_t len,
+				  uint8_t *dst);
+
+TEE_Result tee_prng_add_entropy(const uint8_t *in, size_t len);
+void plat_prng_add_jitter_entropy(void);
+
+#endif
diff --git a/core/include/tee/tee_fs.h b/core/include/tee/tee_fs.h
new file mode 100644
index 0000000..299ef74
--- /dev/null
+++ b/core/include/tee/tee_fs.h
@@ -0,0 +1,79 @@
+/*
+ * 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.
+ */
+
+#ifndef TEE_FS_H
+#define TEE_FS_H
+
+#include <stddef.h>
+#include <stdint.h>
+#include <tee_api_types.h>
+
+#define TEE_FS_NAME_MAX 350
+
+typedef int64_t tee_fs_off_t;
+typedef uint32_t tee_fs_mode_t;
+
+struct tee_fs_dirent {
+	char *d_name;
+};
+
+struct tee_fs_dir;
+struct tee_file_handle;
+
+/*
+ * 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);
+	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,
+			     bool overwrite);
+	TEE_Result (*remove)(const char *name);
+	TEE_Result (*truncate)(struct tee_file_handle *fh, size_t size);
+
+	TEE_Result (*opendir)(const char *name, 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);
+};
+
+#ifdef CFG_REE_FS
+extern const struct tee_file_operations ree_fs_ops;
+#endif
+#ifdef CFG_RPMB_FS
+extern const struct tee_file_operations rpmb_fs_ops;
+#endif
+#ifdef CFG_SQL_FS
+extern const struct tee_file_operations sql_fs_ops;
+#endif
+
+#endif
diff --git a/core/include/tee/tee_fs_defs.h b/core/include/tee/tee_fs_defs.h
new file mode 100644
index 0000000..9c11de9
--- /dev/null
+++ b/core/include/tee/tee_fs_defs.h
@@ -0,0 +1,64 @@
+/*
+ * 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.
+ */
+
+#ifndef TEE_FS_DEFS_H
+#define TEE_FS_DEFS_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
+
+/*
+ * tee_fs_lseek
+ */
+#define TEE_FS_SEEK_SET 0x1
+#define TEE_FS_SEEK_END 0x2
+#define TEE_FS_SEEK_CUR 0x4
+
+/*
+ * file modes
+ */
+#define TEE_FS_S_IWUSR 0x1
+#define TEE_FS_S_IRUSR 0x2
+#define TEE_FS_S_IXUSR 0x4
+
+/*
+ * 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
diff --git a/core/include/tee/tee_fs_key_manager.h b/core/include/tee/tee_fs_key_manager.h
new file mode 100644
index 0000000..7f26d26
--- /dev/null
+++ b/core/include/tee/tee_fs_key_manager.h
@@ -0,0 +1,94 @@
+/*
+ * 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.
+ */
+
+#ifndef TEE_FS_KEY_MANAGER_H
+#define TEE_FS_KEY_MANAGER_H
+
+#include <tee_api_types.h>
+#include <utee_defines.h>
+
+#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);
+#endif
diff --git a/core/include/tee/tee_fs_rpc.h b/core/include/tee/tee_fs_rpc.h
new file mode 100644
index 0000000..4d73c4b
--- /dev/null
+++ b/core/include/tee/tee_fs_rpc.h
@@ -0,0 +1,96 @@
+/*
+ * Copyright (c) 2016, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/*
+ * Interface with tee-supplicant for file operations
+ */
+
+#ifndef TEE_FS_RPC_H
+#define TEE_FS_RPC_H
+
+#include <stdbool.h>
+#include <stddef.h>
+#include <tee_api_types.h>
+#include <tee/tee_fs.h>
+#include <kernel/thread.h>
+
+struct tee_fs_rpc_operation {
+	uint32_t id;
+	struct optee_msg_param params[THREAD_RPC_MAX_NUM_PARAMS];
+	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_close(uint32_t id, int fd);
+
+TEE_Result tee_fs_rpc_read_init(struct tee_fs_rpc_operation *op,
+				uint32_t id, int fd, tee_fs_off_t offset,
+				size_t data_len, void **out_data);
+TEE_Result tee_fs_rpc_read_final(struct tee_fs_rpc_operation *op,
+				 size_t *data_len);
+
+TEE_Result tee_fs_rpc_write_init(struct tee_fs_rpc_operation *op,
+				 uint32_t id, int fd, tee_fs_off_t offset,
+				 size_t data_len, void **data);
+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_opendir(uint32_t id, const char *name,
+				  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,
+			      struct tee_fs_dirent **ent);
+
+TEE_Result tee_fs_rpc_begin_transaction(uint32_t id);
+TEE_Result tee_fs_rpc_end_transaction(uint32_t id, bool rollback);
+
+struct thread_specific_data;
+#if defined(CFG_WITH_USER_TA) && \
+	(defined(CFG_REE_FS) || defined(CFG_SQL_FS) || defined(CFG_RPMB_FS))
+/* Frees the cache of allocated FS RPC memory */
+void tee_fs_rpc_cache_clear(struct thread_specific_data *tsd);
+#else
+static inline void tee_fs_rpc_cache_clear(
+			struct thread_specific_data *tsd __unused)
+{
+}
+#endif
+
+/*
+ * Returns a pointer to the cached FS RPC memory. Each thread has a unique
+ * cache. The pointer is guaranteed to point to a large enough area or to
+ * be NULL.
+ */
+void *tee_fs_rpc_cache_alloc(size_t size, paddr_t *pa, uint64_t *cookie);
+
+#endif /* TEE_FS_RPC_H */
diff --git a/core/include/tee/tee_obj.h b/core/include/tee/tee_obj.h
new file mode 100644
index 0000000..bb82c9c
--- /dev/null
+++ b/core/include/tee/tee_obj.h
@@ -0,0 +1,63 @@
+/*
+ * 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.
+ */
+
+#ifndef TEE_OBJ_H
+#define TEE_OBJ_H
+
+#include <tee_api_types.h>
+#include <kernel/tee_ta_manager.h>
+#include <sys/queue.h>
+
+#define TEE_USAGE_DEFAULT   0xffffffff
+
+struct tee_obj {
+	TAILQ_ENTRY(tee_obj) link;
+	TEE_ObjectInfo info;
+	bool busy;		/* true if used by an operation */
+	uint32_t have_attrs;	/* bitfield identifying set properties */
+	void *attr;
+	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 */
+};
+
+void tee_obj_add(struct user_ta_ctx *utc, struct tee_obj *o);
+
+TEE_Result tee_obj_get(struct user_ta_ctx *utc, uint32_t obj_id,
+		       struct tee_obj **obj);
+
+void tee_obj_close(struct user_ta_ctx *utc, struct tee_obj *o);
+
+void tee_obj_close_all(struct user_ta_ctx *utc);
+
+TEE_Result tee_obj_verify(struct tee_ta_session *sess, struct tee_obj *o);
+
+struct tee_obj *tee_obj_alloc(void);
+void tee_obj_free(struct tee_obj *o);
+
+#endif
diff --git a/core/include/tee/tee_pobj.h b/core/include/tee/tee_pobj.h
new file mode 100644
index 0000000..db7c9a6
--- /dev/null
+++ b/core/include/tee/tee_pobj.h
@@ -0,0 +1,56 @@
+/*
+ * 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.
+ */
+
+#ifndef TEE_POBJ_H
+#define TEE_POBJ_H
+
+#include <stdint.h>
+#include <sys/queue.h>
+#include <tee_api_types.h>
+#include <tee/tee_fs.h>
+
+struct tee_pobj {
+	TAILQ_ENTRY(tee_pobj) link;
+	uint32_t refcnt;
+	TEE_UUID uuid;
+	void *obj_id;
+	uint32_t obj_id_len;
+	uint32_t flags;
+	/* 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,
+			struct tee_pobj **obj);
+
+TEE_Result tee_pobj_release(struct tee_pobj *obj);
+
+TEE_Result tee_pobj_rename(struct tee_pobj *obj, void *obj_id,
+			   uint32_t obj_id_len);
+
+#endif
diff --git a/core/include/tee/tee_svc.h b/core/include/tee/tee_svc.h
new file mode 100644
index 0000000..190c50f
--- /dev/null
+++ b/core/include/tee/tee_svc.h
@@ -0,0 +1,122 @@
+/*
+ * 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.
+ */
+#ifndef TEE_SVC_H
+#define TEE_SVC_H
+
+#include <assert.h>
+#include <stdint.h>
+#include <types_ext.h>
+#include <tee_api_types.h>
+#include <utee_types.h>
+
+extern vaddr_t tee_svc_uref_base;
+
+struct tee_ta_session;
+
+/* TA Properties */
+struct tee_props {
+	const char *name;
+
+	/* prop_type is of type enum user_ta_prop_type*/
+	const uint32_t prop_type;
+
+	/* either get_prop_func or both data and len */
+	TEE_Result (*get_prop_func)(struct tee_ta_session *sess,
+				    void *buf, size_t *blen);
+	const void *data;
+	const size_t len;
+};
+
+struct tee_vendor_props {
+	const struct tee_props *props;
+	size_t len;
+};
+void syscall_sys_return(unsigned long ret);
+
+void syscall_log(const void *buf, size_t len);
+
+void syscall_panic(unsigned long code);
+
+TEE_Result syscall_not_supported(void);
+
+/* prop_set defined by enum utee_property */
+TEE_Result syscall_get_property(unsigned long prop_set,
+				unsigned long index,
+				void *name, uint32_t *name_len,
+				void *buf, uint32_t *blen,
+				uint32_t *prop_type);
+TEE_Result syscall_get_property_name_to_index(unsigned long prop_set,
+					      void *name,
+					      unsigned long name_len,
+					      uint32_t *index);
+
+TEE_Result syscall_open_ta_session(const TEE_UUID *dest,
+			unsigned long cancel_req_to, struct utee_params *params,
+			uint32_t *sess, uint32_t *ret_orig);
+
+TEE_Result syscall_close_ta_session(unsigned long sess);
+
+TEE_Result syscall_invoke_ta_command(unsigned long sess,
+			unsigned long cancel_req_to, unsigned long cmd_id,
+			struct utee_params *params, uint32_t *ret_orig);
+
+TEE_Result syscall_check_access_rights(unsigned long flags, const void *buf,
+				       size_t len);
+
+TEE_Result tee_svc_copy_from_user(void *kaddr, const void *uaddr, size_t len);
+TEE_Result tee_svc_copy_to_user(void *uaddr, const void *kaddr, size_t len);
+
+TEE_Result tee_svc_copy_kaddr_to_uref(uint32_t *uref, void *kaddr);
+
+static inline uint32_t tee_svc_kaddr_to_uref(void *kaddr)
+{
+	assert(((vaddr_t)kaddr - tee_svc_uref_base) < UINT32_MAX);
+	return (vaddr_t)kaddr - tee_svc_uref_base;
+}
+
+static inline vaddr_t tee_svc_uref_to_vaddr(uint32_t uref)
+{
+	return tee_svc_uref_base + uref;
+}
+
+static inline void *tee_svc_uref_to_kaddr(uint32_t uref)
+{
+	return (void *)tee_svc_uref_to_vaddr(uref);
+}
+
+TEE_Result syscall_get_cancellation_flag(uint32_t *cancel);
+
+TEE_Result syscall_unmask_cancellation(uint32_t *old_mask);
+
+TEE_Result syscall_mask_cancellation(uint32_t *old_mask);
+
+TEE_Result syscall_wait(unsigned long timeout);
+
+TEE_Result syscall_get_time(unsigned long cat, TEE_Time *time);
+TEE_Result syscall_set_ta_time(const TEE_Time *time);
+
+#endif /* TEE_SVC_H */
diff --git a/core/include/tee/tee_svc_cryp.h b/core/include/tee/tee_svc_cryp.h
new file mode 100644
index 0000000..feb78f2
--- /dev/null
+++ b/core/include/tee/tee_svc_cryp.h
@@ -0,0 +1,117 @@
+/*
+* 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.
+*/
+#ifndef TEE_SVC_CRYP_H
+#define TEE_SVC_CRYP_H
+
+#include <tee_api_types.h>
+#include <utee_types.h>
+#include <tee/tee_obj.h>
+
+struct user_ta_ctx;
+
+TEE_Result syscall_cryp_obj_get_info(unsigned long obj, TEE_ObjectInfo *info);
+TEE_Result syscall_cryp_obj_restrict_usage(unsigned long obj,
+			unsigned long usage);
+TEE_Result syscall_cryp_obj_get_attr(unsigned long obj, unsigned long attr_id,
+			void *buffer, uint64_t *size);
+
+TEE_Result syscall_cryp_obj_alloc(unsigned long obj_type,
+			unsigned long max_key_size, uint32_t *obj);
+TEE_Result syscall_cryp_obj_close(unsigned long obj);
+TEE_Result syscall_cryp_obj_reset(unsigned long obj);
+TEE_Result syscall_cryp_obj_populate(unsigned long obj,
+			struct utee_attribute *attrs, unsigned long attr_count);
+TEE_Result syscall_cryp_obj_copy(unsigned long dst_obj,
+			unsigned long src_obj);
+TEE_Result syscall_obj_generate_key(unsigned long obj, unsigned long key_size,
+			const struct utee_attribute *params,
+			unsigned long param_count);
+
+TEE_Result syscall_cryp_state_alloc(unsigned long algo, unsigned long op_mode,
+			unsigned long key1, unsigned long key2,
+			uint32_t *state);
+TEE_Result syscall_cryp_state_copy(unsigned long dst, unsigned long src);
+TEE_Result syscall_cryp_state_free(unsigned long state);
+void tee_svc_cryp_free_states(struct user_ta_ctx *utc);
+
+/* iv and iv_len are ignored for hash algorithms */
+TEE_Result syscall_hash_init(unsigned long state, const void *iv,
+			size_t iv_len);
+TEE_Result syscall_hash_update(unsigned long state, const void *chunk,
+			size_t chunk_size);
+TEE_Result syscall_hash_final(unsigned long state, const void *chunk,
+			size_t chunk_size, void *hash, uint64_t *hash_len);
+
+TEE_Result syscall_cipher_init(unsigned long state, const void *iv,
+			size_t iv_len);
+TEE_Result syscall_cipher_update(unsigned long state, const void *src,
+			size_t src_len, void *dest, uint64_t *dest_len);
+TEE_Result syscall_cipher_final(unsigned long state, const void *src,
+			size_t src_len, void *dest, uint64_t *dest_len);
+
+TEE_Result syscall_cryp_derive_key(unsigned long state,
+			const struct utee_attribute *params,
+			unsigned long param_count, unsigned long derived_key);
+
+TEE_Result syscall_cryp_random_number_generate(void *buf, size_t blen);
+
+TEE_Result syscall_authenc_init(unsigned long state, const void *nonce,
+			size_t nonce_len, size_t tag_len,
+			size_t aad_len, size_t payload_len);
+TEE_Result syscall_authenc_update_aad(unsigned long state,
+			const void *aad_data, size_t aad_data_len);
+TEE_Result syscall_authenc_update_payload(unsigned long state,
+			const void *src_data, size_t src_len, void *dest_data,
+			uint64_t *dest_len);
+TEE_Result syscall_authenc_enc_final(unsigned long state,
+			const void *src_data, size_t src_len, void *dest_data,
+			uint64_t *dest_len, void *tag, uint64_t *tag_len);
+TEE_Result syscall_authenc_dec_final(unsigned long state,
+			const void *src_data, size_t src_len, void *dest_data,
+			uint64_t *dest_len, const void *tag, size_t tag_len);
+
+TEE_Result syscall_asymm_operate(unsigned long state,
+			const struct utee_attribute *usr_params,
+			size_t num_params, const void *src_data,
+			size_t src_len, void *dest_data, uint64_t *dest_len);
+TEE_Result syscall_asymm_verify(unsigned long state,
+			const struct utee_attribute *usr_params,
+			size_t num_params, const void *data, size_t data_len,
+			const void *sig, size_t sig_len);
+
+TEE_Result tee_obj_set_type(struct tee_obj *o, uint32_t obj_type,
+			    size_t max_key_size);
+
+void tee_obj_attr_free(struct tee_obj *o);
+void tee_obj_attr_clear(struct tee_obj *o);
+TEE_Result tee_obj_attr_to_binary(struct tee_obj *o, void *data,
+				  size_t *data_len);
+TEE_Result tee_obj_attr_from_binary(struct tee_obj *o, const void *data,
+				    size_t data_len);
+TEE_Result tee_obj_attr_copy_from(struct tee_obj *o, const struct tee_obj *src);
+
+#endif /* TEE_SVC_CRYP_H */
diff --git a/core/include/tee/tee_svc_storage.h b/core/include/tee/tee_svc_storage.h
new file mode 100644
index 0000000..7e4c10e
--- /dev/null
+++ b/core/include/tee/tee_svc_storage.h
@@ -0,0 +1,92 @@
+/*
+ * 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.
+ */
+
+#ifndef TEE_SVC_STORAGE_H
+#define TEE_SVC_STORAGE_H
+
+#include <tee_api_types.h>
+#include <kernel/tee_ta_manager.h>
+#include <tee/tee_fs.h>
+
+/*
+ * Persistant Object Functions
+ */
+TEE_Result syscall_storage_obj_open(unsigned long storage_id, void *object_id,
+			size_t object_id_len, unsigned long flags,
+			uint32_t *obj);
+
+TEE_Result syscall_storage_obj_create(unsigned long storage_id, void *object_id,
+			size_t object_id_len, unsigned long flags,
+			unsigned long attr, void *data, size_t len,
+			uint32_t *obj);
+
+TEE_Result syscall_storage_obj_del(unsigned long obj);
+
+TEE_Result syscall_storage_obj_rename(unsigned long obj, void *object_id,
+			size_t object_id_len);
+
+/*
+ * Persistent Object Enumeration Functions
+ */
+TEE_Result syscall_storage_alloc_enum(uint32_t *obj_enum);
+
+TEE_Result syscall_storage_free_enum(unsigned long obj_enum);
+
+TEE_Result syscall_storage_reset_enum(unsigned long obj_enum);
+
+TEE_Result syscall_storage_start_enum(unsigned long obj_enum,
+			unsigned long storage_id);
+
+TEE_Result syscall_storage_next_enum(unsigned long obj_enum,
+			TEE_ObjectInfo *info, void *obj_id, uint64_t *len);
+
+/*
+ * Data Stream Access Functions
+ */
+TEE_Result syscall_storage_obj_read(unsigned long obj, void *data, size_t len,
+			uint64_t *count);
+
+TEE_Result syscall_storage_obj_write(unsigned long obj, void *data,
+			size_t len);
+
+TEE_Result syscall_storage_obj_trunc(unsigned long obj, size_t len);
+
+TEE_Result syscall_storage_obj_seek(unsigned long obj, int32_t offset,
+				    unsigned long whence);
+
+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);
+
+char *tee_svc_storage_create_dirname(struct tee_ta_session *sess);
+
+#endif /* TEE_SVC_STORAGE_H */
diff --git a/core/include/tee/uuid.h b/core/include/tee/uuid.h
new file mode 100644
index 0000000..b23ffb1
--- /dev/null
+++ b/core/include/tee/uuid.h
@@ -0,0 +1,49 @@
+/*
+ * Copyright (c) 2016, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef __TEE_UUID
+#define __TEE_UUID
+
+#include <optee_msg.h>
+#include <tee_api_types.h>
+#include <types_ext.h>
+
+/**
+ * tee_uuid_to_octets() - serialize a TEE_UUID into an octet string
+ * @dst: pointer to octet string
+ * @src: pointer TEE_UUID
+ */
+void tee_uuid_to_octets(uint8_t *dst, const TEE_UUID *src);
+
+/**
+ * tee_uuid_from_octets() - de-serialize an octet string into a TEE_UUID
+ * @dst: pointer TEE_UUID
+ * @src: pointer to octet string
+ */
+void tee_uuid_from_octets(TEE_UUID *dst, const uint8_t *src);
+
+#endif /*__TEE_UUID*/
diff --git a/core/kernel/asan.c b/core/kernel/asan.c
new file mode 100644
index 0000000..695f817
--- /dev/null
+++ b/core/kernel/asan.c
@@ -0,0 +1,271 @@
+/*
+ * Copyright (c) 2016, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <assert.h>
+#include <compiler.h>
+#include <kernel/panic.h>
+#include <kernel/asan.h>
+#include <string.h>
+#include <types_ext.h>
+#include <util.h>
+#include <trace.h>
+
+struct asan_source_location {
+	const char *file_name;
+	int line_no;
+	int column_no;
+};
+
+struct asan_global {
+	uintptr_t beg;
+	uintptr_t size;
+	uintptr_t size_with_redzone;
+	const char *name;
+	const char *module_name;
+	uintptr_t has_dynamic_init;
+	struct asan_source_location *location;
+};
+
+static vaddr_t asan_va_base;
+static size_t asan_va_size;
+static bool asan_active;
+
+static int8_t *va_to_shadow(void *va)
+{
+	vaddr_t sa = ((vaddr_t)va / ASAN_BLOCK_SIZE) + CFG_ASAN_SHADOW_OFFSET;
+
+	return (int8_t *)sa;
+}
+
+static size_t va_range_to_shadow_size(void *begin, void *end)
+{
+	return ((vaddr_t)end - (vaddr_t)begin) / ASAN_BLOCK_SIZE;
+}
+
+static bool va_range_inside_shadow(void *begin, void *end)
+{
+	vaddr_t b = (vaddr_t)begin;
+	vaddr_t e = (vaddr_t)end;
+
+	if (b >= e)
+		return false;
+	return (b >= asan_va_base) && (e <= (asan_va_base + asan_va_size));
+}
+
+static bool va_range_outside_shadow(void *begin, void *end)
+{
+	vaddr_t b = (vaddr_t)begin;
+	vaddr_t e = (vaddr_t)end;
+
+	if (b >= e)
+		return false;
+	return (e <= asan_va_base) || (b >= (asan_va_base + asan_va_size));
+}
+
+static size_t va_misalignment(void *va)
+{
+	return (vaddr_t)va & ASAN_BLOCK_MASK;
+}
+
+static bool va_is_well_aligned(void *va)
+{
+	return !va_misalignment(va);
+}
+
+void asan_set_shadowed(void *begin, void *end)
+{
+	vaddr_t b = (vaddr_t)begin;
+	vaddr_t e = (vaddr_t)end;
+
+	assert(!asan_va_base);
+	assert(va_is_well_aligned(begin));
+	assert(va_is_well_aligned(end));
+	assert(b < e);
+
+	asan_va_base = b;
+	asan_va_size = e - b;
+}
+
+void asan_tag_no_access(void *begin, void *end)
+{
+	assert(va_is_well_aligned(begin));
+	assert(va_is_well_aligned(end));
+	assert(va_range_inside_shadow(begin, end));
+
+	memset(va_to_shadow(begin), ASAN_DATA_RED_ZONE,
+	       va_range_to_shadow_size(begin, end));
+}
+
+void asan_tag_access(void *begin, void *end)
+{
+	if (!asan_va_base)
+		return;
+
+	assert(va_range_inside_shadow(begin, end));
+	assert(va_is_well_aligned(begin));
+
+	memset(va_to_shadow(begin), 0, va_range_to_shadow_size(begin, end));
+	if (!va_is_well_aligned(end))
+		*va_to_shadow(end) = ASAN_BLOCK_SIZE - va_misalignment(end);
+}
+
+void asan_tag_heap_free(void *begin, void *end)
+{
+	if (!asan_va_base)
+		return;
+
+	assert(va_range_inside_shadow(begin, end));
+	assert(va_is_well_aligned(begin));
+	assert(va_is_well_aligned(end));
+
+	memset(va_to_shadow(begin), ASAN_HEAP_RED_ZONE,
+	       va_range_to_shadow_size(begin, end));
+}
+
+void asan_start(void)
+{
+	assert(asan_va_base && !asan_active);
+	asan_active = true;
+}
+
+static void check_access(vaddr_t addr, size_t size)
+{
+	void *begin = (void *)addr;
+	void *end = (void *)(addr + size);
+	int8_t *a;
+	int8_t *e;
+
+	if (!asan_active || !size)
+		return;
+	if (va_range_outside_shadow(begin, end))
+		return;
+	/*
+	 * If it isn't outside it has to be completely inside or there's a
+	 * problem.
+	 */
+	if (!va_range_inside_shadow(begin, end))
+		panic();
+
+	e = va_to_shadow(end);
+	for (a = va_to_shadow(begin); a != e; a++)
+		if (*a < 0)
+			panic();
+
+	if (!va_is_well_aligned(end) &&
+	    va_misalignment(end) > (size_t)(*e - ASAN_BLOCK_SIZE))
+		panic();
+}
+
+static void check_load(vaddr_t addr, size_t size)
+{
+	check_access(addr, size);
+}
+
+static void check_store(vaddr_t addr, size_t size)
+{
+	check_access(addr, size);
+}
+
+static void __noreturn report_load(vaddr_t addr __unused, size_t size __unused)
+{
+	panic();
+}
+
+static void __noreturn report_store(vaddr_t addr __unused, size_t size __unused)
+{
+	panic();
+}
+
+
+
+#define DEFINE_ASAN_FUNC(type, size)				\
+	void __asan_##type##size(vaddr_t addr);			\
+	void __asan_##type##size(vaddr_t addr)			\
+	{ check_##type(addr, size); }				\
+	void __asan_##type##size##_noabort(vaddr_t addr);	\
+	void __asan_##type##size##_noabort(vaddr_t addr)	\
+	{ check_##type(addr, size); }				\
+	void __asan_report_##type##size##_noabort(vaddr_t addr);\
+	void __noreturn __asan_report_##type##size##_noabort(vaddr_t addr) \
+	{ report_##type(addr, size); }
+
+DEFINE_ASAN_FUNC(load, 1)
+DEFINE_ASAN_FUNC(load, 2)
+DEFINE_ASAN_FUNC(load, 4)
+DEFINE_ASAN_FUNC(load, 8)
+DEFINE_ASAN_FUNC(load, 16)
+DEFINE_ASAN_FUNC(store, 1)
+DEFINE_ASAN_FUNC(store, 2)
+DEFINE_ASAN_FUNC(store, 4)
+DEFINE_ASAN_FUNC(store, 8)
+DEFINE_ASAN_FUNC(store, 16)
+
+void __asan_loadN_noabort(vaddr_t addr, size_t size);
+void __asan_loadN_noabort(vaddr_t addr, size_t size)
+{
+	check_load(addr, size);
+}
+
+void __asan_storeN_noabort(vaddr_t addr, size_t size);
+void __asan_storeN_noabort(vaddr_t addr, size_t size)
+{
+	check_store(addr, size);
+}
+
+void __asan_report_load_n_noabort(vaddr_t addr, size_t size);
+void __noreturn __asan_report_load_n_noabort(vaddr_t addr, size_t size)
+{
+	report_load(addr, size);
+}
+
+void __asan_report_store_n_noabort(vaddr_t addr, size_t size);
+void __noreturn __asan_report_store_n_noabort(vaddr_t addr, size_t size)
+{
+	report_store(addr, size);
+}
+
+void __asan_handle_no_return(void);
+void __noreturn __asan_handle_no_return(void)
+{
+	panic();
+}
+
+void __asan_register_globals(struct asan_global *globals, size_t size);
+void __asan_register_globals(struct asan_global *globals, size_t size)
+{
+	size_t n;
+
+	for (n = 0; n < size; n++)
+		asan_tag_access((void *)globals[n].beg,
+				(void *)(globals[n].beg + globals[n].size));
+}
+
+void __asan_unregister_globals(struct asan_global *globals, size_t size);
+void __asan_unregister_globals(struct asan_global *globals __unused,
+			       size_t size __unused)
+{
+}
diff --git a/core/kernel/assert.c b/core/kernel/assert.c
new file mode 100644
index 0000000..ca2f774
--- /dev/null
+++ b/core/kernel/assert.c
@@ -0,0 +1,51 @@
+/*
+ * Copyright (c) 2014, STMicroelectronics International N.V.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <assert.h>
+#include <compiler.h>
+#include <trace.h>
+#include <kernel/panic.h>
+
+/* assert log and break for the optee kernel */
+
+void _assert_log(const char *expr __maybe_unused,
+		 const char *file __maybe_unused,
+		 const int line __maybe_unused,
+		 const char *func __maybe_unused)
+{
+#if defined(CFG_TEE_CORE_DEBUG)
+	EMSG_RAW("assertion '%s' failed at %s:%d <%s>",
+		 expr, file, line, func);
+#else
+	EMSG_RAW("assertion failed");
+#endif
+}
+
+void __noreturn _assert_break(void)
+{
+	panic();
+}
diff --git a/core/kernel/handle.c b/core/kernel/handle.c
new file mode 100644
index 0000000..b8dcabf
--- /dev/null
+++ b/core/kernel/handle.c
@@ -0,0 +1,101 @@
+/*
+ * Copyright (c) 2014, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+#include <stdlib.h>
+#include <string.h>
+#include <kernel/handle.h>
+
+/*
+ * Define the initial capacity of the database. It should be a low number
+ * multiple of 2 since some databases a likely to only use a few handles.
+ * Since the algorithm is to doubles up when growing it shouldn't cause a
+ * noticable overhead on large databases.
+ */
+#define HANDLE_DB_INITIAL_MAX_PTRS	4
+
+void handle_db_destroy(struct handle_db *db)
+{
+	if (db) {
+		free(db->ptrs);
+		db->ptrs = NULL;
+		db->max_ptrs = 0;
+	}
+}
+
+int handle_get(struct handle_db *db, void *ptr)
+{
+	size_t n;
+	void *p;
+	size_t new_max_ptrs;
+
+	if (!db || !ptr)
+		return -1;
+
+	/* Try to find an empty location */
+	for (n = 0; n < db->max_ptrs; n++) {
+		if (!db->ptrs[n]) {
+			db->ptrs[n] = ptr;
+			return n;
+		}
+	}
+
+	/* No location available, grow the ptrs array */
+	if (db->max_ptrs)
+		new_max_ptrs = db->max_ptrs * 2;
+	else
+		new_max_ptrs = HANDLE_DB_INITIAL_MAX_PTRS;
+	p = realloc(db->ptrs, new_max_ptrs * sizeof(void *));
+	if (!p)
+		return -1;
+	db->ptrs = p;
+	memset(db->ptrs + db->max_ptrs, 0,
+	       (new_max_ptrs - db->max_ptrs) * sizeof(void *));
+	db->max_ptrs = new_max_ptrs;
+
+	/* Since n stopped at db->max_ptrs there is an empty location there */
+	db->ptrs[n] = ptr;
+	return n;
+}
+
+void *handle_put(struct handle_db *db, int handle)
+{
+	void *p;
+
+	if (!db || handle < 0 || (size_t)handle >= db->max_ptrs)
+		return NULL;
+
+	p = db->ptrs[handle];
+	db->ptrs[handle] = NULL;
+	return p;
+}
+
+void *handle_lookup(struct handle_db *db, int handle)
+{
+	if (!db || handle < 0 || (size_t)handle >= db->max_ptrs)
+		return NULL;
+
+	return db->ptrs[handle];
+}
diff --git a/core/kernel/interrupt.c b/core/kernel/interrupt.c
new file mode 100644
index 0000000..efa8f2e
--- /dev/null
+++ b/core/kernel/interrupt.c
@@ -0,0 +1,102 @@
+/*
+ * Copyright (c) 2016, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <kernel/interrupt.h>
+#include <trace.h>
+
+/*
+ * NOTE!
+ *
+ * We're assuming that there's no concurrent use of this interface, except
+ * delivery of interrupts in parallel. Synchronization will be needed when
+ * we begin to modify settings after boot initialization.
+ */
+
+static struct itr_chip *itr_chip;
+static SLIST_HEAD(, itr_handler) handlers = SLIST_HEAD_INITIALIZER(handlers);
+
+void itr_init(struct itr_chip *chip)
+{
+	itr_chip = chip;
+}
+
+static struct itr_handler *find_handler(size_t it)
+{
+	struct itr_handler *h;
+
+	SLIST_FOREACH(h, &handlers, link)
+		if (h->it == it)
+			return h;
+	return NULL;
+}
+
+void itr_handle(size_t it)
+{
+	struct itr_handler *h = find_handler(it);
+
+	if (!h) {
+		EMSG("Disabling unhandled interrupt %zu", it);
+		itr_chip->ops->disable(itr_chip, it);
+		return;
+	}
+
+	if (h->handler(h) != ITRR_HANDLED) {
+		EMSG("Disabling interrupt %zu not handled by handler", it);
+		itr_chip->ops->disable(itr_chip, it);
+	}
+}
+
+void itr_add(struct itr_handler *h)
+{
+	itr_chip->ops->add(itr_chip, h->it, h->flags);
+	SLIST_INSERT_HEAD(&handlers, h, link);
+}
+
+void itr_enable(size_t it)
+{
+	itr_chip->ops->enable(itr_chip, it);
+}
+
+void itr_disable(size_t it)
+{
+	itr_chip->ops->disable(itr_chip, it);
+}
+
+void itr_raise_pi(size_t it)
+{
+	itr_chip->ops->raise_pi(itr_chip, it);
+}
+
+void itr_raise_sgi(size_t it, uint8_t cpu_mask)
+{
+	itr_chip->ops->raise_sgi(itr_chip, it, cpu_mask);
+}
+
+void itr_set_affinity(size_t it, uint8_t cpu_mask)
+{
+	itr_chip->ops->set_affinity(itr_chip, it, cpu_mask);
+}
diff --git a/core/kernel/panic.c b/core/kernel/panic.c
new file mode 100644
index 0000000..c9623f6
--- /dev/null
+++ b/core/kernel/panic.c
@@ -0,0 +1,56 @@
+/*
+ * Copyright (c) 2016, Linaro Limited
+ * Copyright (c) 2014, STMicroelectronics International N.V.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <kernel/panic.h>
+#include <kernel/thread.h>
+#include <trace.h>
+
+void __do_panic(const char *file __maybe_unused,
+		const int line __maybe_unused,
+		const char *func __maybe_unused,
+		const char *msg __maybe_unused)
+{
+	/* disable prehemption */
+	(void)thread_mask_exceptions(THREAD_EXCP_ALL);
+
+	/* TODO: notify other cores */
+
+	/* trace: Panic ['panic-string-message' ]at FILE:LINE [<FUNCTION>]" */
+	if (!file && !func && !msg)
+		EMSG_RAW("Panic");
+	else
+		EMSG_RAW("Panic %s%s%sat %s:%d %s%s%s",
+			 msg ? "'" : "", msg ? msg : "", msg ? "' " : "",
+			 file ? file : "?", file ? line : 0,
+			 func ? "<" : "", func ? func : "", func ? ">" : "");
+
+	EPRINT_STACK();
+	/* abort current execution */
+	while (1)
+		;
+}
diff --git a/core/kernel/sub.mk b/core/kernel/sub.mk
new file mode 100644
index 0000000..aa00ae5
--- /dev/null
+++ b/core/kernel/sub.mk
@@ -0,0 +1,9 @@
+srcs-y += assert.c
+srcs-y += tee_ta_manager.c
+srcs-y += tee_misc.c
+srcs-y += panic.c
+srcs-y += handle.c
+srcs-y += interrupt.c
+srcs-$(CFG_CORE_SANITIZE_UNDEFINED) += ubsan.c
+srcs-$(CFG_CORE_SANITIZE_KADDRESS) += asan.c
+cflags-remove-asan.c-y += $(cflags_kasan)
diff --git a/core/kernel/tee_misc.c b/core/kernel/tee_misc.c
new file mode 100644
index 0000000..c78e7f5
--- /dev/null
+++ b/core/kernel/tee_misc.c
@@ -0,0 +1,139 @@
+/*
+ * 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 <stdio.h>
+#include <kernel/tee_common.h>
+#include <kernel/chip_services.h>
+#include <kernel/tee_misc.h>
+#include <mm/core_memprot.h>
+#include <kernel/tee_common_otp.h>
+#include <trace.h>
+
+static uint8_t tee_b2hs_add_base(uint8_t in)
+{
+	if (in > 9)
+		return in + 55;
+	else
+		return in + 48;
+}
+
+static int tee_hs2b_rem_base(uint8_t in, uint8_t *out)
+{
+	if (in < 48 || in > 70 || (in > 57 && in < 65))
+		return -1;
+
+	if (in < 58)
+		*out = in - 48;
+	else
+		*out = in - 55;
+
+	return 0;
+}
+
+uint32_t tee_b2hs(uint8_t *b, uint8_t *hs, uint32_t blen, uint32_t hslen)
+{
+	uint32_t i = 0;
+
+	if (blen * 2 + 1 > hslen)
+		return 0;
+
+	for (; i < blen; i++) {
+		hs[i * 2 + 1] = tee_b2hs_add_base(b[i] & 0xf);
+		hs[i * 2] = tee_b2hs_add_base(b[i] >> 4);
+	}
+	hs[blen * 2] = 0;
+
+	return blen * 2;
+}
+
+uint32_t tee_hs2b(uint8_t *hs, uint8_t *b, uint32_t hslen, uint32_t blen)
+{
+	uint32_t i = 0;
+	uint32_t len = TEE_HS2B_BBUF_SIZE(hslen);
+	uint8_t hi;
+	uint8_t lo;
+
+	if (len > blen)
+		return 0;
+
+	for (; i < len; i++) {
+		if (tee_hs2b_rem_base(hs[i * 2], &hi))
+			return 0;
+		if (tee_hs2b_rem_base(hs[i * 2 + 1], &lo))
+			return 0;
+		b[i] = (hi << 4) + lo;
+	}
+
+	return len;
+}
+
+static bool is_valid_conf_and_notnull_size(
+		vaddr_t b, size_t bl, vaddr_t a, size_t al)
+{
+	/* invalid config return false */
+	if ((b - 1 + bl < b) || (a - 1 + al < a))
+		return false;
+	/* null sized areas are never inside / outside / overlap */
+	if (!bl || !al)
+		return false;
+	return true;
+}
+
+/* Returns true when buffer 'b' is fully contained in area 'a' */
+bool _core_is_buffer_inside(vaddr_t b, size_t bl, vaddr_t a, size_t al)
+{
+	/* invalid config or "null size" return false */
+	if (!is_valid_conf_and_notnull_size(b, bl, a, al))
+		return false;
+
+	if ((b >= a) && (b - 1 + bl <= a - 1 + al))
+		return true;
+	return false;
+}
+
+/* Returns true when buffer 'b' is fully contained in area 'a' */
+bool _core_is_buffer_outside(vaddr_t b, size_t bl, vaddr_t a, size_t al)
+{
+	/* invalid config or "null size" return false */
+	if (!is_valid_conf_and_notnull_size(b, bl, a, al))
+		return false;
+
+	if ((b + bl <= a) || (b >= a + al))
+		return true;
+	return false;
+}
+
+/* Returns true when buffer 'b' intersects area 'a' */
+bool _core_is_buffer_intersect(vaddr_t b, size_t bl, vaddr_t a, size_t al)
+{
+	/* invalid config or "null size" return false */
+	if (!is_valid_conf_and_notnull_size(b, bl, a, al))
+		return false;
+
+	if ((b + bl <= a) || (b >= a + al))
+		return false;
+	return true;
+}
diff --git a/core/kernel/tee_ta_manager.c b/core/kernel/tee_ta_manager.c
new file mode 100644
index 0000000..c0c4545
--- /dev/null
+++ b/core/kernel/tee_ta_manager.c
@@ -0,0 +1,766 @@
+/*
+ * 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 <types_ext.h>
+#include <stdbool.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <arm.h>
+#include <assert.h>
+#include <kernel/mutex.h>
+#include <kernel/panic.h>
+#include <kernel/pseudo_ta.h>
+#include <kernel/tee_common.h>
+#include <kernel/tee_misc.h>
+#include <kernel/tee_ta_manager.h>
+#include <kernel/tee_time.h>
+#include <kernel/thread.h>
+#include <kernel/user_ta.h>
+#include <mm/core_mmu.h>
+#include <mm/core_memprot.h>
+#include <mm/tee_mmu.h>
+#include <tee/tee_svc_cryp.h>
+#include <tee/tee_obj.h>
+#include <tee/tee_svc_storage.h>
+#include <tee_api_types.h>
+#include <trace.h>
+#include <utee_types.h>
+#include <util.h>
+
+/* This mutex protects the critical section in tee_ta_init_session */
+struct mutex tee_ta_mutex = MUTEX_INITIALIZER;
+static struct condvar tee_ta_cv = CONDVAR_INITIALIZER;
+static int tee_ta_single_instance_thread = THREAD_ID_INVALID;
+static size_t tee_ta_single_instance_count;
+struct tee_ta_ctx_head tee_ctxes = TAILQ_HEAD_INITIALIZER(tee_ctxes);
+
+static void lock_single_instance(void)
+{
+	/* Requires tee_ta_mutex to be held */
+	if (tee_ta_single_instance_thread != thread_get_id()) {
+		/* Wait until the single-instance lock is available. */
+		while (tee_ta_single_instance_thread != THREAD_ID_INVALID)
+			condvar_wait(&tee_ta_cv, &tee_ta_mutex);
+
+		tee_ta_single_instance_thread = thread_get_id();
+		assert(tee_ta_single_instance_count == 0);
+	}
+
+	tee_ta_single_instance_count++;
+}
+
+static void unlock_single_instance(void)
+{
+	/* Requires tee_ta_mutex to be held */
+	assert(tee_ta_single_instance_thread == thread_get_id());
+	assert(tee_ta_single_instance_count > 0);
+
+	tee_ta_single_instance_count--;
+	if (tee_ta_single_instance_count == 0) {
+		tee_ta_single_instance_thread = THREAD_ID_INVALID;
+		condvar_signal(&tee_ta_cv);
+	}
+}
+
+static bool has_single_instance_lock(void)
+{
+	/* Requires tee_ta_mutex to be held */
+	return tee_ta_single_instance_thread == thread_get_id();
+}
+
+static bool tee_ta_try_set_busy(struct tee_ta_ctx *ctx)
+{
+	bool rc = true;
+
+	mutex_lock(&tee_ta_mutex);
+
+	if (ctx->flags & TA_FLAG_SINGLE_INSTANCE)
+		lock_single_instance();
+
+	if (has_single_instance_lock()) {
+		if (ctx->busy) {
+			/*
+			 * We're holding the single-instance lock and the
+			 * TA is busy, as waiting now would only cause a
+			 * dead-lock, we release the lock and return false.
+			 */
+			rc = false;
+			if (ctx->flags & TA_FLAG_SINGLE_INSTANCE)
+				unlock_single_instance();
+		}
+	} else {
+		/*
+		 * We're not holding the single-instance lock, we're free to
+		 * wait for the TA to become available.
+		 */
+		while (ctx->busy)
+			condvar_wait(&ctx->busy_cv, &tee_ta_mutex);
+	}
+
+	/* Either it's already true or we should set it to true */
+	ctx->busy = true;
+
+	mutex_unlock(&tee_ta_mutex);
+	return rc;
+}
+
+static void tee_ta_set_busy(struct tee_ta_ctx *ctx)
+{
+	if (!tee_ta_try_set_busy(ctx))
+		panic();
+}
+
+static void tee_ta_clear_busy(struct tee_ta_ctx *ctx)
+{
+	mutex_lock(&tee_ta_mutex);
+
+	assert(ctx->busy);
+	ctx->busy = false;
+	condvar_signal(&ctx->busy_cv);
+
+	if (ctx->flags & TA_FLAG_SINGLE_INSTANCE)
+		unlock_single_instance();
+
+	mutex_unlock(&tee_ta_mutex);
+}
+
+static void dec_session_ref_count(struct tee_ta_session *s)
+{
+	assert(s->ref_count > 0);
+	s->ref_count--;
+	if (s->ref_count == 1)
+		condvar_signal(&s->refc_cv);
+}
+
+void tee_ta_put_session(struct tee_ta_session *s)
+{
+	mutex_lock(&tee_ta_mutex);
+
+	if (s->lock_thread == thread_get_id()) {
+		s->lock_thread = THREAD_ID_INVALID;
+		condvar_signal(&s->lock_cv);
+	}
+	dec_session_ref_count(s);
+
+	mutex_unlock(&tee_ta_mutex);
+}
+
+static struct tee_ta_session *find_session(uint32_t id,
+			struct tee_ta_session_head *open_sessions)
+{
+	struct tee_ta_session *s;
+
+	TAILQ_FOREACH(s, open_sessions, link) {
+		if ((vaddr_t)s == id)
+			return s;
+	}
+	return NULL;
+}
+
+struct tee_ta_session *tee_ta_get_session(uint32_t id, bool exclusive,
+			struct tee_ta_session_head *open_sessions)
+{
+	struct tee_ta_session *s;
+
+	mutex_lock(&tee_ta_mutex);
+
+	while (true) {
+		s = find_session(id, open_sessions);
+		if (!s)
+			break;
+		if (s->unlink) {
+			s = NULL;
+			break;
+		}
+		s->ref_count++;
+		if (!exclusive)
+			break;
+
+		assert(s->lock_thread != thread_get_id());
+
+		while (s->lock_thread != THREAD_ID_INVALID && !s->unlink)
+			condvar_wait(&s->lock_cv, &tee_ta_mutex);
+
+		if (s->unlink) {
+			dec_session_ref_count(s);
+			s = NULL;
+			break;
+		}
+
+		s->lock_thread = thread_get_id();
+		break;
+	}
+
+	mutex_unlock(&tee_ta_mutex);
+	return s;
+}
+
+static void tee_ta_unlink_session(struct tee_ta_session *s,
+			struct tee_ta_session_head *open_sessions)
+{
+	mutex_lock(&tee_ta_mutex);
+
+	assert(s->ref_count >= 1);
+	assert(s->lock_thread == thread_get_id());
+	assert(!s->unlink);
+
+	s->unlink = true;
+	condvar_broadcast(&s->lock_cv);
+
+	while (s->ref_count != 1)
+		condvar_wait(&s->refc_cv, &tee_ta_mutex);
+
+	TAILQ_REMOVE(open_sessions, s, link);
+
+	mutex_unlock(&tee_ta_mutex);
+}
+
+/*
+ * tee_ta_context_find - Find TA in session list based on a UUID (input)
+ * Returns a pointer to the session
+ */
+static struct tee_ta_ctx *tee_ta_context_find(const TEE_UUID *uuid)
+{
+	struct tee_ta_ctx *ctx;
+
+	TAILQ_FOREACH(ctx, &tee_ctxes, link) {
+		if (memcmp(&ctx->uuid, uuid, sizeof(TEE_UUID)) == 0)
+			return ctx;
+	}
+
+	return NULL;
+}
+
+/* check if requester (client ID) matches session initial client */
+static TEE_Result check_client(struct tee_ta_session *s, const TEE_Identity *id)
+{
+	if (id == KERN_IDENTITY)
+		return TEE_SUCCESS;
+
+	if (id == NSAPP_IDENTITY) {
+		if (s->clnt_id.login == TEE_LOGIN_TRUSTED_APP) {
+			DMSG("nsec tries to hijack TA session");
+			return TEE_ERROR_ACCESS_DENIED;
+		}
+		return TEE_SUCCESS;
+	}
+
+	if (memcmp(&s->clnt_id, id, sizeof(TEE_Identity)) != 0) {
+		DMSG("client id mismatch");
+		return TEE_ERROR_ACCESS_DENIED;
+	}
+	return TEE_SUCCESS;
+}
+
+static void set_invoke_timeout(struct tee_ta_session *sess,
+				      uint32_t cancel_req_to)
+{
+	TEE_Time current_time;
+	TEE_Time cancel_time = { UINT32_MAX, UINT32_MAX };
+
+	if (cancel_req_to == TEE_TIMEOUT_INFINITE)
+		goto out;
+
+	if (tee_time_get_sys_time(&current_time) != TEE_SUCCESS)
+		goto out;
+
+	/* Check that it doesn't wrap */
+	if (current_time.seconds + (cancel_req_to / 1000) >=
+	    current_time.seconds) {
+		cancel_time.seconds =
+		    current_time.seconds + cancel_req_to / 1000;
+		cancel_time.millis = current_time.millis + cancel_req_to % 1000;
+		if (cancel_time.millis > 1000) {
+			cancel_time.seconds++;
+			cancel_time.millis -= 1000;
+		}
+	}
+
+out:
+	sess->cancel_time = cancel_time;
+}
+
+/*-----------------------------------------------------------------------------
+ * Close a Trusted Application and free available resources
+ *---------------------------------------------------------------------------*/
+TEE_Result tee_ta_close_session(struct tee_ta_session *csess,
+				struct tee_ta_session_head *open_sessions,
+				const TEE_Identity *clnt_id)
+{
+	struct tee_ta_session *sess;
+	struct tee_ta_ctx *ctx;
+
+	DMSG("tee_ta_close_session(0x%" PRIxVA ")",  (vaddr_t)csess);
+
+	if (!csess)
+		return TEE_ERROR_ITEM_NOT_FOUND;
+
+	sess = tee_ta_get_session((vaddr_t)csess, true, open_sessions);
+
+	if (!sess) {
+		EMSG("session 0x%" PRIxVA " to be removed is not found",
+		     (vaddr_t)csess);
+		return TEE_ERROR_ITEM_NOT_FOUND;
+	}
+
+	if (check_client(sess, clnt_id) != TEE_SUCCESS) {
+		tee_ta_put_session(sess);
+		return TEE_ERROR_BAD_PARAMETERS; /* intentional generic error */
+	}
+
+	ctx = sess->ctx;
+	DMSG("   ... Destroy session");
+
+	tee_ta_set_busy(ctx);
+
+	if (!ctx->panicked) {
+		set_invoke_timeout(sess, TEE_TIMEOUT_INFINITE);
+		ctx->ops->enter_close_session(sess);
+	}
+
+	tee_ta_unlink_session(sess, open_sessions);
+#if defined(CFG_TA_GPROF_SUPPORT)
+	free(sess->sbuf);
+#endif
+	free(sess);
+
+	tee_ta_clear_busy(ctx);
+
+	mutex_lock(&tee_ta_mutex);
+
+	if (ctx->ref_count <= 0)
+		panic();
+
+	ctx->ref_count--;
+	if (!ctx->ref_count && !(ctx->flags & TA_FLAG_INSTANCE_KEEP_ALIVE)) {
+		DMSG("   ... Destroy TA ctx");
+
+		TAILQ_REMOVE(&tee_ctxes, ctx, link);
+		mutex_unlock(&tee_ta_mutex);
+
+		condvar_destroy(&ctx->busy_cv);
+
+		pgt_flush_ctx(ctx);
+		ctx->ops->destroy(ctx);
+	} else
+		mutex_unlock(&tee_ta_mutex);
+
+	return TEE_SUCCESS;
+}
+
+static TEE_Result tee_ta_init_session_with_context(struct tee_ta_ctx *ctx,
+			struct tee_ta_session *s)
+{
+	/*
+	 * If TA isn't single instance it should be loaded as new
+	 * instance instead of doing anything with this instance.
+	 * So tell the caller that we didn't find the TA it the
+	 * caller will load a new instance.
+	 */
+	if ((ctx->flags & TA_FLAG_SINGLE_INSTANCE) == 0)
+		return TEE_ERROR_ITEM_NOT_FOUND;
+
+	/*
+	 * The TA is single instance, if it isn't multi session we
+	 * can't create another session unless it's the first
+	 * new session towards a keepAlive TA.
+	 */
+
+	if (((ctx->flags & TA_FLAG_MULTI_SESSION) == 0) &&
+	    !(((ctx->flags & TA_FLAG_INSTANCE_KEEP_ALIVE) != 0) &&
+	      (ctx->ref_count == 0)))
+		return TEE_ERROR_BUSY;
+
+	DMSG("   ... Re-open TA %pUl", (void *)&ctx->uuid);
+
+	ctx->ref_count++;
+	s->ctx = ctx;
+	return TEE_SUCCESS;
+}
+
+
+static TEE_Result tee_ta_init_session(TEE_ErrorOrigin *err,
+				struct tee_ta_session_head *open_sessions,
+				const TEE_UUID *uuid,
+				struct tee_ta_session **sess)
+{
+	TEE_Result res;
+	struct tee_ta_ctx *ctx;
+	struct tee_ta_session *s = calloc(1, sizeof(struct tee_ta_session));
+
+	*err = TEE_ORIGIN_TEE;
+	if (!s)
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	s->cancel_mask = true;
+	condvar_init(&s->refc_cv);
+	condvar_init(&s->lock_cv);
+	s->lock_thread = THREAD_ID_INVALID;
+	s->ref_count = 1;
+
+
+	/*
+	 * We take the global TA mutex here and hold it while doing
+	 * RPC to load the TA. This big critical section should be broken
+	 * down into smaller pieces.
+	 */
+
+
+	mutex_lock(&tee_ta_mutex);
+	TAILQ_INSERT_TAIL(open_sessions, s, link);
+
+	/* Look for already loaded TA */
+	ctx = tee_ta_context_find(uuid);
+	if (ctx) {
+		res = tee_ta_init_session_with_context(ctx, s);
+		if (res == TEE_SUCCESS || res != TEE_ERROR_ITEM_NOT_FOUND)
+			goto out;
+	}
+
+	/* Look for static TA */
+	res = tee_ta_init_pseudo_ta_session(uuid, s);
+	if (res == TEE_SUCCESS || res != TEE_ERROR_ITEM_NOT_FOUND)
+		goto out;
+
+	/* Look for user TA */
+	res = tee_ta_init_user_ta_session(uuid, s);
+
+out:
+	if (res == TEE_SUCCESS) {
+		*sess = s;
+	} else {
+		TAILQ_REMOVE(open_sessions, s, link);
+		free(s);
+	}
+	mutex_unlock(&tee_ta_mutex);
+	return res;
+}
+
+TEE_Result tee_ta_open_session(TEE_ErrorOrigin *err,
+			       struct tee_ta_session **sess,
+			       struct tee_ta_session_head *open_sessions,
+			       const TEE_UUID *uuid,
+			       const TEE_Identity *clnt_id,
+			       uint32_t cancel_req_to,
+			       struct tee_ta_param *param)
+{
+	TEE_Result res;
+	struct tee_ta_session *s = NULL;
+	struct tee_ta_ctx *ctx;
+	bool panicked;
+	bool was_busy = false;
+
+	res = tee_ta_init_session(err, open_sessions, uuid, &s);
+	if (res != TEE_SUCCESS) {
+		DMSG("init session failed 0x%x", res);
+		return res;
+	}
+
+	ctx = s->ctx;
+
+	if (ctx->panicked) {
+		DMSG("panicked, call tee_ta_close_session()");
+		tee_ta_close_session(s, open_sessions, KERN_IDENTITY);
+		*err = TEE_ORIGIN_TEE;
+		return TEE_ERROR_TARGET_DEAD;
+	}
+
+	*sess = s;
+	/* Save identity of the owner of the session */
+	s->clnt_id = *clnt_id;
+
+	if (tee_ta_try_set_busy(ctx)) {
+		set_invoke_timeout(s, cancel_req_to);
+		res = ctx->ops->enter_open_session(s, param, err);
+		tee_ta_clear_busy(ctx);
+	} else {
+		/* Deadlock avoided */
+		res = TEE_ERROR_BUSY;
+		was_busy = true;
+	}
+
+	panicked = ctx->panicked;
+
+	tee_ta_put_session(s);
+	if (panicked || (res != TEE_SUCCESS))
+		tee_ta_close_session(s, open_sessions, KERN_IDENTITY);
+
+	/*
+	 * Origin error equal to TEE_ORIGIN_TRUSTED_APP for "regular" error,
+	 * apart from panicking.
+	 */
+	if (panicked || was_busy)
+		*err = TEE_ORIGIN_TEE;
+	else
+		*err = TEE_ORIGIN_TRUSTED_APP;
+
+	if (res != TEE_SUCCESS)
+		EMSG("Failed. Return error 0x%x", res);
+
+	return res;
+}
+
+TEE_Result tee_ta_invoke_command(TEE_ErrorOrigin *err,
+				 struct tee_ta_session *sess,
+				 const TEE_Identity *clnt_id,
+				 uint32_t cancel_req_to, uint32_t cmd,
+				 struct tee_ta_param *param)
+{
+	TEE_Result res;
+
+	if (check_client(sess, clnt_id) != TEE_SUCCESS)
+		return TEE_ERROR_BAD_PARAMETERS; /* intentional generic error */
+
+	if (sess->ctx->panicked) {
+		DMSG("   Panicked !");
+		*err = TEE_ORIGIN_TEE;
+		return TEE_ERROR_TARGET_DEAD;
+	}
+
+	tee_ta_set_busy(sess->ctx);
+
+	set_invoke_timeout(sess, cancel_req_to);
+	res = sess->ctx->ops->enter_invoke_cmd(sess, cmd, param, err);
+
+	if (sess->ctx->panicked) {
+		*err = TEE_ORIGIN_TEE;
+		res = TEE_ERROR_TARGET_DEAD;
+	}
+
+	tee_ta_clear_busy(sess->ctx);
+	if (res != TEE_SUCCESS)
+		DMSG("  => Error: %x of %d\n", res, *err);
+	return res;
+}
+
+TEE_Result tee_ta_cancel_command(TEE_ErrorOrigin *err,
+				 struct tee_ta_session *sess,
+				 const TEE_Identity *clnt_id)
+{
+	*err = TEE_ORIGIN_TEE;
+
+	if (check_client(sess, clnt_id) != TEE_SUCCESS)
+		return TEE_ERROR_BAD_PARAMETERS; /* intentional generic error */
+
+	sess->cancel = true;
+	return TEE_SUCCESS;
+}
+
+bool tee_ta_session_is_cancelled(struct tee_ta_session *s, TEE_Time *curr_time)
+{
+	TEE_Time current_time;
+
+	if (s->cancel_mask)
+		return false;
+
+	if (s->cancel)
+		return true;
+
+	if (s->cancel_time.seconds == UINT32_MAX)
+		return false;
+
+	if (curr_time != NULL)
+		current_time = *curr_time;
+	else if (tee_time_get_sys_time(&current_time) != TEE_SUCCESS)
+		return false;
+
+	if (current_time.seconds > s->cancel_time.seconds ||
+	    (current_time.seconds == s->cancel_time.seconds &&
+	     current_time.millis >= s->cancel_time.millis)) {
+		return true;
+	}
+
+	return false;
+}
+
+static void update_current_ctx(struct thread_specific_data *tsd)
+{
+	struct tee_ta_ctx *ctx = NULL;
+	struct tee_ta_session *s = TAILQ_FIRST(&tsd->sess_stack);
+
+	if (s) {
+		if (is_pseudo_ta_ctx(s->ctx))
+			s = TAILQ_NEXT(s, link_tsd);
+
+		if (s)
+			ctx = s->ctx;
+	}
+
+	if (tsd->ctx != ctx)
+		tee_mmu_set_ctx(ctx);
+	/*
+	 * If ctx->mmu == NULL we must not have user mapping active,
+	 * if ctx->mmu != NULL we must have user mapping active.
+	 */
+	if (((ctx && is_user_ta_ctx(ctx) ?
+			to_user_ta_ctx(ctx)->mmu : NULL) == NULL) ==
+					core_mmu_user_mapping_is_active())
+		panic("unexpected active mapping");
+}
+
+void tee_ta_push_current_session(struct tee_ta_session *sess)
+{
+	struct thread_specific_data *tsd = thread_get_tsd();
+
+	TAILQ_INSERT_HEAD(&tsd->sess_stack, sess, link_tsd);
+	update_current_ctx(tsd);
+}
+
+struct tee_ta_session *tee_ta_pop_current_session(void)
+{
+	struct thread_specific_data *tsd = thread_get_tsd();
+	struct tee_ta_session *s = TAILQ_FIRST(&tsd->sess_stack);
+
+	if (s) {
+		TAILQ_REMOVE(&tsd->sess_stack, s, link_tsd);
+		update_current_ctx(tsd);
+	}
+	return s;
+}
+
+TEE_Result tee_ta_get_current_session(struct tee_ta_session **sess)
+{
+	struct tee_ta_session *s = TAILQ_FIRST(&thread_get_tsd()->sess_stack);
+
+	if (!s)
+		return TEE_ERROR_BAD_STATE;
+	*sess = s;
+	return TEE_SUCCESS;
+}
+
+struct tee_ta_session *tee_ta_get_calling_session(void)
+{
+	struct tee_ta_session *s = TAILQ_FIRST(&thread_get_tsd()->sess_stack);
+
+	if (s)
+		s = TAILQ_NEXT(s, link_tsd);
+	return s;
+}
+
+TEE_Result tee_ta_get_client_id(TEE_Identity *id)
+{
+	TEE_Result res;
+	struct tee_ta_session *sess;
+
+	res = tee_ta_get_current_session(&sess);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	if (id == NULL)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	*id = sess->clnt_id;
+	return TEE_SUCCESS;
+}
+
+/*
+ * dump_state - Display TA state as an error log.
+ */
+static void dump_state(struct tee_ta_ctx *ctx)
+{
+	struct tee_ta_session *s = NULL;
+	bool active __maybe_unused;
+
+	active = ((tee_ta_get_current_session(&s) == TEE_SUCCESS) &&
+		  s && s->ctx == ctx);
+
+	EMSG_RAW("Status of TA %pUl (%p) %s", (void *)&ctx->uuid, (void *)ctx,
+		active ? "(active)" : "");
+	ctx->ops->dump_state(ctx);
+}
+
+void tee_ta_dump_current(void)
+{
+	struct tee_ta_session *s = NULL;
+
+	if (tee_ta_get_current_session(&s) != TEE_SUCCESS) {
+		EMSG("no valid session found, cannot log TA status");
+		return;
+	}
+
+	dump_state(s->ctx);
+}
+
+#if defined(CFG_TA_GPROF_SUPPORT)
+void tee_ta_gprof_sample_pc(vaddr_t pc)
+{
+	struct tee_ta_session *s;
+	struct sample_buf *sbuf;
+	size_t idx;
+
+	if (tee_ta_get_current_session(&s) != TEE_SUCCESS)
+		return;
+	sbuf = s->sbuf;
+	if (!sbuf || !sbuf->enabled)
+		return; /* PC sampling is not enabled */
+
+	idx = (((uint64_t)pc - sbuf->offset)/2 * sbuf->scale)/65536;
+	if (idx < sbuf->nsamples)
+		sbuf->samples[idx]++;
+	sbuf->count++;
+}
+
+/*
+ * Update user-mode CPU time for the current session
+ * @suspend: true if session is being suspended (leaving user mode), false if
+ * it is resumed (entering user mode)
+ */
+static void tee_ta_update_session_utime(bool suspend)
+{
+	struct tee_ta_session *s;
+	struct sample_buf *sbuf;
+	uint64_t now;
+
+	if (tee_ta_get_current_session(&s) != TEE_SUCCESS)
+		return;
+	sbuf = s->sbuf;
+	if (!sbuf)
+		return;
+	now = read_cntpct();
+	if (suspend) {
+		assert(sbuf->usr_entered);
+		sbuf->usr += now - sbuf->usr_entered;
+		sbuf->usr_entered = 0;
+	} else {
+		assert(!sbuf->usr_entered);
+		if (!now)
+			now++; /* 0 is reserved */
+		sbuf->usr_entered = now;
+	}
+}
+
+void tee_ta_update_session_utime_suspend(void)
+{
+	tee_ta_update_session_utime(true);
+}
+
+void tee_ta_update_session_utime_resume(void)
+{
+	tee_ta_update_session_utime(false);
+}
+#endif
diff --git a/core/kernel/ubsan.c b/core/kernel/ubsan.c
new file mode 100644
index 0000000..53a920b
--- /dev/null
+++ b/core/kernel/ubsan.c
@@ -0,0 +1,235 @@
+/*
+ * Copyright (c) 2016, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <types_ext.h>
+#include <kernel/panic.h>
+#include <trace.h>
+#include <string.h>
+
+struct source_location {
+	const char *file_name;
+	uint32_t line;
+	uint32_t column;
+};
+
+struct type_descriptor {
+	uint16_t type_kind;
+	uint16_t type_info;
+	char type_name[1];
+};
+
+struct type_mismatch_data {
+	struct source_location loc;
+	struct type_descriptor *type;
+	unsigned long alignment;
+	unsigned char type_check_kind;
+};
+
+struct overflow_data {
+	struct source_location loc;
+	struct type_descriptor *type;
+};
+
+struct shift_out_of_bounds_data {
+	struct source_location loc;
+	struct type_descriptor *lhs_type;
+	struct type_descriptor *rhs_type;
+};
+
+struct out_of_bounds_data {
+	struct source_location loc;
+	struct type_descriptor *array_type;
+	struct type_descriptor *index_type;
+};
+
+struct unreachable_data {
+	struct source_location loc;
+};
+
+struct vla_bound_data {
+	struct source_location loc;
+	struct type_descriptor *type;
+};
+
+struct invalid_value_data {
+	struct source_location loc;
+	struct type_descriptor *type;
+};
+
+struct nonnull_arg_data {
+	struct source_location loc;
+};
+
+/*
+ * When compiling with -fsanitize=undefined the compiler expects functions
+ * with the following signatures. The functions are never called directly,
+ * only when undefined behavior is detected in instrumented code.
+ */
+void __ubsan_handle_type_mismatch(struct type_mismatch_data *data,
+				  unsigned long ptr);
+void __ubsan_handle_add_overflow(struct overflow_data *data,
+				  unsigned long lhs, unsigned long rhs);
+void __ubsan_handle_sub_overflow(struct overflow_data *data,
+				  unsigned long lhs, unsigned long rhs);
+void __ubsan_handle_mul_overflow(struct overflow_data *data,
+				  unsigned long lhs, unsigned long rhs);
+void __ubsan_handle_negate_overflow(struct overflow_data *data,
+				    unsigned long old_val);
+void __ubsan_handle_divrem_overflow(struct overflow_data *data,
+				    unsigned long lhs, unsigned long rhs);
+void __ubsan_handle_shift_out_of_bounds(struct shift_out_of_bounds_data *data,
+					unsigned long lhs, unsigned long rhs);
+void __ubsan_handle_out_of_bounds(struct out_of_bounds_data *data,
+				  unsigned long idx);
+void __ubsan_handle_unreachable(struct unreachable_data *data);
+void __ubsan_handle_missing_return(struct unreachable_data *data);
+void __ubsan_handle_vla_bound_not_positive(struct vla_bound_data *data,
+					   unsigned long bound);
+void __ubsan_handle_load_invalid_value(struct invalid_value_data *data,
+				       unsigned long val);
+void __ubsan_handle_nonnull_arg(struct nonnull_arg_data *data, size_t arg_no);
+
+static void print_loc(const char *func, struct source_location *loc)
+{
+	const char *f = func;
+	const char func_prefix[] = "__ubsan_handle";
+
+	if (!memcmp(f, func_prefix, sizeof(func_prefix) - 1))
+		f += sizeof(func_prefix);
+
+	EMSG_RAW("Undefined behavior %s at %s:%" PRIu32 " col %" PRIu32,
+		 f, loc->file_name, loc->line, loc->column);
+}
+
+
+static volatile bool ubsan_panic = true;
+
+void __ubsan_handle_type_mismatch(struct type_mismatch_data *data,
+				  unsigned long ptr __unused)
+{
+	print_loc(__func__, &data->loc);
+	if (ubsan_panic)
+		panic();
+}
+
+void __ubsan_handle_add_overflow(struct overflow_data *data,
+				 unsigned long lhs __unused,
+				 unsigned long rhs __unused)
+{
+	print_loc(__func__, &data->loc);
+	if (ubsan_panic)
+		panic();
+}
+
+void __ubsan_handle_sub_overflow(struct overflow_data *data,
+				 unsigned long lhs __unused,
+				 unsigned long rhs __unused)
+{
+	print_loc(__func__, &data->loc);
+	if (ubsan_panic)
+		panic();
+}
+
+void __ubsan_handle_mul_overflow(struct overflow_data *data,
+				 unsigned long lhs __unused,
+				 unsigned long rhs __unused)
+{
+	print_loc(__func__, &data->loc);
+	if (ubsan_panic)
+		panic();
+}
+
+void __ubsan_handle_negate_overflow(struct overflow_data *data,
+				    unsigned long old_val __unused)
+{
+	print_loc(__func__, &data->loc);
+	if (ubsan_panic)
+		panic();
+}
+
+void __ubsan_handle_divrem_overflow(struct overflow_data *data,
+				    unsigned long lhs __unused,
+				    unsigned long rhs __unused)
+{
+	print_loc(__func__, &data->loc);
+	if (ubsan_panic)
+		panic();
+}
+
+void __ubsan_handle_shift_out_of_bounds(struct shift_out_of_bounds_data *data,
+					unsigned long lhs __unused,
+					unsigned long rhs __unused)
+{
+	print_loc(__func__, &data->loc);
+	if (ubsan_panic)
+		panic();
+}
+
+void __ubsan_handle_out_of_bounds(struct out_of_bounds_data *data,
+				  unsigned long idx __unused)
+{
+	print_loc(__func__, &data->loc);
+	if (ubsan_panic)
+		panic();
+}
+
+void __ubsan_handle_unreachable(struct unreachable_data *data)
+{
+	print_loc(__func__, &data->loc);
+	if (ubsan_panic)
+		panic();
+}
+
+void __noreturn __ubsan_handle_missing_return(struct unreachable_data *data)
+{
+	print_loc(__func__, &data->loc);
+	panic();
+}
+
+void __ubsan_handle_vla_bound_not_positive(struct vla_bound_data *data,
+					   unsigned long bound __unused)
+{
+	print_loc(__func__, &data->loc);
+	if (ubsan_panic)
+		panic();
+}
+
+void __ubsan_handle_load_invalid_value(struct invalid_value_data *data,
+				       unsigned long val __unused)
+{
+	print_loc(__func__, &data->loc);
+	if (ubsan_panic)
+		panic();
+}
+
+void __ubsan_handle_nonnull_arg(struct nonnull_arg_data *data,
+				size_t arg_no __unused)
+{
+	print_loc(__func__, &data->loc);
+	if (ubsan_panic)
+		panic();
+}
diff --git a/core/lib/libfdt/README.license b/core/lib/libfdt/README.license
new file mode 100644
index 0000000..d56c88f
--- /dev/null
+++ b/core/lib/libfdt/README.license
@@ -0,0 +1,56 @@
+Licensing and contribution policy of dtc and libfdt
+===================================================
+
+This dtc package contains two pieces of software: dtc itself, and
+libfdt which comprises the files in the libfdt/ subdirectory.  These
+two pieces of software, although closely related, are quite distinct.
+dtc does not incoporate or rely on libfdt for its operation, nor vice
+versa.  It is important that these two pieces of software have
+different license conditions.
+
+As the copyright banners in each source file attest, dtc is licensed
+under the GNU GPL.  The full text of the GPL can be found in the file
+entitled 'GPL' which should be included in this package.  dtc code,
+therefore, may not be incorporated into works which do not have a GPL
+compatible license.
+
+libfdt, however, is GPL/BSD dual-licensed.  That is, it may be used
+either under the terms of the GPL, or under the terms of the 2-clause
+BSD license (aka the ISC license).  The full terms of that license are
+given in the copyright banners of each of the libfdt source files.
+This is, in practice, equivalent to being BSD licensed, since the
+terms of the BSD license are strictly more permissive than the GPL.
+
+I made the decision to license libfdt in this way because I want to
+encourage widespread and correct usage of flattened device trees,
+including by proprietary or otherwise GPL-incompatible firmware or
+tools.  Allowing libfdt to be used under the terms of the BSD license
+makes that it easier for vendors or authors of such software to do so.
+
+This does mean that libfdt code could be "stolen" - say, included in a
+proprietary fimware and extended without contributing those extensions
+back to the libfdt mainline.  While I hope that doesn't happen, I
+believe the goal of allowing libfdt to be widely used is more
+important than avoiding that.  libfdt is quite small, and hardly
+rocket science; so the incentive for such impolite behaviour is small,
+and the inconvenience caused therby is not dire.
+
+Licenses such as the LGPL which would allow code to be used in non-GPL
+software, but also require contributions to be returned were
+considered.  However, libfdt is designed to be used in firmwares and
+other environments with unusual technical constraints.  It's difficult
+to anticipate all possible changes which might be needed to meld
+libfdt into such environments and so difficult to suitably word a
+license that puts the boundary between what is and isn't permitted in
+the intended place.  Again, I judged encouraging widespread use of
+libfdt by keeping the license terms simple and familiar to be the more
+important goal.
+
+**IMPORTANT** It's intended that all of libfdt as released remain
+permissively licensed this way.  Therefore only contributions which
+are released under these terms can be merged into the libfdt mainline.
+
+
+David Gibson <david@gibson.dropbear.id.au>
+(principal original author of dtc and libfdt)
+2 November 2007
diff --git a/core/lib/libfdt/fdt.c b/core/lib/libfdt/fdt.c
new file mode 100644
index 0000000..2ce6a44
--- /dev/null
+++ b/core/lib/libfdt/fdt.c
@@ -0,0 +1,250 @@
+/*
+ * libfdt - Flat Device Tree manipulation
+ * Copyright (C) 2006 David Gibson, IBM Corporation.
+ *
+ * libfdt is dual licensed: you can use it either under the terms of
+ * the GPL, or the BSD license, at your option.
+ *
+ *  a) This library is free software; you can redistribute it and/or
+ *     modify it under the terms of the GNU General Public License as
+ *     published by the Free Software Foundation; either version 2 of the
+ *     License, or (at your option) any later version.
+ *
+ *     This library is distributed in the hope that it will be useful,
+ *     but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *     GNU General Public License for more details.
+ *
+ *     You should have received a copy of the GNU General Public
+ *     License along with this library; if not, write to the Free
+ *     Software Foundation, Inc., 51 Franklin St, Fifth Floor, Boston,
+ *     MA 02110-1301 USA
+ *
+ * Alternatively,
+ *
+ *  b) 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 OWNER 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 "libfdt_env.h"
+
+#include <fdt.h>
+#include <libfdt.h>
+
+#include "libfdt_internal.h"
+
+int fdt_check_header(const void *fdt)
+{
+	if (fdt_magic(fdt) == FDT_MAGIC) {
+		/* Complete tree */
+		if (fdt_version(fdt) < FDT_FIRST_SUPPORTED_VERSION)
+			return -FDT_ERR_BADVERSION;
+		if (fdt_last_comp_version(fdt) > FDT_LAST_SUPPORTED_VERSION)
+			return -FDT_ERR_BADVERSION;
+	} else if (fdt_magic(fdt) == FDT_SW_MAGIC) {
+		/* Unfinished sequential-write blob */
+		if (fdt_size_dt_struct(fdt) == 0)
+			return -FDT_ERR_BADSTATE;
+	} else {
+		return -FDT_ERR_BADMAGIC;
+	}
+
+	return 0;
+}
+
+const void *fdt_offset_ptr(const void *fdt, int offset, unsigned int len)
+{
+	const char *p;
+
+	if (fdt_version(fdt) >= 0x11)
+		if (((offset + len) < offset)
+		    || ((offset + len) > fdt_size_dt_struct(fdt)))
+			return NULL;
+
+	p = _fdt_offset_ptr(fdt, offset);
+
+	if (p + len < p)
+		return NULL;
+	return p;
+}
+
+uint32_t fdt_next_tag(const void *fdt, int startoffset, int *nextoffset)
+{
+	const fdt32_t *tagp, *lenp;
+	uint32_t tag;
+	int offset = startoffset;
+	const char *p;
+
+	*nextoffset = -FDT_ERR_TRUNCATED;
+	tagp = fdt_offset_ptr(fdt, offset, FDT_TAGSIZE);
+	if (!tagp)
+		return FDT_END; /* premature end */
+	tag = fdt32_to_cpu(*tagp);
+	offset += FDT_TAGSIZE;
+
+	*nextoffset = -FDT_ERR_BADSTRUCTURE;
+	switch (tag) {
+	case FDT_BEGIN_NODE:
+		/* skip name */
+		do {
+			p = fdt_offset_ptr(fdt, offset++, 1);
+		} while (p && (*p != '\0'));
+		if (!p)
+			return FDT_END; /* premature end */
+		break;
+
+	case FDT_PROP:
+		lenp = fdt_offset_ptr(fdt, offset, sizeof(*lenp));
+		if (!lenp)
+			return FDT_END; /* premature end */
+		/* skip-name offset, length and value */
+		offset += sizeof(struct fdt_property) - FDT_TAGSIZE
+			+ fdt32_to_cpu(*lenp);
+		break;
+
+	case FDT_END:
+	case FDT_END_NODE:
+	case FDT_NOP:
+		break;
+
+	default:
+		return FDT_END;
+	}
+
+	if (!fdt_offset_ptr(fdt, startoffset, offset - startoffset))
+		return FDT_END; /* premature end */
+
+	*nextoffset = FDT_TAGALIGN(offset);
+	return tag;
+}
+
+int _fdt_check_node_offset(const void *fdt, int offset)
+{
+	if ((offset < 0) || (offset % FDT_TAGSIZE)
+	    || (fdt_next_tag(fdt, offset, &offset) != FDT_BEGIN_NODE))
+		return -FDT_ERR_BADOFFSET;
+
+	return offset;
+}
+
+int _fdt_check_prop_offset(const void *fdt, int offset)
+{
+	if ((offset < 0) || (offset % FDT_TAGSIZE)
+	    || (fdt_next_tag(fdt, offset, &offset) != FDT_PROP))
+		return -FDT_ERR_BADOFFSET;
+
+	return offset;
+}
+
+int fdt_next_node(const void *fdt, int offset, int *depth)
+{
+	int nextoffset = 0;
+	uint32_t tag;
+
+	if (offset >= 0)
+		if ((nextoffset = _fdt_check_node_offset(fdt, offset)) < 0)
+			return nextoffset;
+
+	do {
+		offset = nextoffset;
+		tag = fdt_next_tag(fdt, offset, &nextoffset);
+
+		switch (tag) {
+		case FDT_PROP:
+		case FDT_NOP:
+			break;
+
+		case FDT_BEGIN_NODE:
+			if (depth)
+				(*depth)++;
+			break;
+
+		case FDT_END_NODE:
+			if (depth && ((--(*depth)) < 0))
+				return nextoffset;
+			break;
+
+		case FDT_END:
+			if ((nextoffset >= 0)
+			    || ((nextoffset == -FDT_ERR_TRUNCATED) && !depth))
+				return -FDT_ERR_NOTFOUND;
+			else
+				return nextoffset;
+		}
+	} while (tag != FDT_BEGIN_NODE);
+
+	return offset;
+}
+
+int fdt_first_subnode(const void *fdt, int offset)
+{
+	int depth = 0;
+
+	offset = fdt_next_node(fdt, offset, &depth);
+	if (offset < 0 || depth != 1)
+		return -FDT_ERR_NOTFOUND;
+
+	return offset;
+}
+
+int fdt_next_subnode(const void *fdt, int offset)
+{
+	int depth = 1;
+
+	/*
+	 * With respect to the parent, the depth of the next subnode will be
+	 * the same as the last.
+	 */
+	do {
+		offset = fdt_next_node(fdt, offset, &depth);
+		if (offset < 0 || depth < 1)
+			return -FDT_ERR_NOTFOUND;
+	} while (depth > 1);
+
+	return offset;
+}
+
+const char *_fdt_find_string(const char *strtab, int tabsize, const char *s)
+{
+	int len = strlen(s) + 1;
+	const char *last = strtab + tabsize - len;
+	const char *p;
+
+	for (p = strtab; p <= last; p++)
+		if (memcmp(p, s, len) == 0)
+			return p;
+	return NULL;
+}
+
+int fdt_move(const void *fdt, void *buf, int bufsize)
+{
+	FDT_CHECK_HEADER(fdt);
+
+	if (fdt_totalsize(fdt) > bufsize)
+		return -FDT_ERR_NOSPACE;
+
+	memmove(buf, fdt, fdt_totalsize(fdt));
+	return 0;
+}
diff --git a/core/lib/libfdt/fdt_addresses.c b/core/lib/libfdt/fdt_addresses.c
new file mode 100644
index 0000000..eff4dbc
--- /dev/null
+++ b/core/lib/libfdt/fdt_addresses.c
@@ -0,0 +1,96 @@
+/*
+ * libfdt - Flat Device Tree manipulation
+ * Copyright (C) 2014 David Gibson <david@gibson.dropbear.id.au>
+ *
+ * libfdt is dual licensed: you can use it either under the terms of
+ * the GPL, or the BSD license, at your option.
+ *
+ *  a) This library is free software; you can redistribute it and/or
+ *     modify it under the terms of the GNU General Public License as
+ *     published by the Free Software Foundation; either version 2 of the
+ *     License, or (at your option) any later version.
+ *
+ *     This library is distributed in the hope that it will be useful,
+ *     but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *     GNU General Public License for more details.
+ *
+ *     You should have received a copy of the GNU General Public
+ *     License along with this library; if not, write to the Free
+ *     Software Foundation, Inc., 51 Franklin St, Fifth Floor, Boston,
+ *     MA 02110-1301 USA
+ *
+ * Alternatively,
+ *
+ *  b) 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 OWNER 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 "libfdt_env.h"
+
+#include <fdt.h>
+#include <libfdt.h>
+
+#include "libfdt_internal.h"
+
+int fdt_address_cells(const void *fdt, int nodeoffset)
+{
+	const fdt32_t *ac;
+	int val;
+	int len;
+
+	ac = fdt_getprop(fdt, nodeoffset, "#address-cells", &len);
+	if (!ac)
+		return 2;
+
+	if (len != sizeof(*ac))
+		return -FDT_ERR_BADNCELLS;
+
+	val = fdt32_to_cpu(*ac);
+	if ((val <= 0) || (val > FDT_MAX_NCELLS))
+		return -FDT_ERR_BADNCELLS;
+
+	return val;
+}
+
+int fdt_size_cells(const void *fdt, int nodeoffset)
+{
+	const fdt32_t *sc;
+	int val;
+	int len;
+
+	sc = fdt_getprop(fdt, nodeoffset, "#size-cells", &len);
+	if (!sc)
+		return 2;
+
+	if (len != sizeof(*sc))
+		return -FDT_ERR_BADNCELLS;
+
+	val = fdt32_to_cpu(*sc);
+	if ((val < 0) || (val > FDT_MAX_NCELLS))
+		return -FDT_ERR_BADNCELLS;
+
+	return val;
+}
diff --git a/core/lib/libfdt/fdt_empty_tree.c b/core/lib/libfdt/fdt_empty_tree.c
new file mode 100644
index 0000000..f72d13b
--- /dev/null
+++ b/core/lib/libfdt/fdt_empty_tree.c
@@ -0,0 +1,84 @@
+/*
+ * libfdt - Flat Device Tree manipulation
+ * Copyright (C) 2012 David Gibson, IBM Corporation.
+ *
+ * libfdt is dual licensed: you can use it either under the terms of
+ * the GPL, or the BSD license, at your option.
+ *
+ *  a) This library is free software; you can redistribute it and/or
+ *     modify it under the terms of the GNU General Public License as
+ *     published by the Free Software Foundation; either version 2 of the
+ *     License, or (at your option) any later version.
+ *
+ *     This library is distributed in the hope that it will be useful,
+ *     but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *     GNU General Public License for more details.
+ *
+ *     You should have received a copy of the GNU General Public
+ *     License along with this library; if not, write to the Free
+ *     Software Foundation, Inc., 51 Franklin St, Fifth Floor, Boston,
+ *     MA 02110-1301 USA
+ *
+ * Alternatively,
+ *
+ *  b) 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 OWNER 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 "libfdt_env.h"
+
+#include <fdt.h>
+#include <libfdt.h>
+
+#include "libfdt_internal.h"
+
+int fdt_create_empty_tree(void *buf, int bufsize)
+{
+	int err;
+
+	err = fdt_create(buf, bufsize);
+	if (err)
+		return err;
+
+	err = fdt_finish_reservemap(buf);
+	if (err)
+		return err;
+
+	err = fdt_begin_node(buf, "");
+	if (err)
+		return err;
+
+	err =  fdt_end_node(buf);
+	if (err)
+		return err;
+
+	err = fdt_finish(buf);
+	if (err)
+		return err;
+
+	return fdt_open_into(buf, buf, bufsize);
+}
+
diff --git a/core/lib/libfdt/fdt_ro.c b/core/lib/libfdt/fdt_ro.c
new file mode 100644
index 0000000..50007f6
--- /dev/null
+++ b/core/lib/libfdt/fdt_ro.c
@@ -0,0 +1,573 @@
+/*
+ * libfdt - Flat Device Tree manipulation
+ * Copyright (C) 2006 David Gibson, IBM Corporation.
+ *
+ * libfdt is dual licensed: you can use it either under the terms of
+ * the GPL, or the BSD license, at your option.
+ *
+ *  a) This library is free software; you can redistribute it and/or
+ *     modify it under the terms of the GNU General Public License as
+ *     published by the Free Software Foundation; either version 2 of the
+ *     License, or (at your option) any later version.
+ *
+ *     This library is distributed in the hope that it will be useful,
+ *     but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *     GNU General Public License for more details.
+ *
+ *     You should have received a copy of the GNU General Public
+ *     License along with this library; if not, write to the Free
+ *     Software Foundation, Inc., 51 Franklin St, Fifth Floor, Boston,
+ *     MA 02110-1301 USA
+ *
+ * Alternatively,
+ *
+ *  b) 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 OWNER 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 "libfdt_env.h"
+
+#include <fdt.h>
+#include <libfdt.h>
+
+#include "libfdt_internal.h"
+
+static int _fdt_nodename_eq(const void *fdt, int offset,
+			    const char *s, int len)
+{
+	const char *p = fdt_offset_ptr(fdt, offset + FDT_TAGSIZE, len+1);
+
+	if (! p)
+		/* short match */
+		return 0;
+
+	if (memcmp(p, s, len) != 0)
+		return 0;
+
+	if (p[len] == '\0')
+		return 1;
+	else if (!memchr(s, '@', len) && (p[len] == '@'))
+		return 1;
+	else
+		return 0;
+}
+
+const char *fdt_string(const void *fdt, int stroffset)
+{
+	return (const char *)fdt + fdt_off_dt_strings(fdt) + stroffset;
+}
+
+static int _fdt_string_eq(const void *fdt, int stroffset,
+			  const char *s, int len)
+{
+	const char *p = fdt_string(fdt, stroffset);
+
+	return (strlen(p) == len) && (memcmp(p, s, len) == 0);
+}
+
+int fdt_get_mem_rsv(const void *fdt, int n, uint64_t *address, uint64_t *size)
+{
+	FDT_CHECK_HEADER(fdt);
+	*address = fdt64_to_cpu(_fdt_mem_rsv(fdt, n)->address);
+	*size = fdt64_to_cpu(_fdt_mem_rsv(fdt, n)->size);
+	return 0;
+}
+
+int fdt_num_mem_rsv(const void *fdt)
+{
+	int i = 0;
+
+	while (fdt64_to_cpu(_fdt_mem_rsv(fdt, i)->size) != 0)
+		i++;
+	return i;
+}
+
+static int _nextprop(const void *fdt, int offset)
+{
+	uint32_t tag;
+	int nextoffset;
+
+	do {
+		tag = fdt_next_tag(fdt, offset, &nextoffset);
+
+		switch (tag) {
+		case FDT_END:
+			if (nextoffset >= 0)
+				return -FDT_ERR_BADSTRUCTURE;
+			else
+				return nextoffset;
+
+		case FDT_PROP:
+			return offset;
+		}
+		offset = nextoffset;
+	} while (tag == FDT_NOP);
+
+	return -FDT_ERR_NOTFOUND;
+}
+
+int fdt_subnode_offset_namelen(const void *fdt, int offset,
+			       const char *name, int namelen)
+{
+	int depth;
+
+	FDT_CHECK_HEADER(fdt);
+
+	for (depth = 0;
+	     (offset >= 0) && (depth >= 0);
+	     offset = fdt_next_node(fdt, offset, &depth))
+		if ((depth == 1)
+		    && _fdt_nodename_eq(fdt, offset, name, namelen))
+			return offset;
+
+	if (depth < 0)
+		return -FDT_ERR_NOTFOUND;
+	return offset; /* error */
+}
+
+int fdt_subnode_offset(const void *fdt, int parentoffset,
+		       const char *name)
+{
+	return fdt_subnode_offset_namelen(fdt, parentoffset, name, strlen(name));
+}
+
+int fdt_path_offset(const void *fdt, const char *path)
+{
+	const char *end = path + strlen(path);
+	const char *p = path;
+	int offset = 0;
+
+	FDT_CHECK_HEADER(fdt);
+
+	/* see if we have an alias */
+	if (*path != '/') {
+		const char *q = strchr(path, '/');
+
+		if (!q)
+			q = end;
+
+		p = fdt_get_alias_namelen(fdt, p, q - p);
+		if (!p)
+			return -FDT_ERR_BADPATH;
+		offset = fdt_path_offset(fdt, p);
+
+		p = q;
+	}
+
+	while (*p) {
+		const char *q;
+
+		while (*p == '/')
+			p++;
+		if (! *p)
+			return offset;
+		q = strchr(p, '/');
+		if (! q)
+			q = end;
+
+		offset = fdt_subnode_offset_namelen(fdt, offset, p, q-p);
+		if (offset < 0)
+			return offset;
+
+		p = q;
+	}
+
+	return offset;
+}
+
+const char *fdt_get_name(const void *fdt, int nodeoffset, int *len)
+{
+	const struct fdt_node_header *nh = _fdt_offset_ptr(fdt, nodeoffset);
+	int err;
+
+	if (((err = fdt_check_header(fdt)) != 0)
+	    || ((err = _fdt_check_node_offset(fdt, nodeoffset)) < 0))
+			goto fail;
+
+	if (len)
+		*len = strlen(nh->name);
+
+	return nh->name;
+
+ fail:
+	if (len)
+		*len = err;
+	return NULL;
+}
+
+int fdt_first_property_offset(const void *fdt, int nodeoffset)
+{
+	int offset;
+
+	if ((offset = _fdt_check_node_offset(fdt, nodeoffset)) < 0)
+		return offset;
+
+	return _nextprop(fdt, offset);
+}
+
+int fdt_next_property_offset(const void *fdt, int offset)
+{
+	if ((offset = _fdt_check_prop_offset(fdt, offset)) < 0)
+		return offset;
+
+	return _nextprop(fdt, offset);
+}
+
+const struct fdt_property *fdt_get_property_by_offset(const void *fdt,
+						      int offset,
+						      int *lenp)
+{
+	int err;
+	const struct fdt_property *prop;
+
+	if ((err = _fdt_check_prop_offset(fdt, offset)) < 0) {
+		if (lenp)
+			*lenp = err;
+		return NULL;
+	}
+
+	prop = _fdt_offset_ptr(fdt, offset);
+
+	if (lenp)
+		*lenp = fdt32_to_cpu(prop->len);
+
+	return prop;
+}
+
+const struct fdt_property *fdt_get_property_namelen(const void *fdt,
+						    int offset,
+						    const char *name,
+						    int namelen, int *lenp)
+{
+	for (offset = fdt_first_property_offset(fdt, offset);
+	     (offset >= 0);
+	     (offset = fdt_next_property_offset(fdt, offset))) {
+		const struct fdt_property *prop;
+
+		if (!(prop = fdt_get_property_by_offset(fdt, offset, lenp))) {
+			offset = -FDT_ERR_INTERNAL;
+			break;
+		}
+		if (_fdt_string_eq(fdt, fdt32_to_cpu(prop->nameoff),
+				   name, namelen))
+			return prop;
+	}
+
+	if (lenp)
+		*lenp = offset;
+	return NULL;
+}
+
+const struct fdt_property *fdt_get_property(const void *fdt,
+					    int nodeoffset,
+					    const char *name, int *lenp)
+{
+	return fdt_get_property_namelen(fdt, nodeoffset, name,
+					strlen(name), lenp);
+}
+
+const void *fdt_getprop_namelen(const void *fdt, int nodeoffset,
+				const char *name, int namelen, int *lenp)
+{
+	const struct fdt_property *prop;
+
+	prop = fdt_get_property_namelen(fdt, nodeoffset, name, namelen, lenp);
+	if (! prop)
+		return NULL;
+
+	return prop->data;
+}
+
+const void *fdt_getprop_by_offset(const void *fdt, int offset,
+				  const char **namep, int *lenp)
+{
+	const struct fdt_property *prop;
+
+	prop = fdt_get_property_by_offset(fdt, offset, lenp);
+	if (!prop)
+		return NULL;
+	if (namep)
+		*namep = fdt_string(fdt, fdt32_to_cpu(prop->nameoff));
+	return prop->data;
+}
+
+const void *fdt_getprop(const void *fdt, int nodeoffset,
+			const char *name, int *lenp)
+{
+	return fdt_getprop_namelen(fdt, nodeoffset, name, strlen(name), lenp);
+}
+
+uint32_t fdt_get_phandle(const void *fdt, int nodeoffset)
+{
+	const fdt32_t *php;
+	int len;
+
+	/* FIXME: This is a bit sub-optimal, since we potentially scan
+	 * over all the properties twice. */
+	php = fdt_getprop(fdt, nodeoffset, "phandle", &len);
+	if (!php || (len != sizeof(*php))) {
+		php = fdt_getprop(fdt, nodeoffset, "linux,phandle", &len);
+		if (!php || (len != sizeof(*php)))
+			return 0;
+	}
+
+	return fdt32_to_cpu(*php);
+}
+
+const char *fdt_get_alias_namelen(const void *fdt,
+				  const char *name, int namelen)
+{
+	int aliasoffset;
+
+	aliasoffset = fdt_path_offset(fdt, "/aliases");
+	if (aliasoffset < 0)
+		return NULL;
+
+	return fdt_getprop_namelen(fdt, aliasoffset, name, namelen, NULL);
+}
+
+const char *fdt_get_alias(const void *fdt, const char *name)
+{
+	return fdt_get_alias_namelen(fdt, name, strlen(name));
+}
+
+int fdt_get_path(const void *fdt, int nodeoffset, char *buf, int buflen)
+{
+	int pdepth = 0, p = 0;
+	int offset, depth, namelen;
+	const char *name;
+
+	FDT_CHECK_HEADER(fdt);
+
+	if (buflen < 2)
+		return -FDT_ERR_NOSPACE;
+
+	for (offset = 0, depth = 0;
+	     (offset >= 0) && (offset <= nodeoffset);
+	     offset = fdt_next_node(fdt, offset, &depth)) {
+		while (pdepth > depth) {
+			do {
+				p--;
+			} while (buf[p-1] != '/');
+			pdepth--;
+		}
+
+		if (pdepth >= depth) {
+			name = fdt_get_name(fdt, offset, &namelen);
+			if (!name)
+				return namelen;
+			if ((p + namelen + 1) <= buflen) {
+				memcpy(buf + p, name, namelen);
+				p += namelen;
+				buf[p++] = '/';
+				pdepth++;
+			}
+		}
+
+		if (offset == nodeoffset) {
+			if (pdepth < (depth + 1))
+				return -FDT_ERR_NOSPACE;
+
+			if (p > 1) /* special case so that root path is "/", not "" */
+				p--;
+			buf[p] = '\0';
+			return 0;
+		}
+	}
+
+	if ((offset == -FDT_ERR_NOTFOUND) || (offset >= 0))
+		return -FDT_ERR_BADOFFSET;
+	else if (offset == -FDT_ERR_BADOFFSET)
+		return -FDT_ERR_BADSTRUCTURE;
+
+	return offset; /* error from fdt_next_node() */
+}
+
+int fdt_supernode_atdepth_offset(const void *fdt, int nodeoffset,
+				 int supernodedepth, int *nodedepth)
+{
+	int offset, depth;
+	int supernodeoffset = -FDT_ERR_INTERNAL;
+
+	FDT_CHECK_HEADER(fdt);
+
+	if (supernodedepth < 0)
+		return -FDT_ERR_NOTFOUND;
+
+	for (offset = 0, depth = 0;
+	     (offset >= 0) && (offset <= nodeoffset);
+	     offset = fdt_next_node(fdt, offset, &depth)) {
+		if (depth == supernodedepth)
+			supernodeoffset = offset;
+
+		if (offset == nodeoffset) {
+			if (nodedepth)
+				*nodedepth = depth;
+
+			if (supernodedepth > depth)
+				return -FDT_ERR_NOTFOUND;
+			else
+				return supernodeoffset;
+		}
+	}
+
+	if ((offset == -FDT_ERR_NOTFOUND) || (offset >= 0))
+		return -FDT_ERR_BADOFFSET;
+	else if (offset == -FDT_ERR_BADOFFSET)
+		return -FDT_ERR_BADSTRUCTURE;
+
+	return offset; /* error from fdt_next_node() */
+}
+
+int fdt_node_depth(const void *fdt, int nodeoffset)
+{
+	int nodedepth;
+	int err;
+
+	err = fdt_supernode_atdepth_offset(fdt, nodeoffset, 0, &nodedepth);
+	if (err)
+		return (err < 0) ? err : -FDT_ERR_INTERNAL;
+	return nodedepth;
+}
+
+int fdt_parent_offset(const void *fdt, int nodeoffset)
+{
+	int nodedepth = fdt_node_depth(fdt, nodeoffset);
+
+	if (nodedepth < 0)
+		return nodedepth;
+	return fdt_supernode_atdepth_offset(fdt, nodeoffset,
+					    nodedepth - 1, NULL);
+}
+
+int fdt_node_offset_by_prop_value(const void *fdt, int startoffset,
+				  const char *propname,
+				  const void *propval, int proplen)
+{
+	int offset;
+	const void *val;
+	int len;
+
+	FDT_CHECK_HEADER(fdt);
+
+	/* FIXME: The algorithm here is pretty horrible: we scan each
+	 * property of a node in fdt_getprop(), then if that didn't
+	 * find what we want, we scan over them again making our way
+	 * to the next node.  Still it's the easiest to implement
+	 * approach; performance can come later. */
+	for (offset = fdt_next_node(fdt, startoffset, NULL);
+	     offset >= 0;
+	     offset = fdt_next_node(fdt, offset, NULL)) {
+		val = fdt_getprop(fdt, offset, propname, &len);
+		if (val && (len == proplen)
+		    && (memcmp(val, propval, len) == 0))
+			return offset;
+	}
+
+	return offset; /* error from fdt_next_node() */
+}
+
+int fdt_node_offset_by_phandle(const void *fdt, uint32_t phandle)
+{
+	int offset;
+
+	if ((phandle == 0) || (phandle == -1))
+		return -FDT_ERR_BADPHANDLE;
+
+	FDT_CHECK_HEADER(fdt);
+
+	/* FIXME: The algorithm here is pretty horrible: we
+	 * potentially scan each property of a node in
+	 * fdt_get_phandle(), then if that didn't find what
+	 * we want, we scan over them again making our way to the next
+	 * node.  Still it's the easiest to implement approach;
+	 * performance can come later. */
+	for (offset = fdt_next_node(fdt, -1, NULL);
+	     offset >= 0;
+	     offset = fdt_next_node(fdt, offset, NULL)) {
+		if (fdt_get_phandle(fdt, offset) == phandle)
+			return offset;
+	}
+
+	return offset; /* error from fdt_next_node() */
+}
+
+int fdt_stringlist_contains(const char *strlist, int listlen, const char *str)
+{
+	int len = strlen(str);
+	const char *p;
+
+	while (listlen >= len) {
+		if (memcmp(str, strlist, len+1) == 0)
+			return 1;
+		p = memchr(strlist, '\0', listlen);
+		if (!p)
+			return 0; /* malformed strlist.. */
+		listlen -= (p-strlist) + 1;
+		strlist = p + 1;
+	}
+	return 0;
+}
+
+int fdt_node_check_compatible(const void *fdt, int nodeoffset,
+			      const char *compatible)
+{
+	const void *prop;
+	int len;
+
+	prop = fdt_getprop(fdt, nodeoffset, "compatible", &len);
+	if (!prop)
+		return len;
+	if (fdt_stringlist_contains(prop, len, compatible))
+		return 0;
+	else
+		return 1;
+}
+
+int fdt_node_offset_by_compatible(const void *fdt, int startoffset,
+				  const char *compatible)
+{
+	int offset, err;
+
+	FDT_CHECK_HEADER(fdt);
+
+	/* FIXME: The algorithm here is pretty horrible: we scan each
+	 * property of a node in fdt_node_check_compatible(), then if
+	 * that didn't find what we want, we scan over them again
+	 * making our way to the next node.  Still it's the easiest to
+	 * implement approach; performance can come later. */
+	for (offset = fdt_next_node(fdt, startoffset, NULL);
+	     offset >= 0;
+	     offset = fdt_next_node(fdt, offset, NULL)) {
+		err = fdt_node_check_compatible(fdt, offset, compatible);
+		if ((err < 0) && (err != -FDT_ERR_NOTFOUND))
+			return err;
+		else if (err == 0)
+			return offset;
+	}
+
+	return offset; /* error from fdt_next_node() */
+}
diff --git a/core/lib/libfdt/fdt_rw.c b/core/lib/libfdt/fdt_rw.c
new file mode 100644
index 0000000..09b1fdc
--- /dev/null
+++ b/core/lib/libfdt/fdt_rw.c
@@ -0,0 +1,493 @@
+/*
+ * libfdt - Flat Device Tree manipulation
+ * Copyright (C) 2006 David Gibson, IBM Corporation.
+ *
+ * libfdt is dual licensed: you can use it either under the terms of
+ * the GPL, or the BSD license, at your option.
+ *
+ *  a) This library is free software; you can redistribute it and/or
+ *     modify it under the terms of the GNU General Public License as
+ *     published by the Free Software Foundation; either version 2 of the
+ *     License, or (at your option) any later version.
+ *
+ *     This library is distributed in the hope that it will be useful,
+ *     but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *     GNU General Public License for more details.
+ *
+ *     You should have received a copy of the GNU General Public
+ *     License along with this library; if not, write to the Free
+ *     Software Foundation, Inc., 51 Franklin St, Fifth Floor, Boston,
+ *     MA 02110-1301 USA
+ *
+ * Alternatively,
+ *
+ *  b) 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 OWNER 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 "libfdt_env.h"
+
+#include <fdt.h>
+#include <libfdt.h>
+
+#include "libfdt_internal.h"
+
+static int _fdt_blocks_misordered(const void *fdt,
+			      int mem_rsv_size, int struct_size)
+{
+	return (fdt_off_mem_rsvmap(fdt) < FDT_ALIGN(sizeof(struct fdt_header), 8))
+		|| (fdt_off_dt_struct(fdt) <
+		    (fdt_off_mem_rsvmap(fdt) + mem_rsv_size))
+		|| (fdt_off_dt_strings(fdt) <
+		    (fdt_off_dt_struct(fdt) + struct_size))
+		|| (fdt_totalsize(fdt) <
+		    (fdt_off_dt_strings(fdt) + fdt_size_dt_strings(fdt)));
+}
+
+static int _fdt_rw_check_header(void *fdt)
+{
+	FDT_CHECK_HEADER(fdt);
+
+	if (fdt_version(fdt) < 17)
+		return -FDT_ERR_BADVERSION;
+	if (_fdt_blocks_misordered(fdt, sizeof(struct fdt_reserve_entry),
+				   fdt_size_dt_struct(fdt)))
+		return -FDT_ERR_BADLAYOUT;
+	if (fdt_version(fdt) > 17)
+		fdt_set_version(fdt, 17);
+
+	return 0;
+}
+
+#define FDT_RW_CHECK_HEADER(fdt) \
+	{ \
+		int __err; \
+		if ((__err = _fdt_rw_check_header(fdt)) != 0) \
+			return __err; \
+	}
+
+static inline int _fdt_data_size(void *fdt)
+{
+	return fdt_off_dt_strings(fdt) + fdt_size_dt_strings(fdt);
+}
+
+static int _fdt_splice(void *fdt, void *splicepoint, int oldlen, int newlen)
+{
+	char *p = splicepoint;
+	char *end = (char *)fdt + _fdt_data_size(fdt);
+
+	if (((p + oldlen) < p) || ((p + oldlen) > end))
+		return -FDT_ERR_BADOFFSET;
+	if ((end - oldlen + newlen) > ((char *)fdt + fdt_totalsize(fdt)))
+		return -FDT_ERR_NOSPACE;
+	memmove(p + newlen, p + oldlen, end - p - oldlen);
+	return 0;
+}
+
+static int _fdt_splice_mem_rsv(void *fdt, struct fdt_reserve_entry *p,
+			       int oldn, int newn)
+{
+	int delta = (newn - oldn) * sizeof(*p);
+	int err;
+	err = _fdt_splice(fdt, p, oldn * sizeof(*p), newn * sizeof(*p));
+	if (err)
+		return err;
+	fdt_set_off_dt_struct(fdt, fdt_off_dt_struct(fdt) + delta);
+	fdt_set_off_dt_strings(fdt, fdt_off_dt_strings(fdt) + delta);
+	return 0;
+}
+
+static int _fdt_splice_struct(void *fdt, void *p,
+			      int oldlen, int newlen)
+{
+	int delta = newlen - oldlen;
+	int err;
+
+	if ((err = _fdt_splice(fdt, p, oldlen, newlen)))
+		return err;
+
+	fdt_set_size_dt_struct(fdt, fdt_size_dt_struct(fdt) + delta);
+	fdt_set_off_dt_strings(fdt, fdt_off_dt_strings(fdt) + delta);
+	return 0;
+}
+
+static int _fdt_splice_string(void *fdt, int newlen)
+{
+	void *p = (char *)fdt
+		+ fdt_off_dt_strings(fdt) + fdt_size_dt_strings(fdt);
+	int err;
+
+	if ((err = _fdt_splice(fdt, p, 0, newlen)))
+		return err;
+
+	fdt_set_size_dt_strings(fdt, fdt_size_dt_strings(fdt) + newlen);
+	return 0;
+}
+
+static int _fdt_find_add_string(void *fdt, const char *s)
+{
+	char *strtab = (char *)fdt + fdt_off_dt_strings(fdt);
+	const char *p;
+	char *new;
+	int len = strlen(s) + 1;
+	int err;
+
+	p = _fdt_find_string(strtab, fdt_size_dt_strings(fdt), s);
+	if (p)
+		/* found it */
+		return (p - strtab);
+
+	new = strtab + fdt_size_dt_strings(fdt);
+	err = _fdt_splice_string(fdt, len);
+	if (err)
+		return err;
+
+	memcpy(new, s, len);
+	return (new - strtab);
+}
+
+int fdt_add_mem_rsv(void *fdt, uint64_t address, uint64_t size)
+{
+	struct fdt_reserve_entry *re;
+	int err;
+
+	FDT_RW_CHECK_HEADER(fdt);
+
+	re = _fdt_mem_rsv_w(fdt, fdt_num_mem_rsv(fdt));
+	err = _fdt_splice_mem_rsv(fdt, re, 0, 1);
+	if (err)
+		return err;
+
+	re->address = cpu_to_fdt64(address);
+	re->size = cpu_to_fdt64(size);
+	return 0;
+}
+
+int fdt_del_mem_rsv(void *fdt, int n)
+{
+	struct fdt_reserve_entry *re = _fdt_mem_rsv_w(fdt, n);
+	int err;
+
+	FDT_RW_CHECK_HEADER(fdt);
+
+	if (n >= fdt_num_mem_rsv(fdt))
+		return -FDT_ERR_NOTFOUND;
+
+	err = _fdt_splice_mem_rsv(fdt, re, 1, 0);
+	if (err)
+		return err;
+	return 0;
+}
+
+static int _fdt_resize_property(void *fdt, int nodeoffset, const char *name,
+				int len, struct fdt_property **prop)
+{
+	int oldlen;
+	int err;
+
+	*prop = fdt_get_property_w(fdt, nodeoffset, name, &oldlen);
+	if (! (*prop))
+		return oldlen;
+
+	if ((err = _fdt_splice_struct(fdt, (*prop)->data, FDT_TAGALIGN(oldlen),
+				      FDT_TAGALIGN(len))))
+		return err;
+
+	(*prop)->len = cpu_to_fdt32(len);
+	return 0;
+}
+
+static int _fdt_add_property(void *fdt, int nodeoffset, const char *name,
+			     int len, struct fdt_property **prop)
+{
+	int proplen;
+	int nextoffset;
+	int namestroff;
+	int err;
+
+	if ((nextoffset = _fdt_check_node_offset(fdt, nodeoffset)) < 0)
+		return nextoffset;
+
+	namestroff = _fdt_find_add_string(fdt, name);
+	if (namestroff < 0)
+		return namestroff;
+
+	*prop = _fdt_offset_ptr_w(fdt, nextoffset);
+	proplen = sizeof(**prop) + FDT_TAGALIGN(len);
+
+	err = _fdt_splice_struct(fdt, *prop, 0, proplen);
+	if (err)
+		return err;
+
+	(*prop)->tag = cpu_to_fdt32(FDT_PROP);
+	(*prop)->nameoff = cpu_to_fdt32(namestroff);
+	(*prop)->len = cpu_to_fdt32(len);
+	return 0;
+}
+
+int fdt_set_name(void *fdt, int nodeoffset, const char *name)
+{
+	char *namep;
+	int oldlen, newlen;
+	int err;
+
+	FDT_RW_CHECK_HEADER(fdt);
+
+	namep = (char *)(uintptr_t)fdt_get_name(fdt, nodeoffset, &oldlen);
+	if (!namep)
+		return oldlen;
+
+	newlen = strlen(name);
+
+	err = _fdt_splice_struct(fdt, namep, FDT_TAGALIGN(oldlen+1),
+				 FDT_TAGALIGN(newlen+1));
+	if (err)
+		return err;
+
+	memcpy(namep, name, newlen+1);
+	return 0;
+}
+
+int fdt_setprop(void *fdt, int nodeoffset, const char *name,
+		const void *val, int len)
+{
+	struct fdt_property *prop;
+	int err;
+
+	FDT_RW_CHECK_HEADER(fdt);
+
+	err = _fdt_resize_property(fdt, nodeoffset, name, len, &prop);
+	if (err == -FDT_ERR_NOTFOUND)
+		err = _fdt_add_property(fdt, nodeoffset, name, len, &prop);
+	if (err)
+		return err;
+
+	if (len)
+		memcpy(prop->data, val, len);
+	return 0;
+}
+
+int fdt_appendprop(void *fdt, int nodeoffset, const char *name,
+		   const void *val, int len)
+{
+	struct fdt_property *prop;
+	int err, oldlen, newlen;
+
+	FDT_RW_CHECK_HEADER(fdt);
+
+	prop = fdt_get_property_w(fdt, nodeoffset, name, &oldlen);
+	if (prop) {
+		newlen = len + oldlen;
+		err = _fdt_splice_struct(fdt, prop->data,
+					 FDT_TAGALIGN(oldlen),
+					 FDT_TAGALIGN(newlen));
+		if (err)
+			return err;
+		prop->len = cpu_to_fdt32(newlen);
+		memcpy(prop->data + oldlen, val, len);
+	} else {
+		err = _fdt_add_property(fdt, nodeoffset, name, len, &prop);
+		if (err)
+			return err;
+		memcpy(prop->data, val, len);
+	}
+	return 0;
+}
+
+int fdt_delprop(void *fdt, int nodeoffset, const char *name)
+{
+	struct fdt_property *prop;
+	int len, proplen;
+
+	FDT_RW_CHECK_HEADER(fdt);
+
+	prop = fdt_get_property_w(fdt, nodeoffset, name, &len);
+	if (! prop)
+		return len;
+
+	proplen = sizeof(*prop) + FDT_TAGALIGN(len);
+	return _fdt_splice_struct(fdt, prop, proplen, 0);
+}
+
+int fdt_add_subnode_namelen(void *fdt, int parentoffset,
+			    const char *name, int namelen)
+{
+	struct fdt_node_header *nh;
+	int offset, nextoffset;
+	int nodelen;
+	int err;
+	uint32_t tag;
+	fdt32_t *endtag;
+
+	FDT_RW_CHECK_HEADER(fdt);
+
+	offset = fdt_subnode_offset_namelen(fdt, parentoffset, name, namelen);
+	if (offset >= 0)
+		return -FDT_ERR_EXISTS;
+	else if (offset != -FDT_ERR_NOTFOUND)
+		return offset;
+
+	/* Try to place the new node after the parent's properties */
+	fdt_next_tag(fdt, parentoffset, &nextoffset); /* skip the BEGIN_NODE */
+	do {
+		offset = nextoffset;
+		tag = fdt_next_tag(fdt, offset, &nextoffset);
+	} while ((tag == FDT_PROP) || (tag == FDT_NOP));
+
+	nh = _fdt_offset_ptr_w(fdt, offset);
+	nodelen = sizeof(*nh) + FDT_TAGALIGN(namelen+1) + FDT_TAGSIZE;
+
+	err = _fdt_splice_struct(fdt, nh, 0, nodelen);
+	if (err)
+		return err;
+
+	nh->tag = cpu_to_fdt32(FDT_BEGIN_NODE);
+	memset(nh->name, 0, FDT_TAGALIGN(namelen+1));
+	memcpy(nh->name, name, namelen);
+	endtag = (fdt32_t *)((char *)nh + nodelen - FDT_TAGSIZE);
+	*endtag = cpu_to_fdt32(FDT_END_NODE);
+
+	return offset;
+}
+
+int fdt_add_subnode(void *fdt, int parentoffset, const char *name)
+{
+	return fdt_add_subnode_namelen(fdt, parentoffset, name, strlen(name));
+}
+
+int fdt_del_node(void *fdt, int nodeoffset)
+{
+	int endoffset;
+
+	FDT_RW_CHECK_HEADER(fdt);
+
+	endoffset = _fdt_node_end_offset(fdt, nodeoffset);
+	if (endoffset < 0)
+		return endoffset;
+
+	return _fdt_splice_struct(fdt, _fdt_offset_ptr_w(fdt, nodeoffset),
+				  endoffset - nodeoffset, 0);
+}
+
+static void _fdt_packblocks(const char *old, char *new,
+			    int mem_rsv_size, int struct_size)
+{
+	int mem_rsv_off, struct_off, strings_off;
+
+	mem_rsv_off = FDT_ALIGN(sizeof(struct fdt_header), 8);
+	struct_off = mem_rsv_off + mem_rsv_size;
+	strings_off = struct_off + struct_size;
+
+	memmove(new + mem_rsv_off, old + fdt_off_mem_rsvmap(old), mem_rsv_size);
+	fdt_set_off_mem_rsvmap(new, mem_rsv_off);
+
+	memmove(new + struct_off, old + fdt_off_dt_struct(old), struct_size);
+	fdt_set_off_dt_struct(new, struct_off);
+	fdt_set_size_dt_struct(new, struct_size);
+
+	memmove(new + strings_off, old + fdt_off_dt_strings(old),
+		fdt_size_dt_strings(old));
+	fdt_set_off_dt_strings(new, strings_off);
+	fdt_set_size_dt_strings(new, fdt_size_dt_strings(old));
+}
+
+int fdt_open_into(const void *fdt, void *buf, int bufsize)
+{
+	int err;
+	int mem_rsv_size, struct_size;
+	int newsize;
+	const char *fdtstart = fdt;
+	const char *fdtend = fdtstart + fdt_totalsize(fdt);
+	char *tmp;
+
+	FDT_CHECK_HEADER(fdt);
+
+	mem_rsv_size = (fdt_num_mem_rsv(fdt)+1)
+		* sizeof(struct fdt_reserve_entry);
+
+	if (fdt_version(fdt) >= 17) {
+		struct_size = fdt_size_dt_struct(fdt);
+	} else {
+		struct_size = 0;
+		while (fdt_next_tag(fdt, struct_size, &struct_size) != FDT_END)
+			;
+		if (struct_size < 0)
+			return struct_size;
+	}
+
+	if (!_fdt_blocks_misordered(fdt, mem_rsv_size, struct_size)) {
+		/* no further work necessary */
+		err = fdt_move(fdt, buf, bufsize);
+		if (err)
+			return err;
+		fdt_set_version(buf, 17);
+		fdt_set_size_dt_struct(buf, struct_size);
+		fdt_set_totalsize(buf, bufsize);
+		return 0;
+	}
+
+	/* Need to reorder */
+	newsize = FDT_ALIGN(sizeof(struct fdt_header), 8) + mem_rsv_size
+		+ struct_size + fdt_size_dt_strings(fdt);
+
+	if (bufsize < newsize)
+		return -FDT_ERR_NOSPACE;
+
+	/* First attempt to build converted tree at beginning of buffer */
+	tmp = buf;
+	/* But if that overlaps with the old tree... */
+	if (((tmp + newsize) > fdtstart) && (tmp < fdtend)) {
+		/* Try right after the old tree instead */
+		tmp = (char *)(uintptr_t)fdtend;
+		if ((tmp + newsize) > ((char *)buf + bufsize))
+			return -FDT_ERR_NOSPACE;
+	}
+
+	_fdt_packblocks(fdt, tmp, mem_rsv_size, struct_size);
+	memmove(buf, tmp, newsize);
+
+	fdt_set_magic(buf, FDT_MAGIC);
+	fdt_set_totalsize(buf, bufsize);
+	fdt_set_version(buf, 17);
+	fdt_set_last_comp_version(buf, 16);
+	fdt_set_boot_cpuid_phys(buf, fdt_boot_cpuid_phys(fdt));
+
+	return 0;
+}
+
+int fdt_pack(void *fdt)
+{
+	int mem_rsv_size;
+
+	FDT_RW_CHECK_HEADER(fdt);
+
+	mem_rsv_size = (fdt_num_mem_rsv(fdt)+1)
+		* sizeof(struct fdt_reserve_entry);
+	_fdt_packblocks(fdt, fdt, mem_rsv_size, fdt_size_dt_struct(fdt));
+	fdt_set_totalsize(fdt, _fdt_data_size(fdt));
+
+	return 0;
+}
diff --git a/core/lib/libfdt/fdt_strerror.c b/core/lib/libfdt/fdt_strerror.c
new file mode 100644
index 0000000..e6c3cee
--- /dev/null
+++ b/core/lib/libfdt/fdt_strerror.c
@@ -0,0 +1,96 @@
+/*
+ * libfdt - Flat Device Tree manipulation
+ * Copyright (C) 2006 David Gibson, IBM Corporation.
+ *
+ * libfdt is dual licensed: you can use it either under the terms of
+ * the GPL, or the BSD license, at your option.
+ *
+ *  a) This library is free software; you can redistribute it and/or
+ *     modify it under the terms of the GNU General Public License as
+ *     published by the Free Software Foundation; either version 2 of the
+ *     License, or (at your option) any later version.
+ *
+ *     This library is distributed in the hope that it will be useful,
+ *     but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *     GNU General Public License for more details.
+ *
+ *     You should have received a copy of the GNU General Public
+ *     License along with this library; if not, write to the Free
+ *     Software Foundation, Inc., 51 Franklin St, Fifth Floor, Boston,
+ *     MA 02110-1301 USA
+ *
+ * Alternatively,
+ *
+ *  b) 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 OWNER 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 "libfdt_env.h"
+
+#include <fdt.h>
+#include <libfdt.h>
+
+#include "libfdt_internal.h"
+
+struct fdt_errtabent {
+	const char *str;
+};
+
+#define FDT_ERRTABENT(val) \
+	[(val)] = { .str = #val, }
+
+static struct fdt_errtabent fdt_errtable[] = {
+	FDT_ERRTABENT(FDT_ERR_NOTFOUND),
+	FDT_ERRTABENT(FDT_ERR_EXISTS),
+	FDT_ERRTABENT(FDT_ERR_NOSPACE),
+
+	FDT_ERRTABENT(FDT_ERR_BADOFFSET),
+	FDT_ERRTABENT(FDT_ERR_BADPATH),
+	FDT_ERRTABENT(FDT_ERR_BADSTATE),
+
+	FDT_ERRTABENT(FDT_ERR_TRUNCATED),
+	FDT_ERRTABENT(FDT_ERR_BADMAGIC),
+	FDT_ERRTABENT(FDT_ERR_BADVERSION),
+	FDT_ERRTABENT(FDT_ERR_BADSTRUCTURE),
+	FDT_ERRTABENT(FDT_ERR_BADLAYOUT),
+};
+#define FDT_ERRTABSIZE	(sizeof(fdt_errtable) / sizeof(fdt_errtable[0]))
+
+const char *fdt_strerror(int errval)
+{
+	if (errval > 0)
+		return "<valid offset/length>";
+	else if (errval == 0)
+		return "<no error>";
+	else if (errval > -FDT_ERRTABSIZE) {
+		const char *s = fdt_errtable[-errval].str;
+
+		if (s)
+			return s;
+	}
+
+	return "<unknown error>";
+}
diff --git a/core/lib/libfdt/fdt_sw.c b/core/lib/libfdt/fdt_sw.c
new file mode 100644
index 0000000..6a80485
--- /dev/null
+++ b/core/lib/libfdt/fdt_sw.c
@@ -0,0 +1,288 @@
+/*
+ * libfdt - Flat Device Tree manipulation
+ * Copyright (C) 2006 David Gibson, IBM Corporation.
+ *
+ * libfdt is dual licensed: you can use it either under the terms of
+ * the GPL, or the BSD license, at your option.
+ *
+ *  a) This library is free software; you can redistribute it and/or
+ *     modify it under the terms of the GNU General Public License as
+ *     published by the Free Software Foundation; either version 2 of the
+ *     License, or (at your option) any later version.
+ *
+ *     This library is distributed in the hope that it will be useful,
+ *     but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *     GNU General Public License for more details.
+ *
+ *     You should have received a copy of the GNU General Public
+ *     License along with this library; if not, write to the Free
+ *     Software Foundation, Inc., 51 Franklin St, Fifth Floor, Boston,
+ *     MA 02110-1301 USA
+ *
+ * Alternatively,
+ *
+ *  b) 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 OWNER 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 "libfdt_env.h"
+
+#include <fdt.h>
+#include <libfdt.h>
+
+#include "libfdt_internal.h"
+
+static int _fdt_sw_check_header(void *fdt)
+{
+	if (fdt_magic(fdt) != FDT_SW_MAGIC)
+		return -FDT_ERR_BADMAGIC;
+	/* FIXME: should check more details about the header state */
+	return 0;
+}
+
+#define FDT_SW_CHECK_HEADER(fdt) \
+	{ \
+		int err; \
+		if ((err = _fdt_sw_check_header(fdt)) != 0) \
+			return err; \
+	}
+
+static void *_fdt_grab_space(void *fdt, size_t len)
+{
+	int offset = fdt_size_dt_struct(fdt);
+	int spaceleft;
+
+	spaceleft = fdt_totalsize(fdt) - fdt_off_dt_struct(fdt)
+		- fdt_size_dt_strings(fdt);
+
+	if ((offset + len < offset) || (offset + len > spaceleft))
+		return NULL;
+
+	fdt_set_size_dt_struct(fdt, offset + len);
+	return _fdt_offset_ptr_w(fdt, offset);
+}
+
+int fdt_create(void *buf, int bufsize)
+{
+	void *fdt = buf;
+
+	if (bufsize < sizeof(struct fdt_header))
+		return -FDT_ERR_NOSPACE;
+
+	memset(buf, 0, bufsize);
+
+	fdt_set_magic(fdt, FDT_SW_MAGIC);
+	fdt_set_version(fdt, FDT_LAST_SUPPORTED_VERSION);
+	fdt_set_last_comp_version(fdt, FDT_FIRST_SUPPORTED_VERSION);
+	fdt_set_totalsize(fdt,  bufsize);
+
+	fdt_set_off_mem_rsvmap(fdt, FDT_ALIGN(sizeof(struct fdt_header),
+					      sizeof(struct fdt_reserve_entry)));
+	fdt_set_off_dt_struct(fdt, fdt_off_mem_rsvmap(fdt));
+	fdt_set_off_dt_strings(fdt, bufsize);
+
+	return 0;
+}
+
+int fdt_resize(void *fdt, void *buf, int bufsize)
+{
+	size_t headsize, tailsize;
+	char *oldtail, *newtail;
+
+	FDT_SW_CHECK_HEADER(fdt);
+
+	headsize = fdt_off_dt_struct(fdt);
+	tailsize = fdt_size_dt_strings(fdt);
+
+	if ((headsize + tailsize) > bufsize)
+		return -FDT_ERR_NOSPACE;
+
+	oldtail = (char *)fdt + fdt_totalsize(fdt) - tailsize;
+	newtail = (char *)buf + bufsize - tailsize;
+
+	/* Two cases to avoid clobbering data if the old and new
+	 * buffers partially overlap */
+	if (buf <= fdt) {
+		memmove(buf, fdt, headsize);
+		memmove(newtail, oldtail, tailsize);
+	} else {
+		memmove(newtail, oldtail, tailsize);
+		memmove(buf, fdt, headsize);
+	}
+
+	fdt_set_off_dt_strings(buf, bufsize);
+	fdt_set_totalsize(buf, bufsize);
+
+	return 0;
+}
+
+int fdt_add_reservemap_entry(void *fdt, uint64_t addr, uint64_t size)
+{
+	struct fdt_reserve_entry *re;
+	int offset;
+
+	FDT_SW_CHECK_HEADER(fdt);
+
+	if (fdt_size_dt_struct(fdt))
+		return -FDT_ERR_BADSTATE;
+
+	offset = fdt_off_dt_struct(fdt);
+	if ((offset + sizeof(*re)) > fdt_totalsize(fdt))
+		return -FDT_ERR_NOSPACE;
+
+	re = (struct fdt_reserve_entry *)((char *)fdt + offset);
+	re->address = cpu_to_fdt64(addr);
+	re->size = cpu_to_fdt64(size);
+
+	fdt_set_off_dt_struct(fdt, offset + sizeof(*re));
+
+	return 0;
+}
+
+int fdt_finish_reservemap(void *fdt)
+{
+	return fdt_add_reservemap_entry(fdt, 0, 0);
+}
+
+int fdt_begin_node(void *fdt, const char *name)
+{
+	struct fdt_node_header *nh;
+	int namelen = strlen(name) + 1;
+
+	FDT_SW_CHECK_HEADER(fdt);
+
+	nh = _fdt_grab_space(fdt, sizeof(*nh) + FDT_TAGALIGN(namelen));
+	if (! nh)
+		return -FDT_ERR_NOSPACE;
+
+	nh->tag = cpu_to_fdt32(FDT_BEGIN_NODE);
+	memcpy(nh->name, name, namelen);
+	return 0;
+}
+
+int fdt_end_node(void *fdt)
+{
+	fdt32_t *en;
+
+	FDT_SW_CHECK_HEADER(fdt);
+
+	en = _fdt_grab_space(fdt, FDT_TAGSIZE);
+	if (! en)
+		return -FDT_ERR_NOSPACE;
+
+	*en = cpu_to_fdt32(FDT_END_NODE);
+	return 0;
+}
+
+static int _fdt_find_add_string(void *fdt, const char *s)
+{
+	char *strtab = (char *)fdt + fdt_totalsize(fdt);
+	const char *p;
+	int strtabsize = fdt_size_dt_strings(fdt);
+	int len = strlen(s) + 1;
+	int struct_top, offset;
+
+	p = _fdt_find_string(strtab - strtabsize, strtabsize, s);
+	if (p)
+		return p - strtab;
+
+	/* Add it */
+	offset = -strtabsize - len;
+	struct_top = fdt_off_dt_struct(fdt) + fdt_size_dt_struct(fdt);
+	if (fdt_totalsize(fdt) + offset < struct_top)
+		return 0; /* no more room :( */
+
+	memcpy(strtab + offset, s, len);
+	fdt_set_size_dt_strings(fdt, strtabsize + len);
+	return offset;
+}
+
+int fdt_property(void *fdt, const char *name, const void *val, int len)
+{
+	struct fdt_property *prop;
+	int nameoff;
+
+	FDT_SW_CHECK_HEADER(fdt);
+
+	nameoff = _fdt_find_add_string(fdt, name);
+	if (nameoff == 0)
+		return -FDT_ERR_NOSPACE;
+
+	prop = _fdt_grab_space(fdt, sizeof(*prop) + FDT_TAGALIGN(len));
+	if (! prop)
+		return -FDT_ERR_NOSPACE;
+
+	prop->tag = cpu_to_fdt32(FDT_PROP);
+	prop->nameoff = cpu_to_fdt32(nameoff);
+	prop->len = cpu_to_fdt32(len);
+	memcpy(prop->data, val, len);
+	return 0;
+}
+
+int fdt_finish(void *fdt)
+{
+	char *p = (char *)fdt;
+	fdt32_t *end;
+	int oldstroffset, newstroffset;
+	uint32_t tag;
+	int offset, nextoffset;
+
+	FDT_SW_CHECK_HEADER(fdt);
+
+	/* Add terminator */
+	end = _fdt_grab_space(fdt, sizeof(*end));
+	if (! end)
+		return -FDT_ERR_NOSPACE;
+	*end = cpu_to_fdt32(FDT_END);
+
+	/* Relocate the string table */
+	oldstroffset = fdt_totalsize(fdt) - fdt_size_dt_strings(fdt);
+	newstroffset = fdt_off_dt_struct(fdt) + fdt_size_dt_struct(fdt);
+	memmove(p + newstroffset, p + oldstroffset, fdt_size_dt_strings(fdt));
+	fdt_set_off_dt_strings(fdt, newstroffset);
+
+	/* Walk the structure, correcting string offsets */
+	offset = 0;
+	while ((tag = fdt_next_tag(fdt, offset, &nextoffset)) != FDT_END) {
+		if (tag == FDT_PROP) {
+			struct fdt_property *prop =
+				_fdt_offset_ptr_w(fdt, offset);
+			int nameoff;
+
+			nameoff = fdt32_to_cpu(prop->nameoff);
+			nameoff += fdt_size_dt_strings(fdt);
+			prop->nameoff = cpu_to_fdt32(nameoff);
+		}
+		offset = nextoffset;
+	}
+	if (nextoffset < 0)
+		return nextoffset;
+
+	/* Finally, adjust the header */
+	fdt_set_totalsize(fdt, newstroffset + fdt_size_dt_strings(fdt));
+	fdt_set_magic(fdt, FDT_MAGIC);
+	return 0;
+}
diff --git a/core/lib/libfdt/fdt_wip.c b/core/lib/libfdt/fdt_wip.c
new file mode 100644
index 0000000..c5bbb68
--- /dev/null
+++ b/core/lib/libfdt/fdt_wip.c
@@ -0,0 +1,118 @@
+/*
+ * libfdt - Flat Device Tree manipulation
+ * Copyright (C) 2006 David Gibson, IBM Corporation.
+ *
+ * libfdt is dual licensed: you can use it either under the terms of
+ * the GPL, or the BSD license, at your option.
+ *
+ *  a) This library is free software; you can redistribute it and/or
+ *     modify it under the terms of the GNU General Public License as
+ *     published by the Free Software Foundation; either version 2 of the
+ *     License, or (at your option) any later version.
+ *
+ *     This library is distributed in the hope that it will be useful,
+ *     but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *     GNU General Public License for more details.
+ *
+ *     You should have received a copy of the GNU General Public
+ *     License along with this library; if not, write to the Free
+ *     Software Foundation, Inc., 51 Franklin St, Fifth Floor, Boston,
+ *     MA 02110-1301 USA
+ *
+ * Alternatively,
+ *
+ *  b) 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 OWNER 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 "libfdt_env.h"
+
+#include <fdt.h>
+#include <libfdt.h>
+
+#include "libfdt_internal.h"
+
+int fdt_setprop_inplace(void *fdt, int nodeoffset, const char *name,
+			const void *val, int len)
+{
+	void *propval;
+	int proplen;
+
+	propval = fdt_getprop_w(fdt, nodeoffset, name, &proplen);
+	if (! propval)
+		return proplen;
+
+	if (proplen != len)
+		return -FDT_ERR_NOSPACE;
+
+	memcpy(propval, val, len);
+	return 0;
+}
+
+static void _fdt_nop_region(void *start, int len)
+{
+	fdt32_t *p;
+
+	for (p = start; (char *)p < ((char *)start + len); p++)
+		*p = cpu_to_fdt32(FDT_NOP);
+}
+
+int fdt_nop_property(void *fdt, int nodeoffset, const char *name)
+{
+	struct fdt_property *prop;
+	int len;
+
+	prop = fdt_get_property_w(fdt, nodeoffset, name, &len);
+	if (! prop)
+		return len;
+
+	_fdt_nop_region(prop, len + sizeof(*prop));
+
+	return 0;
+}
+
+int _fdt_node_end_offset(void *fdt, int offset)
+{
+	int depth = 0;
+
+	while ((offset >= 0) && (depth >= 0))
+		offset = fdt_next_node(fdt, offset, &depth);
+
+	return offset;
+}
+
+int fdt_nop_node(void *fdt, int nodeoffset)
+{
+	int endoffset;
+
+	endoffset = _fdt_node_end_offset(fdt, nodeoffset);
+	if (endoffset < 0)
+		return endoffset;
+
+	_fdt_nop_region(fdt_offset_ptr_w(fdt, nodeoffset, 0),
+			endoffset - nodeoffset);
+	return 0;
+}
diff --git a/core/lib/libfdt/include/fdt.h b/core/lib/libfdt/include/fdt.h
new file mode 100644
index 0000000..d3f4d2f
--- /dev/null
+++ b/core/lib/libfdt/include/fdt.h
@@ -0,0 +1,111 @@
+#ifndef _FDT_H
+#define _FDT_H
+/*
+ * libfdt - Flat Device Tree manipulation
+ * Copyright (C) 2006 David Gibson, IBM Corporation.
+ * Copyright 2012 Kim Phillips, Freescale Semiconductor.
+ *
+ * libfdt is dual licensed: you can use it either under the terms of
+ * the GPL, or the BSD license, at your option.
+ *
+ *  a) This library is free software; you can redistribute it and/or
+ *     modify it under the terms of the GNU General Public License as
+ *     published by the Free Software Foundation; either version 2 of the
+ *     License, or (at your option) any later version.
+ *
+ *     This library is distributed in the hope that it will be useful,
+ *     but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *     GNU General Public License for more details.
+ *
+ *     You should have received a copy of the GNU General Public
+ *     License along with this library; if not, write to the Free
+ *     Software Foundation, Inc., 51 Franklin St, Fifth Floor, Boston,
+ *     MA 02110-1301 USA
+ *
+ * Alternatively,
+ *
+ *  b) 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 OWNER 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 __ASSEMBLY__
+
+struct fdt_header {
+	fdt32_t magic;			 /* magic word FDT_MAGIC */
+	fdt32_t totalsize;		 /* total size of DT block */
+	fdt32_t off_dt_struct;		 /* offset to structure */
+	fdt32_t off_dt_strings;		 /* offset to strings */
+	fdt32_t off_mem_rsvmap;		 /* offset to memory reserve map */
+	fdt32_t version;		 /* format version */
+	fdt32_t last_comp_version;	 /* last compatible version */
+
+	/* version 2 fields below */
+	fdt32_t boot_cpuid_phys;	 /* Which physical CPU id we're
+					    booting on */
+	/* version 3 fields below */
+	fdt32_t size_dt_strings;	 /* size of the strings block */
+
+	/* version 17 fields below */
+	fdt32_t size_dt_struct;		 /* size of the structure block */
+};
+
+struct fdt_reserve_entry {
+	fdt64_t address;
+	fdt64_t size;
+};
+
+struct fdt_node_header {
+	fdt32_t tag;
+	char name[];
+};
+
+struct fdt_property {
+	fdt32_t tag;
+	fdt32_t len;
+	fdt32_t nameoff;
+	char data[];
+};
+
+#endif /* !__ASSEMBLY */
+
+#define FDT_MAGIC	0xd00dfeed	/* 4: version, 4: total size */
+#define FDT_TAGSIZE	sizeof(fdt32_t)
+
+#define FDT_BEGIN_NODE	0x1		/* Start node: full name */
+#define FDT_END_NODE	0x2		/* End node */
+#define FDT_PROP	0x3		/* Property: name off,
+					   size, content */
+#define FDT_NOP		0x4		/* nop */
+#define FDT_END		0x9
+
+#define FDT_V1_SIZE	(7*sizeof(fdt32_t))
+#define FDT_V2_SIZE	(FDT_V1_SIZE + sizeof(fdt32_t))
+#define FDT_V3_SIZE	(FDT_V2_SIZE + sizeof(fdt32_t))
+#define FDT_V16_SIZE	FDT_V3_SIZE
+#define FDT_V17_SIZE	(FDT_V16_SIZE + sizeof(fdt32_t))
+
+#endif /* _FDT_H */
diff --git a/core/lib/libfdt/include/libfdt.h b/core/lib/libfdt/include/libfdt.h
new file mode 100644
index 0000000..a24020d
--- /dev/null
+++ b/core/lib/libfdt/include/libfdt.h
@@ -0,0 +1,1575 @@
+#ifndef _LIBFDT_H
+#define _LIBFDT_H
+/*
+ * libfdt - Flat Device Tree manipulation
+ * Copyright (C) 2006 David Gibson, IBM Corporation.
+ *
+ * libfdt is dual licensed: you can use it either under the terms of
+ * the GPL, or the BSD license, at your option.
+ *
+ *  a) This library is free software; you can redistribute it and/or
+ *     modify it under the terms of the GNU General Public License as
+ *     published by the Free Software Foundation; either version 2 of the
+ *     License, or (at your option) any later version.
+ *
+ *     This library is distributed in the hope that it will be useful,
+ *     but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *     GNU General Public License for more details.
+ *
+ *     You should have received a copy of the GNU General Public
+ *     License along with this library; if not, write to the Free
+ *     Software Foundation, Inc., 51 Franklin St, Fifth Floor, Boston,
+ *     MA 02110-1301 USA
+ *
+ * Alternatively,
+ *
+ *  b) 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 OWNER 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 <libfdt_env.h>
+#include <fdt.h>
+
+#define FDT_FIRST_SUPPORTED_VERSION	0x10
+#define FDT_LAST_SUPPORTED_VERSION	0x11
+
+/* Error codes: informative error codes */
+#define FDT_ERR_NOTFOUND	1
+	/* FDT_ERR_NOTFOUND: The requested node or property does not exist */
+#define FDT_ERR_EXISTS		2
+	/* FDT_ERR_EXISTS: Attemped to create a node or property which
+	 * already exists */
+#define FDT_ERR_NOSPACE		3
+	/* FDT_ERR_NOSPACE: Operation needed to expand the device
+	 * tree, but its buffer did not have sufficient space to
+	 * contain the expanded tree. Use fdt_open_into() to move the
+	 * device tree to a buffer with more space. */
+
+/* Error codes: codes for bad parameters */
+#define FDT_ERR_BADOFFSET	4
+	/* FDT_ERR_BADOFFSET: Function was passed a structure block
+	 * offset which is out-of-bounds, or which points to an
+	 * unsuitable part of the structure for the operation. */
+#define FDT_ERR_BADPATH		5
+	/* FDT_ERR_BADPATH: Function was passed a badly formatted path
+	 * (e.g. missing a leading / for a function which requires an
+	 * absolute path) */
+#define FDT_ERR_BADPHANDLE	6
+	/* FDT_ERR_BADPHANDLE: Function was passed an invalid phandle
+	 * value.  phandle values of 0 and -1 are not permitted. */
+#define FDT_ERR_BADSTATE	7
+	/* FDT_ERR_BADSTATE: Function was passed an incomplete device
+	 * tree created by the sequential-write functions, which is
+	 * not sufficiently complete for the requested operation. */
+
+/* Error codes: codes for bad device tree blobs */
+#define FDT_ERR_TRUNCATED	8
+	/* FDT_ERR_TRUNCATED: Structure block of the given device tree
+	 * ends without an FDT_END tag. */
+#define FDT_ERR_BADMAGIC	9
+	/* FDT_ERR_BADMAGIC: Given "device tree" appears not to be a
+	 * device tree at all - it is missing the flattened device
+	 * tree magic number. */
+#define FDT_ERR_BADVERSION	10
+	/* FDT_ERR_BADVERSION: Given device tree has a version which
+	 * can't be handled by the requested operation.  For
+	 * read-write functions, this may mean that fdt_open_into() is
+	 * required to convert the tree to the expected version. */
+#define FDT_ERR_BADSTRUCTURE	11
+	/* FDT_ERR_BADSTRUCTURE: Given device tree has a corrupt
+	 * structure block or other serious error (e.g. misnested
+	 * nodes, or subnodes preceding properties). */
+#define FDT_ERR_BADLAYOUT	12
+	/* FDT_ERR_BADLAYOUT: For read-write functions, the given
+	 * device tree has it's sub-blocks in an order that the
+	 * function can't handle (memory reserve map, then structure,
+	 * then strings).  Use fdt_open_into() to reorganize the tree
+	 * into a form suitable for the read-write operations. */
+
+/* "Can't happen" error indicating a bug in libfdt */
+#define FDT_ERR_INTERNAL	13
+	/* FDT_ERR_INTERNAL: libfdt has failed an internal assertion.
+	 * Should never be returned, if it is, it indicates a bug in
+	 * libfdt itself. */
+
+/* Errors in device tree content */
+#define FDT_ERR_BADNCELLS	14
+	/* FDT_ERR_BADNCELLS: Device tree has a #address-cells, #size-cells
+	 * or similar property with a bad format or value */
+
+#define FDT_ERR_MAX		14
+
+/**********************************************************************/
+/* Low-level functions (you probably don't need these)                */
+/**********************************************************************/
+
+const void *fdt_offset_ptr(const void *fdt, int offset, unsigned int checklen);
+static inline void *fdt_offset_ptr_w(void *fdt, int offset, int checklen)
+{
+	return (void *)(uintptr_t)fdt_offset_ptr(fdt, offset, checklen);
+}
+
+uint32_t fdt_next_tag(const void *fdt, int offset, int *nextoffset);
+
+/**********************************************************************/
+/* Traversal functions                                                */
+/**********************************************************************/
+
+int fdt_next_node(const void *fdt, int offset, int *depth);
+
+/**
+ * fdt_first_subnode() - get offset of first direct subnode
+ *
+ * @fdt:	FDT blob
+ * @offset:	Offset of node to check
+ * @return offset of first subnode, or -FDT_ERR_NOTFOUND if there is none
+ */
+int fdt_first_subnode(const void *fdt, int offset);
+
+/**
+ * fdt_next_subnode() - get offset of next direct subnode
+ *
+ * After first calling fdt_first_subnode(), call this function repeatedly to
+ * get direct subnodes of a parent node.
+ *
+ * @fdt:	FDT blob
+ * @offset:	Offset of previous subnode
+ * @return offset of next subnode, or -FDT_ERR_NOTFOUND if there are no more
+ * subnodes
+ */
+int fdt_next_subnode(const void *fdt, int offset);
+
+/**********************************************************************/
+/* General functions                                                  */
+/**********************************************************************/
+
+#define fdt_get_header(fdt, field) \
+	(fdt32_to_cpu(((const struct fdt_header *)(fdt))->field))
+#define fdt_magic(fdt) 			(fdt_get_header(fdt, magic))
+#define fdt_totalsize(fdt)		(fdt_get_header(fdt, totalsize))
+#define fdt_off_dt_struct(fdt)		(fdt_get_header(fdt, off_dt_struct))
+#define fdt_off_dt_strings(fdt)		(fdt_get_header(fdt, off_dt_strings))
+#define fdt_off_mem_rsvmap(fdt)		(fdt_get_header(fdt, off_mem_rsvmap))
+#define fdt_version(fdt)		(fdt_get_header(fdt, version))
+#define fdt_last_comp_version(fdt) 	(fdt_get_header(fdt, last_comp_version))
+#define fdt_boot_cpuid_phys(fdt) 	(fdt_get_header(fdt, boot_cpuid_phys))
+#define fdt_size_dt_strings(fdt) 	(fdt_get_header(fdt, size_dt_strings))
+#define fdt_size_dt_struct(fdt)		(fdt_get_header(fdt, size_dt_struct))
+
+#define __fdt_set_hdr(name) \
+	static inline void fdt_set_##name(void *fdt, uint32_t val) \
+	{ \
+		struct fdt_header *fdth = (struct fdt_header*)fdt; \
+		fdth->name = cpu_to_fdt32(val); \
+	}
+__fdt_set_hdr(magic)
+__fdt_set_hdr(totalsize)
+__fdt_set_hdr(off_dt_struct)
+__fdt_set_hdr(off_dt_strings)
+__fdt_set_hdr(off_mem_rsvmap)
+__fdt_set_hdr(version)
+__fdt_set_hdr(last_comp_version)
+__fdt_set_hdr(boot_cpuid_phys)
+__fdt_set_hdr(size_dt_strings)
+__fdt_set_hdr(size_dt_struct)
+#undef __fdt_set_hdr
+
+/**
+ * fdt_check_header - sanity check a device tree or possible device tree
+ * @fdt: pointer to data which might be a flattened device tree
+ *
+ * fdt_check_header() checks that the given buffer contains what
+ * appears to be a flattened device tree with sane information in its
+ * header.
+ *
+ * returns:
+ *     0, if the buffer appears to contain a valid device tree
+ *     -FDT_ERR_BADMAGIC,
+ *     -FDT_ERR_BADVERSION,
+ *     -FDT_ERR_BADSTATE, standard meanings, as above
+ */
+int fdt_check_header(const void *fdt);
+
+/**
+ * fdt_move - move a device tree around in memory
+ * @fdt: pointer to the device tree to move
+ * @buf: pointer to memory where the device is to be moved
+ * @bufsize: size of the memory space at buf
+ *
+ * fdt_move() relocates, if possible, the device tree blob located at
+ * fdt to the buffer at buf of size bufsize.  The buffer may overlap
+ * with the existing device tree blob at fdt.  Therefore,
+ *     fdt_move(fdt, fdt, fdt_totalsize(fdt))
+ * should always succeed.
+ *
+ * returns:
+ *     0, on success
+ *     -FDT_ERR_NOSPACE, bufsize is insufficient to contain the device tree
+ *     -FDT_ERR_BADMAGIC,
+ *     -FDT_ERR_BADVERSION,
+ *     -FDT_ERR_BADSTATE, standard meanings
+ */
+int fdt_move(const void *fdt, void *buf, int bufsize);
+
+/**********************************************************************/
+/* Read-only functions                                                */
+/**********************************************************************/
+
+/**
+ * fdt_string - retrieve a string from the strings block of a device tree
+ * @fdt: pointer to the device tree blob
+ * @stroffset: offset of the string within the strings block (native endian)
+ *
+ * fdt_string() retrieves a pointer to a single string from the
+ * strings block of the device tree blob at fdt.
+ *
+ * returns:
+ *     a pointer to the string, on success
+ *     NULL, if stroffset is out of bounds
+ */
+const char *fdt_string(const void *fdt, int stroffset);
+
+/**
+ * fdt_num_mem_rsv - retrieve the number of memory reserve map entries
+ * @fdt: pointer to the device tree blob
+ *
+ * Returns the number of entries in the device tree blob's memory
+ * reservation map.  This does not include the terminating 0,0 entry
+ * or any other (0,0) entries reserved for expansion.
+ *
+ * returns:
+ *     the number of entries
+ */
+int fdt_num_mem_rsv(const void *fdt);
+
+/**
+ * fdt_get_mem_rsv - retrieve one memory reserve map entry
+ * @fdt: pointer to the device tree blob
+ * @address, @size: pointers to 64-bit variables
+ *
+ * On success, *address and *size will contain the address and size of
+ * the n-th reserve map entry from the device tree blob, in
+ * native-endian format.
+ *
+ * returns:
+ *     0, on success
+ *     -FDT_ERR_BADMAGIC,
+ *     -FDT_ERR_BADVERSION,
+ *     -FDT_ERR_BADSTATE, standard meanings
+ */
+int fdt_get_mem_rsv(const void *fdt, int n, uint64_t *address, uint64_t *size);
+
+/**
+ * fdt_subnode_offset_namelen - find a subnode based on substring
+ * @fdt: pointer to the device tree blob
+ * @parentoffset: structure block offset of a node
+ * @name: name of the subnode to locate
+ * @namelen: number of characters of name to consider
+ *
+ * Identical to fdt_subnode_offset(), but only examine the first
+ * namelen characters of name for matching the subnode name.  This is
+ * useful for finding subnodes based on a portion of a larger string,
+ * such as a full path.
+ */
+int fdt_subnode_offset_namelen(const void *fdt, int parentoffset,
+			       const char *name, int namelen);
+/**
+ * fdt_subnode_offset - find a subnode of a given node
+ * @fdt: pointer to the device tree blob
+ * @parentoffset: structure block offset of a node
+ * @name: name of the subnode to locate
+ *
+ * fdt_subnode_offset() finds a subnode of the node at structure block
+ * offset parentoffset with the given name.  name may include a unit
+ * address, in which case fdt_subnode_offset() will find the subnode
+ * with that unit address, or the unit address may be omitted, in
+ * which case fdt_subnode_offset() will find an arbitrary subnode
+ * whose name excluding unit address matches the given name.
+ *
+ * returns:
+ *	structure block offset of the requested subnode (>=0), on success
+ *	-FDT_ERR_NOTFOUND, if the requested subnode does not exist
+ *	-FDT_ERR_BADOFFSET, if parentoffset did not point to an FDT_BEGIN_NODE tag
+ *      -FDT_ERR_BADMAGIC,
+ *	-FDT_ERR_BADVERSION,
+ *	-FDT_ERR_BADSTATE,
+ *	-FDT_ERR_BADSTRUCTURE,
+ *	-FDT_ERR_TRUNCATED, standard meanings.
+ */
+int fdt_subnode_offset(const void *fdt, int parentoffset, const char *name);
+
+/**
+ * fdt_path_offset - find a tree node by its full path
+ * @fdt: pointer to the device tree blob
+ * @path: full path of the node to locate
+ *
+ * fdt_path_offset() finds a node of a given path in the device tree.
+ * Each path component may omit the unit address portion, but the
+ * results of this are undefined if any such path component is
+ * ambiguous (that is if there are multiple nodes at the relevant
+ * level matching the given component, differentiated only by unit
+ * address).
+ *
+ * returns:
+ *	structure block offset of the node with the requested path (>=0), on success
+ *	-FDT_ERR_BADPATH, given path does not begin with '/' or is invalid
+ *	-FDT_ERR_NOTFOUND, if the requested node does not exist
+ *      -FDT_ERR_BADMAGIC,
+ *	-FDT_ERR_BADVERSION,
+ *	-FDT_ERR_BADSTATE,
+ *	-FDT_ERR_BADSTRUCTURE,
+ *	-FDT_ERR_TRUNCATED, standard meanings.
+ */
+int fdt_path_offset(const void *fdt, const char *path);
+
+/**
+ * fdt_get_name - retrieve the name of a given node
+ * @fdt: pointer to the device tree blob
+ * @nodeoffset: structure block offset of the starting node
+ * @lenp: pointer to an integer variable (will be overwritten) or NULL
+ *
+ * fdt_get_name() retrieves the name (including unit address) of the
+ * device tree node at structure block offset nodeoffset.  If lenp is
+ * non-NULL, the length of this name is also returned, in the integer
+ * pointed to by lenp.
+ *
+ * returns:
+ *	pointer to the node's name, on success
+ *		If lenp is non-NULL, *lenp contains the length of that name (>=0)
+ *	NULL, on error
+ *		if lenp is non-NULL *lenp contains an error code (<0):
+ *		-FDT_ERR_BADOFFSET, nodeoffset did not point to FDT_BEGIN_NODE tag
+ *		-FDT_ERR_BADMAGIC,
+ *		-FDT_ERR_BADVERSION,
+ *		-FDT_ERR_BADSTATE, standard meanings
+ */
+const char *fdt_get_name(const void *fdt, int nodeoffset, int *lenp);
+
+/**
+ * fdt_first_property_offset - find the offset of a node's first property
+ * @fdt: pointer to the device tree blob
+ * @nodeoffset: structure block offset of a node
+ *
+ * fdt_first_property_offset() finds the first property of the node at
+ * the given structure block offset.
+ *
+ * returns:
+ *	structure block offset of the property (>=0), on success
+ *	-FDT_ERR_NOTFOUND, if the requested node has no properties
+ *	-FDT_ERR_BADOFFSET, if nodeoffset did not point to an FDT_BEGIN_NODE tag
+ *      -FDT_ERR_BADMAGIC,
+ *	-FDT_ERR_BADVERSION,
+ *	-FDT_ERR_BADSTATE,
+ *	-FDT_ERR_BADSTRUCTURE,
+ *	-FDT_ERR_TRUNCATED, standard meanings.
+ */
+int fdt_first_property_offset(const void *fdt, int nodeoffset);
+
+/**
+ * fdt_next_property_offset - step through a node's properties
+ * @fdt: pointer to the device tree blob
+ * @offset: structure block offset of a property
+ *
+ * fdt_next_property_offset() finds the property immediately after the
+ * one at the given structure block offset.  This will be a property
+ * of the same node as the given property.
+ *
+ * returns:
+ *	structure block offset of the next property (>=0), on success
+ *	-FDT_ERR_NOTFOUND, if the given property is the last in its node
+ *	-FDT_ERR_BADOFFSET, if nodeoffset did not point to an FDT_PROP tag
+ *      -FDT_ERR_BADMAGIC,
+ *	-FDT_ERR_BADVERSION,
+ *	-FDT_ERR_BADSTATE,
+ *	-FDT_ERR_BADSTRUCTURE,
+ *	-FDT_ERR_TRUNCATED, standard meanings.
+ */
+int fdt_next_property_offset(const void *fdt, int offset);
+
+/**
+ * fdt_get_property_by_offset - retrieve the property at a given offset
+ * @fdt: pointer to the device tree blob
+ * @offset: offset of the property to retrieve
+ * @lenp: pointer to an integer variable (will be overwritten) or NULL
+ *
+ * fdt_get_property_by_offset() retrieves a pointer to the
+ * fdt_property structure within the device tree blob at the given
+ * offset.  If lenp is non-NULL, the length of the property value is
+ * also returned, in the integer pointed to by lenp.
+ *
+ * returns:
+ *	pointer to the structure representing the property
+ *		if lenp is non-NULL, *lenp contains the length of the property
+ *		value (>=0)
+ *	NULL, on error
+ *		if lenp is non-NULL, *lenp contains an error code (<0):
+ *		-FDT_ERR_BADOFFSET, nodeoffset did not point to FDT_PROP tag
+ *		-FDT_ERR_BADMAGIC,
+ *		-FDT_ERR_BADVERSION,
+ *		-FDT_ERR_BADSTATE,
+ *		-FDT_ERR_BADSTRUCTURE,
+ *		-FDT_ERR_TRUNCATED, standard meanings
+ */
+const struct fdt_property *fdt_get_property_by_offset(const void *fdt,
+						      int offset,
+						      int *lenp);
+
+/**
+ * fdt_get_property_namelen - find a property based on substring
+ * @fdt: pointer to the device tree blob
+ * @nodeoffset: offset of the node whose property to find
+ * @name: name of the property to find
+ * @namelen: number of characters of name to consider
+ * @lenp: pointer to an integer variable (will be overwritten) or NULL
+ *
+ * Identical to fdt_get_property_namelen(), but only examine the first
+ * namelen characters of name for matching the property name.
+ */
+const struct fdt_property *fdt_get_property_namelen(const void *fdt,
+						    int nodeoffset,
+						    const char *name,
+						    int namelen, int *lenp);
+
+/**
+ * fdt_get_property - find a given property in a given node
+ * @fdt: pointer to the device tree blob
+ * @nodeoffset: offset of the node whose property to find
+ * @name: name of the property to find
+ * @lenp: pointer to an integer variable (will be overwritten) or NULL
+ *
+ * fdt_get_property() retrieves a pointer to the fdt_property
+ * structure within the device tree blob corresponding to the property
+ * named 'name' of the node at offset nodeoffset.  If lenp is
+ * non-NULL, the length of the property value is also returned, in the
+ * integer pointed to by lenp.
+ *
+ * returns:
+ *	pointer to the structure representing the property
+ *		if lenp is non-NULL, *lenp contains the length of the property
+ *		value (>=0)
+ *	NULL, on error
+ *		if lenp is non-NULL, *lenp contains an error code (<0):
+ *		-FDT_ERR_NOTFOUND, node does not have named property
+ *		-FDT_ERR_BADOFFSET, nodeoffset did not point to FDT_BEGIN_NODE tag
+ *		-FDT_ERR_BADMAGIC,
+ *		-FDT_ERR_BADVERSION,
+ *		-FDT_ERR_BADSTATE,
+ *		-FDT_ERR_BADSTRUCTURE,
+ *		-FDT_ERR_TRUNCATED, standard meanings
+ */
+const struct fdt_property *fdt_get_property(const void *fdt, int nodeoffset,
+					    const char *name, int *lenp);
+static inline struct fdt_property *fdt_get_property_w(void *fdt, int nodeoffset,
+						      const char *name,
+						      int *lenp)
+{
+	return (struct fdt_property *)(uintptr_t)
+		fdt_get_property(fdt, nodeoffset, name, lenp);
+}
+
+/**
+ * fdt_getprop_by_offset - retrieve the value of a property at a given offset
+ * @fdt: pointer to the device tree blob
+ * @ffset: offset of the property to read
+ * @namep: pointer to a string variable (will be overwritten) or NULL
+ * @lenp: pointer to an integer variable (will be overwritten) or NULL
+ *
+ * fdt_getprop_by_offset() retrieves a pointer to the value of the
+ * property at structure block offset 'offset' (this will be a pointer
+ * to within the device blob itself, not a copy of the value).  If
+ * lenp is non-NULL, the length of the property value is also
+ * returned, in the integer pointed to by lenp.  If namep is non-NULL,
+ * the property's namne will also be returned in the char * pointed to
+ * by namep (this will be a pointer to within the device tree's string
+ * block, not a new copy of the name).
+ *
+ * returns:
+ *	pointer to the property's value
+ *		if lenp is non-NULL, *lenp contains the length of the property
+ *		value (>=0)
+ *		if namep is non-NULL *namep contiains a pointer to the property
+ *		name.
+ *	NULL, on error
+ *		if lenp is non-NULL, *lenp contains an error code (<0):
+ *		-FDT_ERR_BADOFFSET, nodeoffset did not point to FDT_PROP tag
+ *		-FDT_ERR_BADMAGIC,
+ *		-FDT_ERR_BADVERSION,
+ *		-FDT_ERR_BADSTATE,
+ *		-FDT_ERR_BADSTRUCTURE,
+ *		-FDT_ERR_TRUNCATED, standard meanings
+ */
+const void *fdt_getprop_by_offset(const void *fdt, int offset,
+				  const char **namep, int *lenp);
+
+/**
+ * fdt_getprop_namelen - get property value based on substring
+ * @fdt: pointer to the device tree blob
+ * @nodeoffset: offset of the node whose property to find
+ * @name: name of the property to find
+ * @namelen: number of characters of name to consider
+ * @lenp: pointer to an integer variable (will be overwritten) or NULL
+ *
+ * Identical to fdt_getprop(), but only examine the first namelen
+ * characters of name for matching the property name.
+ */
+const void *fdt_getprop_namelen(const void *fdt, int nodeoffset,
+				const char *name, int namelen, int *lenp);
+
+/**
+ * fdt_getprop - retrieve the value of a given property
+ * @fdt: pointer to the device tree blob
+ * @nodeoffset: offset of the node whose property to find
+ * @name: name of the property to find
+ * @lenp: pointer to an integer variable (will be overwritten) or NULL
+ *
+ * fdt_getprop() retrieves a pointer to the value of the property
+ * named 'name' of the node at offset nodeoffset (this will be a
+ * pointer to within the device blob itself, not a copy of the value).
+ * If lenp is non-NULL, the length of the property value is also
+ * returned, in the integer pointed to by lenp.
+ *
+ * returns:
+ *	pointer to the property's value
+ *		if lenp is non-NULL, *lenp contains the length of the property
+ *		value (>=0)
+ *	NULL, on error
+ *		if lenp is non-NULL, *lenp contains an error code (<0):
+ *		-FDT_ERR_NOTFOUND, node does not have named property
+ *		-FDT_ERR_BADOFFSET, nodeoffset did not point to FDT_BEGIN_NODE tag
+ *		-FDT_ERR_BADMAGIC,
+ *		-FDT_ERR_BADVERSION,
+ *		-FDT_ERR_BADSTATE,
+ *		-FDT_ERR_BADSTRUCTURE,
+ *		-FDT_ERR_TRUNCATED, standard meanings
+ */
+const void *fdt_getprop(const void *fdt, int nodeoffset,
+			const char *name, int *lenp);
+static inline void *fdt_getprop_w(void *fdt, int nodeoffset,
+				  const char *name, int *lenp)
+{
+	return (void *)(uintptr_t)fdt_getprop(fdt, nodeoffset, name, lenp);
+}
+
+/**
+ * fdt_get_phandle - retrieve the phandle of a given node
+ * @fdt: pointer to the device tree blob
+ * @nodeoffset: structure block offset of the node
+ *
+ * fdt_get_phandle() retrieves the phandle of the device tree node at
+ * structure block offset nodeoffset.
+ *
+ * returns:
+ *	the phandle of the node at nodeoffset, on success (!= 0, != -1)
+ *	0, if the node has no phandle, or another error occurs
+ */
+uint32_t fdt_get_phandle(const void *fdt, int nodeoffset);
+
+/**
+ * fdt_get_alias_namelen - get alias based on substring
+ * @fdt: pointer to the device tree blob
+ * @name: name of the alias th look up
+ * @namelen: number of characters of name to consider
+ *
+ * Identical to fdt_get_alias(), but only examine the first namelen
+ * characters of name for matching the alias name.
+ */
+const char *fdt_get_alias_namelen(const void *fdt,
+				  const char *name, int namelen);
+
+/**
+ * fdt_get_alias - retreive the path referenced by a given alias
+ * @fdt: pointer to the device tree blob
+ * @name: name of the alias th look up
+ *
+ * fdt_get_alias() retrieves the value of a given alias.  That is, the
+ * value of the property named 'name' in the node /aliases.
+ *
+ * returns:
+ *	a pointer to the expansion of the alias named 'name', if it exists
+ *	NULL, if the given alias or the /aliases node does not exist
+ */
+const char *fdt_get_alias(const void *fdt, const char *name);
+
+/**
+ * fdt_get_path - determine the full path of a node
+ * @fdt: pointer to the device tree blob
+ * @nodeoffset: offset of the node whose path to find
+ * @buf: character buffer to contain the returned path (will be overwritten)
+ * @buflen: size of the character buffer at buf
+ *
+ * fdt_get_path() computes the full path of the node at offset
+ * nodeoffset, and records that path in the buffer at buf.
+ *
+ * NOTE: This function is expensive, as it must scan the device tree
+ * structure from the start to nodeoffset.
+ *
+ * returns:
+ *	0, on success
+ *		buf contains the absolute path of the node at
+ *		nodeoffset, as a NUL-terminated string.
+ * 	-FDT_ERR_BADOFFSET, nodeoffset does not refer to a BEGIN_NODE tag
+ *	-FDT_ERR_NOSPACE, the path of the given node is longer than (bufsize-1)
+ *		characters and will not fit in the given buffer.
+ *	-FDT_ERR_BADMAGIC,
+ *	-FDT_ERR_BADVERSION,
+ *	-FDT_ERR_BADSTATE,
+ *	-FDT_ERR_BADSTRUCTURE, standard meanings
+ */
+int fdt_get_path(const void *fdt, int nodeoffset, char *buf, int buflen);
+
+/**
+ * fdt_supernode_atdepth_offset - find a specific ancestor of a node
+ * @fdt: pointer to the device tree blob
+ * @nodeoffset: offset of the node whose parent to find
+ * @supernodedepth: depth of the ancestor to find
+ * @nodedepth: pointer to an integer variable (will be overwritten) or NULL
+ *
+ * fdt_supernode_atdepth_offset() finds an ancestor of the given node
+ * at a specific depth from the root (where the root itself has depth
+ * 0, its immediate subnodes depth 1 and so forth).  So
+ *	fdt_supernode_atdepth_offset(fdt, nodeoffset, 0, NULL);
+ * will always return 0, the offset of the root node.  If the node at
+ * nodeoffset has depth D, then:
+ *	fdt_supernode_atdepth_offset(fdt, nodeoffset, D, NULL);
+ * will return nodeoffset itself.
+ *
+ * NOTE: This function is expensive, as it must scan the device tree
+ * structure from the start to nodeoffset.
+ *
+ * returns:
+
+ *	structure block offset of the node at node offset's ancestor
+ *		of depth supernodedepth (>=0), on success
+ * 	-FDT_ERR_BADOFFSET, nodeoffset does not refer to a BEGIN_NODE tag
+*	-FDT_ERR_NOTFOUND, supernodedepth was greater than the depth of nodeoffset
+ *	-FDT_ERR_BADMAGIC,
+ *	-FDT_ERR_BADVERSION,
+ *	-FDT_ERR_BADSTATE,
+ *	-FDT_ERR_BADSTRUCTURE, standard meanings
+ */
+int fdt_supernode_atdepth_offset(const void *fdt, int nodeoffset,
+				 int supernodedepth, int *nodedepth);
+
+/**
+ * fdt_node_depth - find the depth of a given node
+ * @fdt: pointer to the device tree blob
+ * @nodeoffset: offset of the node whose parent to find
+ *
+ * fdt_node_depth() finds the depth of a given node.  The root node
+ * has depth 0, its immediate subnodes depth 1 and so forth.
+ *
+ * NOTE: This function is expensive, as it must scan the device tree
+ * structure from the start to nodeoffset.
+ *
+ * returns:
+ *	depth of the node at nodeoffset (>=0), on success
+ * 	-FDT_ERR_BADOFFSET, nodeoffset does not refer to a BEGIN_NODE tag
+ *	-FDT_ERR_BADMAGIC,
+ *	-FDT_ERR_BADVERSION,
+ *	-FDT_ERR_BADSTATE,
+ *	-FDT_ERR_BADSTRUCTURE, standard meanings
+ */
+int fdt_node_depth(const void *fdt, int nodeoffset);
+
+/**
+ * fdt_parent_offset - find the parent of a given node
+ * @fdt: pointer to the device tree blob
+ * @nodeoffset: offset of the node whose parent to find
+ *
+ * fdt_parent_offset() locates the parent node of a given node (that
+ * is, it finds the offset of the node which contains the node at
+ * nodeoffset as a subnode).
+ *
+ * NOTE: This function is expensive, as it must scan the device tree
+ * structure from the start to nodeoffset, *twice*.
+ *
+ * returns:
+ *	structure block offset of the parent of the node at nodeoffset
+ *		(>=0), on success
+ * 	-FDT_ERR_BADOFFSET, nodeoffset does not refer to a BEGIN_NODE tag
+ *	-FDT_ERR_BADMAGIC,
+ *	-FDT_ERR_BADVERSION,
+ *	-FDT_ERR_BADSTATE,
+ *	-FDT_ERR_BADSTRUCTURE, standard meanings
+ */
+int fdt_parent_offset(const void *fdt, int nodeoffset);
+
+/**
+ * fdt_node_offset_by_prop_value - find nodes with a given property value
+ * @fdt: pointer to the device tree blob
+ * @startoffset: only find nodes after this offset
+ * @propname: property name to check
+ * @propval: property value to search for
+ * @proplen: length of the value in propval
+ *
+ * fdt_node_offset_by_prop_value() returns the offset of the first
+ * node after startoffset, which has a property named propname whose
+ * value is of length proplen and has value equal to propval; or if
+ * startoffset is -1, the very first such node in the tree.
+ *
+ * To iterate through all nodes matching the criterion, the following
+ * idiom can be used:
+ *	offset = fdt_node_offset_by_prop_value(fdt, -1, propname,
+ *					       propval, proplen);
+ *	while (offset != -FDT_ERR_NOTFOUND) {
+ *		// other code here
+ *		offset = fdt_node_offset_by_prop_value(fdt, offset, propname,
+ *						       propval, proplen);
+ *	}
+ *
+ * Note the -1 in the first call to the function, if 0 is used here
+ * instead, the function will never locate the root node, even if it
+ * matches the criterion.
+ *
+ * returns:
+ *	structure block offset of the located node (>= 0, >startoffset),
+ *		 on success
+ *	-FDT_ERR_NOTFOUND, no node matching the criterion exists in the
+ *		tree after startoffset
+ * 	-FDT_ERR_BADOFFSET, nodeoffset does not refer to a BEGIN_NODE tag
+ *	-FDT_ERR_BADMAGIC,
+ *	-FDT_ERR_BADVERSION,
+ *	-FDT_ERR_BADSTATE,
+ *	-FDT_ERR_BADSTRUCTURE, standard meanings
+ */
+int fdt_node_offset_by_prop_value(const void *fdt, int startoffset,
+				  const char *propname,
+				  const void *propval, int proplen);
+
+/**
+ * fdt_node_offset_by_phandle - find the node with a given phandle
+ * @fdt: pointer to the device tree blob
+ * @phandle: phandle value
+ *
+ * fdt_node_offset_by_phandle() returns the offset of the node
+ * which has the given phandle value.  If there is more than one node
+ * in the tree with the given phandle (an invalid tree), results are
+ * undefined.
+ *
+ * returns:
+ *	structure block offset of the located node (>= 0), on success
+ *	-FDT_ERR_NOTFOUND, no node with that phandle exists
+ *	-FDT_ERR_BADPHANDLE, given phandle value was invalid (0 or -1)
+ *	-FDT_ERR_BADMAGIC,
+ *	-FDT_ERR_BADVERSION,
+ *	-FDT_ERR_BADSTATE,
+ *	-FDT_ERR_BADSTRUCTURE, standard meanings
+ */
+int fdt_node_offset_by_phandle(const void *fdt, uint32_t phandle);
+
+/**
+ * fdt_node_check_compatible: check a node's compatible property
+ * @fdt: pointer to the device tree blob
+ * @nodeoffset: offset of a tree node
+ * @compatible: string to match against
+ *
+ *
+ * fdt_node_check_compatible() returns 0 if the given node contains a
+ * 'compatible' property with the given string as one of its elements,
+ * it returns non-zero otherwise, or on error.
+ *
+ * returns:
+ *	0, if the node has a 'compatible' property listing the given string
+ *	1, if the node has a 'compatible' property, but it does not list
+ *		the given string
+ *	-FDT_ERR_NOTFOUND, if the given node has no 'compatible' property
+ * 	-FDT_ERR_BADOFFSET, if nodeoffset does not refer to a BEGIN_NODE tag
+ *	-FDT_ERR_BADMAGIC,
+ *	-FDT_ERR_BADVERSION,
+ *	-FDT_ERR_BADSTATE,
+ *	-FDT_ERR_BADSTRUCTURE, standard meanings
+ */
+int fdt_node_check_compatible(const void *fdt, int nodeoffset,
+			      const char *compatible);
+
+/**
+ * fdt_node_offset_by_compatible - find nodes with a given 'compatible' value
+ * @fdt: pointer to the device tree blob
+ * @startoffset: only find nodes after this offset
+ * @compatible: 'compatible' string to match against
+ *
+ * fdt_node_offset_by_compatible() returns the offset of the first
+ * node after startoffset, which has a 'compatible' property which
+ * lists the given compatible string; or if startoffset is -1, the
+ * very first such node in the tree.
+ *
+ * To iterate through all nodes matching the criterion, the following
+ * idiom can be used:
+ *	offset = fdt_node_offset_by_compatible(fdt, -1, compatible);
+ *	while (offset != -FDT_ERR_NOTFOUND) {
+ *		// other code here
+ *		offset = fdt_node_offset_by_compatible(fdt, offset, compatible);
+ *	}
+ *
+ * Note the -1 in the first call to the function, if 0 is used here
+ * instead, the function will never locate the root node, even if it
+ * matches the criterion.
+ *
+ * returns:
+ *	structure block offset of the located node (>= 0, >startoffset),
+ *		 on success
+ *	-FDT_ERR_NOTFOUND, no node matching the criterion exists in the
+ *		tree after startoffset
+ * 	-FDT_ERR_BADOFFSET, nodeoffset does not refer to a BEGIN_NODE tag
+ *	-FDT_ERR_BADMAGIC,
+ *	-FDT_ERR_BADVERSION,
+ *	-FDT_ERR_BADSTATE,
+ *	-FDT_ERR_BADSTRUCTURE, standard meanings
+ */
+int fdt_node_offset_by_compatible(const void *fdt, int startoffset,
+				  const char *compatible);
+
+/**
+ * fdt_stringlist_contains - check a string list property for a string
+ * @strlist: Property containing a list of strings to check
+ * @listlen: Length of property
+ * @str: String to search for
+ *
+ * This is a utility function provided for convenience. The list contains
+ * one or more strings, each terminated by \0, as is found in a device tree
+ * "compatible" property.
+ *
+ * @return: 1 if the string is found in the list, 0 not found, or invalid list
+ */
+int fdt_stringlist_contains(const char *strlist, int listlen, const char *str);
+
+/**********************************************************************/
+/* Read-only functions (addressing related)                           */
+/**********************************************************************/
+
+/**
+ * FDT_MAX_NCELLS - maximum value for #address-cells and #size-cells
+ *
+ * This is the maximum value for #address-cells, #size-cells and
+ * similar properties that will be processed by libfdt.  IEE1275
+ * requires that OF implementations handle values up to 4.
+ * Implementations may support larger values, but in practice higher
+ * values aren't used.
+ */
+#define FDT_MAX_NCELLS		4
+
+/**
+ * fdt_address_cells - retrieve address size for a bus represented in the tree
+ * @fdt: pointer to the device tree blob
+ * @nodeoffset: offset of the node to find the address size for
+ *
+ * When the node has a valid #address-cells property, returns its value.
+ *
+ * returns:
+ *	0 <= n < FDT_MAX_NCELLS, on success
+ *      2, if the node has no #address-cells property
+ *      -FDT_ERR_BADNCELLS, if the node has a badly formatted or invalid #address-cells property
+ *	-FDT_ERR_BADMAGIC,
+ *	-FDT_ERR_BADVERSION,
+ *	-FDT_ERR_BADSTATE,
+ *	-FDT_ERR_BADSTRUCTURE,
+ *	-FDT_ERR_TRUNCATED, standard meanings
+ */
+int fdt_address_cells(const void *fdt, int nodeoffset);
+
+/**
+ * fdt_size_cells - retrieve address range size for a bus represented in the
+ *                  tree
+ * @fdt: pointer to the device tree blob
+ * @nodeoffset: offset of the node to find the address range size for
+ *
+ * When the node has a valid #size-cells property, returns its value.
+ *
+ * returns:
+ *	0 <= n < FDT_MAX_NCELLS, on success
+ *      2, if the node has no #address-cells property
+ *      -FDT_ERR_BADNCELLS, if the node has a badly formatted or invalid #size-cells property
+ *	-FDT_ERR_BADMAGIC,
+ *	-FDT_ERR_BADVERSION,
+ *	-FDT_ERR_BADSTATE,
+ *	-FDT_ERR_BADSTRUCTURE,
+ *	-FDT_ERR_TRUNCATED, standard meanings
+ */
+int fdt_size_cells(const void *fdt, int nodeoffset);
+
+
+/**********************************************************************/
+/* Write-in-place functions                                           */
+/**********************************************************************/
+
+/**
+ * fdt_setprop_inplace - change a property's value, but not its size
+ * @fdt: pointer to the device tree blob
+ * @nodeoffset: offset of the node whose property to change
+ * @name: name of the property to change
+ * @val: pointer to data to replace the property value with
+ * @len: length of the property value
+ *
+ * fdt_setprop_inplace() replaces the value of a given property with
+ * the data in val, of length len.  This function cannot change the
+ * size of a property, and so will only work if len is equal to the
+ * current length of the property.
+ *
+ * This function will alter only the bytes in the blob which contain
+ * the given property value, and will not alter or move any other part
+ * of the tree.
+ *
+ * returns:
+ *	0, on success
+ *	-FDT_ERR_NOSPACE, if len is not equal to the property's current length
+ *	-FDT_ERR_NOTFOUND, node does not have the named property
+ *	-FDT_ERR_BADOFFSET, nodeoffset did not point to FDT_BEGIN_NODE tag
+ *	-FDT_ERR_BADMAGIC,
+ *	-FDT_ERR_BADVERSION,
+ *	-FDT_ERR_BADSTATE,
+ *	-FDT_ERR_BADSTRUCTURE,
+ *	-FDT_ERR_TRUNCATED, standard meanings
+ */
+int fdt_setprop_inplace(void *fdt, int nodeoffset, const char *name,
+			const void *val, int len);
+
+/**
+ * fdt_setprop_inplace_u32 - change the value of a 32-bit integer property
+ * @fdt: pointer to the device tree blob
+ * @nodeoffset: offset of the node whose property to change
+ * @name: name of the property to change
+ * @val: 32-bit integer value to replace the property with
+ *
+ * fdt_setprop_inplace_u32() replaces the value of a given property
+ * with the 32-bit integer value in val, converting val to big-endian
+ * if necessary.  This function cannot change the size of a property,
+ * and so will only work if the property already exists and has length
+ * 4.
+ *
+ * This function will alter only the bytes in the blob which contain
+ * the given property value, and will not alter or move any other part
+ * of the tree.
+ *
+ * returns:
+ *	0, on success
+ *	-FDT_ERR_NOSPACE, if the property's length is not equal to 4
+ *	-FDT_ERR_NOTFOUND, node does not have the named property
+ *	-FDT_ERR_BADOFFSET, nodeoffset did not point to FDT_BEGIN_NODE tag
+ *	-FDT_ERR_BADMAGIC,
+ *	-FDT_ERR_BADVERSION,
+ *	-FDT_ERR_BADSTATE,
+ *	-FDT_ERR_BADSTRUCTURE,
+ *	-FDT_ERR_TRUNCATED, standard meanings
+ */
+static inline int fdt_setprop_inplace_u32(void *fdt, int nodeoffset,
+					  const char *name, uint32_t val)
+{
+	fdt32_t tmp = cpu_to_fdt32(val);
+	return fdt_setprop_inplace(fdt, nodeoffset, name, &tmp, sizeof(tmp));
+}
+
+/**
+ * fdt_setprop_inplace_u64 - change the value of a 64-bit integer property
+ * @fdt: pointer to the device tree blob
+ * @nodeoffset: offset of the node whose property to change
+ * @name: name of the property to change
+ * @val: 64-bit integer value to replace the property with
+ *
+ * fdt_setprop_inplace_u64() replaces the value of a given property
+ * with the 64-bit integer value in val, converting val to big-endian
+ * if necessary.  This function cannot change the size of a property,
+ * and so will only work if the property already exists and has length
+ * 8.
+ *
+ * This function will alter only the bytes in the blob which contain
+ * the given property value, and will not alter or move any other part
+ * of the tree.
+ *
+ * returns:
+ *	0, on success
+ *	-FDT_ERR_NOSPACE, if the property's length is not equal to 8
+ *	-FDT_ERR_NOTFOUND, node does not have the named property
+ *	-FDT_ERR_BADOFFSET, nodeoffset did not point to FDT_BEGIN_NODE tag
+ *	-FDT_ERR_BADMAGIC,
+ *	-FDT_ERR_BADVERSION,
+ *	-FDT_ERR_BADSTATE,
+ *	-FDT_ERR_BADSTRUCTURE,
+ *	-FDT_ERR_TRUNCATED, standard meanings
+ */
+static inline int fdt_setprop_inplace_u64(void *fdt, int nodeoffset,
+					  const char *name, uint64_t val)
+{
+	fdt64_t tmp = cpu_to_fdt64(val);
+	return fdt_setprop_inplace(fdt, nodeoffset, name, &tmp, sizeof(tmp));
+}
+
+/**
+ * fdt_setprop_inplace_cell - change the value of a single-cell property
+ *
+ * This is an alternative name for fdt_setprop_inplace_u32()
+ */
+static inline int fdt_setprop_inplace_cell(void *fdt, int nodeoffset,
+					   const char *name, uint32_t val)
+{
+	return fdt_setprop_inplace_u32(fdt, nodeoffset, name, val);
+}
+
+/**
+ * fdt_nop_property - replace a property with nop tags
+ * @fdt: pointer to the device tree blob
+ * @nodeoffset: offset of the node whose property to nop
+ * @name: name of the property to nop
+ *
+ * fdt_nop_property() will replace a given property's representation
+ * in the blob with FDT_NOP tags, effectively removing it from the
+ * tree.
+ *
+ * This function will alter only the bytes in the blob which contain
+ * the property, and will not alter or move any other part of the
+ * tree.
+ *
+ * returns:
+ *	0, on success
+ *	-FDT_ERR_NOTFOUND, node does not have the named property
+ *	-FDT_ERR_BADOFFSET, nodeoffset did not point to FDT_BEGIN_NODE tag
+ *	-FDT_ERR_BADMAGIC,
+ *	-FDT_ERR_BADVERSION,
+ *	-FDT_ERR_BADSTATE,
+ *	-FDT_ERR_BADSTRUCTURE,
+ *	-FDT_ERR_TRUNCATED, standard meanings
+ */
+int fdt_nop_property(void *fdt, int nodeoffset, const char *name);
+
+/**
+ * fdt_nop_node - replace a node (subtree) with nop tags
+ * @fdt: pointer to the device tree blob
+ * @nodeoffset: offset of the node to nop
+ *
+ * fdt_nop_node() will replace a given node's representation in the
+ * blob, including all its subnodes, if any, with FDT_NOP tags,
+ * effectively removing it from the tree.
+ *
+ * This function will alter only the bytes in the blob which contain
+ * the node and its properties and subnodes, and will not alter or
+ * move any other part of the tree.
+ *
+ * returns:
+ *	0, on success
+ *	-FDT_ERR_BADOFFSET, nodeoffset did not point to FDT_BEGIN_NODE tag
+ *	-FDT_ERR_BADMAGIC,
+ *	-FDT_ERR_BADVERSION,
+ *	-FDT_ERR_BADSTATE,
+ *	-FDT_ERR_BADSTRUCTURE,
+ *	-FDT_ERR_TRUNCATED, standard meanings
+ */
+int fdt_nop_node(void *fdt, int nodeoffset);
+
+/**********************************************************************/
+/* Sequential write functions                                         */
+/**********************************************************************/
+
+int fdt_create(void *buf, int bufsize);
+int fdt_resize(void *fdt, void *buf, int bufsize);
+int fdt_add_reservemap_entry(void *fdt, uint64_t addr, uint64_t size);
+int fdt_finish_reservemap(void *fdt);
+int fdt_begin_node(void *fdt, const char *name);
+int fdt_property(void *fdt, const char *name, const void *val, int len);
+static inline int fdt_property_u32(void *fdt, const char *name, uint32_t val)
+{
+	fdt32_t tmp = cpu_to_fdt32(val);
+	return fdt_property(fdt, name, &tmp, sizeof(tmp));
+}
+static inline int fdt_property_u64(void *fdt, const char *name, uint64_t val)
+{
+	fdt64_t tmp = cpu_to_fdt64(val);
+	return fdt_property(fdt, name, &tmp, sizeof(tmp));
+}
+static inline int fdt_property_cell(void *fdt, const char *name, uint32_t val)
+{
+	return fdt_property_u32(fdt, name, val);
+}
+#define fdt_property_string(fdt, name, str) \
+	fdt_property(fdt, name, str, strlen(str)+1)
+int fdt_end_node(void *fdt);
+int fdt_finish(void *fdt);
+
+/**********************************************************************/
+/* Read-write functions                                               */
+/**********************************************************************/
+
+int fdt_create_empty_tree(void *buf, int bufsize);
+int fdt_open_into(const void *fdt, void *buf, int bufsize);
+int fdt_pack(void *fdt);
+
+/**
+ * fdt_add_mem_rsv - add one memory reserve map entry
+ * @fdt: pointer to the device tree blob
+ * @address, @size: 64-bit values (native endian)
+ *
+ * Adds a reserve map entry to the given blob reserving a region at
+ * address address of length size.
+ *
+ * This function will insert data into the reserve map and will
+ * therefore change the indexes of some entries in the table.
+ *
+ * returns:
+ *	0, on success
+ *	-FDT_ERR_NOSPACE, there is insufficient free space in the blob to
+ *		contain the new reservation entry
+ *	-FDT_ERR_BADMAGIC,
+ *	-FDT_ERR_BADVERSION,
+ *	-FDT_ERR_BADSTATE,
+ *	-FDT_ERR_BADSTRUCTURE,
+ *	-FDT_ERR_BADLAYOUT,
+ *	-FDT_ERR_TRUNCATED, standard meanings
+ */
+int fdt_add_mem_rsv(void *fdt, uint64_t address, uint64_t size);
+
+/**
+ * fdt_del_mem_rsv - remove a memory reserve map entry
+ * @fdt: pointer to the device tree blob
+ * @n: entry to remove
+ *
+ * fdt_del_mem_rsv() removes the n-th memory reserve map entry from
+ * the blob.
+ *
+ * This function will delete data from the reservation table and will
+ * therefore change the indexes of some entries in the table.
+ *
+ * returns:
+ *	0, on success
+ *	-FDT_ERR_NOTFOUND, there is no entry of the given index (i.e. there
+ *		are less than n+1 reserve map entries)
+ *	-FDT_ERR_BADMAGIC,
+ *	-FDT_ERR_BADVERSION,
+ *	-FDT_ERR_BADSTATE,
+ *	-FDT_ERR_BADSTRUCTURE,
+ *	-FDT_ERR_BADLAYOUT,
+ *	-FDT_ERR_TRUNCATED, standard meanings
+ */
+int fdt_del_mem_rsv(void *fdt, int n);
+
+/**
+ * fdt_set_name - change the name of a given node
+ * @fdt: pointer to the device tree blob
+ * @nodeoffset: structure block offset of a node
+ * @name: name to give the node
+ *
+ * fdt_set_name() replaces the name (including unit address, if any)
+ * of the given node with the given string.  NOTE: this function can't
+ * efficiently check if the new name is unique amongst the given
+ * node's siblings; results are undefined if this function is invoked
+ * with a name equal to one of the given node's siblings.
+ *
+ * This function may insert or delete data from the blob, and will
+ * therefore change the offsets of some existing nodes.
+ *
+ * returns:
+ *	0, on success
+ *	-FDT_ERR_NOSPACE, there is insufficient free space in the blob
+ *		to contain the new name
+ *	-FDT_ERR_BADOFFSET, nodeoffset did not point to FDT_BEGIN_NODE tag
+ *	-FDT_ERR_BADMAGIC,
+ *	-FDT_ERR_BADVERSION,
+ *	-FDT_ERR_BADSTATE, standard meanings
+ */
+int fdt_set_name(void *fdt, int nodeoffset, const char *name);
+
+/**
+ * fdt_setprop - create or change a property
+ * @fdt: pointer to the device tree blob
+ * @nodeoffset: offset of the node whose property to change
+ * @name: name of the property to change
+ * @val: pointer to data to set the property value to
+ * @len: length of the property value
+ *
+ * fdt_setprop() sets the value of the named property in the given
+ * node to the given value and length, creating the property if it
+ * does not already exist.
+ *
+ * This function may insert or delete data from the blob, and will
+ * therefore change the offsets of some existing nodes.
+ *
+ * returns:
+ *	0, on success
+ *	-FDT_ERR_NOSPACE, there is insufficient free space in the blob to
+ *		contain the new property value
+ *	-FDT_ERR_BADOFFSET, nodeoffset did not point to FDT_BEGIN_NODE tag
+ *	-FDT_ERR_BADLAYOUT,
+ *	-FDT_ERR_BADMAGIC,
+ *	-FDT_ERR_BADVERSION,
+ *	-FDT_ERR_BADSTATE,
+ *	-FDT_ERR_BADSTRUCTURE,
+ *	-FDT_ERR_BADLAYOUT,
+ *	-FDT_ERR_TRUNCATED, standard meanings
+ */
+int fdt_setprop(void *fdt, int nodeoffset, const char *name,
+		const void *val, int len);
+
+/**
+ * fdt_setprop_u32 - set a property to a 32-bit integer
+ * @fdt: pointer to the device tree blob
+ * @nodeoffset: offset of the node whose property to change
+ * @name: name of the property to change
+ * @val: 32-bit integer value for the property (native endian)
+ *
+ * fdt_setprop_u32() sets the value of the named property in the given
+ * node to the given 32-bit integer value (converting to big-endian if
+ * necessary), or creates a new property with that value if it does
+ * not already exist.
+ *
+ * This function may insert or delete data from the blob, and will
+ * therefore change the offsets of some existing nodes.
+ *
+ * returns:
+ *	0, on success
+ *	-FDT_ERR_NOSPACE, there is insufficient free space in the blob to
+ *		contain the new property value
+ *	-FDT_ERR_BADOFFSET, nodeoffset did not point to FDT_BEGIN_NODE tag
+ *	-FDT_ERR_BADLAYOUT,
+ *	-FDT_ERR_BADMAGIC,
+ *	-FDT_ERR_BADVERSION,
+ *	-FDT_ERR_BADSTATE,
+ *	-FDT_ERR_BADSTRUCTURE,
+ *	-FDT_ERR_BADLAYOUT,
+ *	-FDT_ERR_TRUNCATED, standard meanings
+ */
+static inline int fdt_setprop_u32(void *fdt, int nodeoffset, const char *name,
+				  uint32_t val)
+{
+	fdt32_t tmp = cpu_to_fdt32(val);
+	return fdt_setprop(fdt, nodeoffset, name, &tmp, sizeof(tmp));
+}
+
+/**
+ * fdt_setprop_u64 - set a property to a 64-bit integer
+ * @fdt: pointer to the device tree blob
+ * @nodeoffset: offset of the node whose property to change
+ * @name: name of the property to change
+ * @val: 64-bit integer value for the property (native endian)
+ *
+ * fdt_setprop_u64() sets the value of the named property in the given
+ * node to the given 64-bit integer value (converting to big-endian if
+ * necessary), or creates a new property with that value if it does
+ * not already exist.
+ *
+ * This function may insert or delete data from the blob, and will
+ * therefore change the offsets of some existing nodes.
+ *
+ * returns:
+ *	0, on success
+ *	-FDT_ERR_NOSPACE, there is insufficient free space in the blob to
+ *		contain the new property value
+ *	-FDT_ERR_BADOFFSET, nodeoffset did not point to FDT_BEGIN_NODE tag
+ *	-FDT_ERR_BADLAYOUT,
+ *	-FDT_ERR_BADMAGIC,
+ *	-FDT_ERR_BADVERSION,
+ *	-FDT_ERR_BADSTATE,
+ *	-FDT_ERR_BADSTRUCTURE,
+ *	-FDT_ERR_BADLAYOUT,
+ *	-FDT_ERR_TRUNCATED, standard meanings
+ */
+static inline int fdt_setprop_u64(void *fdt, int nodeoffset, const char *name,
+				  uint64_t val)
+{
+	fdt64_t tmp = cpu_to_fdt64(val);
+	return fdt_setprop(fdt, nodeoffset, name, &tmp, sizeof(tmp));
+}
+
+/**
+ * fdt_setprop_cell - set a property to a single cell value
+ *
+ * This is an alternative name for fdt_setprop_u32()
+ */
+static inline int fdt_setprop_cell(void *fdt, int nodeoffset, const char *name,
+				   uint32_t val)
+{
+	return fdt_setprop_u32(fdt, nodeoffset, name, val);
+}
+
+/**
+ * fdt_setprop_string - set a property to a string value
+ * @fdt: pointer to the device tree blob
+ * @nodeoffset: offset of the node whose property to change
+ * @name: name of the property to change
+ * @str: string value for the property
+ *
+ * fdt_setprop_string() sets the value of the named property in the
+ * given node to the given string value (using the length of the
+ * string to determine the new length of the property), or creates a
+ * new property with that value if it does not already exist.
+ *
+ * This function may insert or delete data from the blob, and will
+ * therefore change the offsets of some existing nodes.
+ *
+ * returns:
+ *	0, on success
+ *	-FDT_ERR_NOSPACE, there is insufficient free space in the blob to
+ *		contain the new property value
+ *	-FDT_ERR_BADOFFSET, nodeoffset did not point to FDT_BEGIN_NODE tag
+ *	-FDT_ERR_BADLAYOUT,
+ *	-FDT_ERR_BADMAGIC,
+ *	-FDT_ERR_BADVERSION,
+ *	-FDT_ERR_BADSTATE,
+ *	-FDT_ERR_BADSTRUCTURE,
+ *	-FDT_ERR_BADLAYOUT,
+ *	-FDT_ERR_TRUNCATED, standard meanings
+ */
+#define fdt_setprop_string(fdt, nodeoffset, name, str) \
+	fdt_setprop((fdt), (nodeoffset), (name), (str), strlen(str)+1)
+
+/**
+ * fdt_appendprop - append to or create a property
+ * @fdt: pointer to the device tree blob
+ * @nodeoffset: offset of the node whose property to change
+ * @name: name of the property to append to
+ * @val: pointer to data to append to the property value
+ * @len: length of the data to append to the property value
+ *
+ * fdt_appendprop() appends the value to the named property in the
+ * given node, creating the property if it does not already exist.
+ *
+ * This function may insert data into the blob, and will therefore
+ * change the offsets of some existing nodes.
+ *
+ * returns:
+ *	0, on success
+ *	-FDT_ERR_NOSPACE, there is insufficient free space in the blob to
+ *		contain the new property value
+ *	-FDT_ERR_BADOFFSET, nodeoffset did not point to FDT_BEGIN_NODE tag
+ *	-FDT_ERR_BADLAYOUT,
+ *	-FDT_ERR_BADMAGIC,
+ *	-FDT_ERR_BADVERSION,
+ *	-FDT_ERR_BADSTATE,
+ *	-FDT_ERR_BADSTRUCTURE,
+ *	-FDT_ERR_BADLAYOUT,
+ *	-FDT_ERR_TRUNCATED, standard meanings
+ */
+int fdt_appendprop(void *fdt, int nodeoffset, const char *name,
+		   const void *val, int len);
+
+/**
+ * fdt_appendprop_u32 - append a 32-bit integer value to a property
+ * @fdt: pointer to the device tree blob
+ * @nodeoffset: offset of the node whose property to change
+ * @name: name of the property to change
+ * @val: 32-bit integer value to append to the property (native endian)
+ *
+ * fdt_appendprop_u32() appends the given 32-bit integer value
+ * (converting to big-endian if necessary) to the value of the named
+ * property in the given node, or creates a new property with that
+ * value if it does not already exist.
+ *
+ * This function may insert data into the blob, and will therefore
+ * change the offsets of some existing nodes.
+ *
+ * returns:
+ *	0, on success
+ *	-FDT_ERR_NOSPACE, there is insufficient free space in the blob to
+ *		contain the new property value
+ *	-FDT_ERR_BADOFFSET, nodeoffset did not point to FDT_BEGIN_NODE tag
+ *	-FDT_ERR_BADLAYOUT,
+ *	-FDT_ERR_BADMAGIC,
+ *	-FDT_ERR_BADVERSION,
+ *	-FDT_ERR_BADSTATE,
+ *	-FDT_ERR_BADSTRUCTURE,
+ *	-FDT_ERR_BADLAYOUT,
+ *	-FDT_ERR_TRUNCATED, standard meanings
+ */
+static inline int fdt_appendprop_u32(void *fdt, int nodeoffset,
+				     const char *name, uint32_t val)
+{
+	fdt32_t tmp = cpu_to_fdt32(val);
+	return fdt_appendprop(fdt, nodeoffset, name, &tmp, sizeof(tmp));
+}
+
+/**
+ * fdt_appendprop_u64 - append a 64-bit integer value to a property
+ * @fdt: pointer to the device tree blob
+ * @nodeoffset: offset of the node whose property to change
+ * @name: name of the property to change
+ * @val: 64-bit integer value to append to the property (native endian)
+ *
+ * fdt_appendprop_u64() appends the given 64-bit integer value
+ * (converting to big-endian if necessary) to the value of the named
+ * property in the given node, or creates a new property with that
+ * value if it does not already exist.
+ *
+ * This function may insert data into the blob, and will therefore
+ * change the offsets of some existing nodes.
+ *
+ * returns:
+ *	0, on success
+ *	-FDT_ERR_NOSPACE, there is insufficient free space in the blob to
+ *		contain the new property value
+ *	-FDT_ERR_BADOFFSET, nodeoffset did not point to FDT_BEGIN_NODE tag
+ *	-FDT_ERR_BADLAYOUT,
+ *	-FDT_ERR_BADMAGIC,
+ *	-FDT_ERR_BADVERSION,
+ *	-FDT_ERR_BADSTATE,
+ *	-FDT_ERR_BADSTRUCTURE,
+ *	-FDT_ERR_BADLAYOUT,
+ *	-FDT_ERR_TRUNCATED, standard meanings
+ */
+static inline int fdt_appendprop_u64(void *fdt, int nodeoffset,
+				     const char *name, uint64_t val)
+{
+	fdt64_t tmp = cpu_to_fdt64(val);
+	return fdt_appendprop(fdt, nodeoffset, name, &tmp, sizeof(tmp));
+}
+
+/**
+ * fdt_appendprop_cell - append a single cell value to a property
+ *
+ * This is an alternative name for fdt_appendprop_u32()
+ */
+static inline int fdt_appendprop_cell(void *fdt, int nodeoffset,
+				      const char *name, uint32_t val)
+{
+	return fdt_appendprop_u32(fdt, nodeoffset, name, val);
+}
+
+/**
+ * fdt_appendprop_string - append a string to a property
+ * @fdt: pointer to the device tree blob
+ * @nodeoffset: offset of the node whose property to change
+ * @name: name of the property to change
+ * @str: string value to append to the property
+ *
+ * fdt_appendprop_string() appends the given string to the value of
+ * the named property in the given node, or creates a new property
+ * with that value if it does not already exist.
+ *
+ * This function may insert data into the blob, and will therefore
+ * change the offsets of some existing nodes.
+ *
+ * returns:
+ *	0, on success
+ *	-FDT_ERR_NOSPACE, there is insufficient free space in the blob to
+ *		contain the new property value
+ *	-FDT_ERR_BADOFFSET, nodeoffset did not point to FDT_BEGIN_NODE tag
+ *	-FDT_ERR_BADLAYOUT,
+ *	-FDT_ERR_BADMAGIC,
+ *	-FDT_ERR_BADVERSION,
+ *	-FDT_ERR_BADSTATE,
+ *	-FDT_ERR_BADSTRUCTURE,
+ *	-FDT_ERR_BADLAYOUT,
+ *	-FDT_ERR_TRUNCATED, standard meanings
+ */
+#define fdt_appendprop_string(fdt, nodeoffset, name, str) \
+	fdt_appendprop((fdt), (nodeoffset), (name), (str), strlen(str)+1)
+
+/**
+ * fdt_delprop - delete a property
+ * @fdt: pointer to the device tree blob
+ * @nodeoffset: offset of the node whose property to nop
+ * @name: name of the property to nop
+ *
+ * fdt_del_property() will delete the given property.
+ *
+ * This function will delete data from the blob, and will therefore
+ * change the offsets of some existing nodes.
+ *
+ * returns:
+ *	0, on success
+ *	-FDT_ERR_NOTFOUND, node does not have the named property
+ *	-FDT_ERR_BADOFFSET, nodeoffset did not point to FDT_BEGIN_NODE tag
+ *	-FDT_ERR_BADLAYOUT,
+ *	-FDT_ERR_BADMAGIC,
+ *	-FDT_ERR_BADVERSION,
+ *	-FDT_ERR_BADSTATE,
+ *	-FDT_ERR_BADSTRUCTURE,
+ *	-FDT_ERR_TRUNCATED, standard meanings
+ */
+int fdt_delprop(void *fdt, int nodeoffset, const char *name);
+
+/**
+ * fdt_add_subnode_namelen - creates a new node based on substring
+ * @fdt: pointer to the device tree blob
+ * @parentoffset: structure block offset of a node
+ * @name: name of the subnode to locate
+ * @namelen: number of characters of name to consider
+ *
+ * Identical to fdt_add_subnode(), but use only the first namelen
+ * characters of name as the name of the new node.  This is useful for
+ * creating subnodes based on a portion of a larger string, such as a
+ * full path.
+ */
+int fdt_add_subnode_namelen(void *fdt, int parentoffset,
+			    const char *name, int namelen);
+
+/**
+ * fdt_add_subnode - creates a new node
+ * @fdt: pointer to the device tree blob
+ * @parentoffset: structure block offset of a node
+ * @name: name of the subnode to locate
+ *
+ * fdt_add_subnode() creates a new node as a subnode of the node at
+ * structure block offset parentoffset, with the given name (which
+ * should include the unit address, if any).
+ *
+ * This function will insert data into the blob, and will therefore
+ * change the offsets of some existing nodes.
+
+ * returns:
+ *	structure block offset of the created nodeequested subnode (>=0), on success
+ *	-FDT_ERR_NOTFOUND, if the requested subnode does not exist
+ *	-FDT_ERR_BADOFFSET, if parentoffset did not point to an FDT_BEGIN_NODE tag
+ *	-FDT_ERR_EXISTS, if the node at parentoffset already has a subnode of
+ *		the given name
+ *	-FDT_ERR_NOSPACE, if there is insufficient free space in the
+ *		blob to contain the new node
+ *	-FDT_ERR_NOSPACE
+ *	-FDT_ERR_BADLAYOUT
+ *      -FDT_ERR_BADMAGIC,
+ *	-FDT_ERR_BADVERSION,
+ *	-FDT_ERR_BADSTATE,
+ *	-FDT_ERR_BADSTRUCTURE,
+ *	-FDT_ERR_TRUNCATED, standard meanings.
+ */
+int fdt_add_subnode(void *fdt, int parentoffset, const char *name);
+
+/**
+ * fdt_del_node - delete a node (subtree)
+ * @fdt: pointer to the device tree blob
+ * @nodeoffset: offset of the node to nop
+ *
+ * fdt_del_node() will remove the given node, including all its
+ * subnodes if any, from the blob.
+ *
+ * This function will delete data from the blob, and will therefore
+ * change the offsets of some existing nodes.
+ *
+ * returns:
+ *	0, on success
+ *	-FDT_ERR_BADOFFSET, nodeoffset did not point to FDT_BEGIN_NODE tag
+ *	-FDT_ERR_BADLAYOUT,
+ *	-FDT_ERR_BADMAGIC,
+ *	-FDT_ERR_BADVERSION,
+ *	-FDT_ERR_BADSTATE,
+ *	-FDT_ERR_BADSTRUCTURE,
+ *	-FDT_ERR_TRUNCATED, standard meanings
+ */
+int fdt_del_node(void *fdt, int nodeoffset);
+
+/**********************************************************************/
+/* Debugging / informational functions                                */
+/**********************************************************************/
+
+const char *fdt_strerror(int errval);
+
+#endif /* _LIBFDT_H */
diff --git a/core/lib/libfdt/include/libfdt_env.h b/core/lib/libfdt/include/libfdt_env.h
new file mode 100644
index 0000000..9dea97d
--- /dev/null
+++ b/core/lib/libfdt/include/libfdt_env.h
@@ -0,0 +1,111 @@
+#ifndef _LIBFDT_ENV_H
+#define _LIBFDT_ENV_H
+/*
+ * libfdt - Flat Device Tree manipulation
+ * Copyright (C) 2006 David Gibson, IBM Corporation.
+ * Copyright 2012 Kim Phillips, Freescale Semiconductor.
+ *
+ * libfdt is dual licensed: you can use it either under the terms of
+ * the GPL, or the BSD license, at your option.
+ *
+ *  a) This library is free software; you can redistribute it and/or
+ *     modify it under the terms of the GNU General Public License as
+ *     published by the Free Software Foundation; either version 2 of the
+ *     License, or (at your option) any later version.
+ *
+ *     This library is distributed in the hope that it will be useful,
+ *     but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *     GNU General Public License for more details.
+ *
+ *     You should have received a copy of the GNU General Public
+ *     License along with this library; if not, write to the Free
+ *     Software Foundation, Inc., 51 Franklin St, Fifth Floor, Boston,
+ *     MA 02110-1301 USA
+ *
+ * Alternatively,
+ *
+ *  b) 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 OWNER 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 <stddef.h>
+#include <stdint.h>
+#include <string.h>
+
+#ifdef __CHECKER__
+#define __force __attribute__((force))
+#define __bitwise __attribute__((bitwise))
+#else
+#define __force
+#define __bitwise
+#endif
+
+typedef uint16_t __bitwise fdt16_t;
+typedef uint32_t __bitwise fdt32_t;
+typedef uint64_t __bitwise fdt64_t;
+
+#define EXTRACT_BYTE(x, n)	((unsigned long long)((uint8_t *)&x)[n])
+#define CPU_TO_FDT16(x) ((EXTRACT_BYTE(x, 0) << 8) | EXTRACT_BYTE(x, 1))
+#define CPU_TO_FDT32(x) ((EXTRACT_BYTE(x, 0) << 24) | (EXTRACT_BYTE(x, 1) << 16) | \
+			 (EXTRACT_BYTE(x, 2) << 8) | EXTRACT_BYTE(x, 3))
+#define CPU_TO_FDT64(x) ((EXTRACT_BYTE(x, 0) << 56) | (EXTRACT_BYTE(x, 1) << 48) | \
+			 (EXTRACT_BYTE(x, 2) << 40) | (EXTRACT_BYTE(x, 3) << 32) | \
+			 (EXTRACT_BYTE(x, 4) << 24) | (EXTRACT_BYTE(x, 5) << 16) | \
+			 (EXTRACT_BYTE(x, 6) << 8) | EXTRACT_BYTE(x, 7))
+
+static inline uint16_t fdt16_to_cpu(fdt16_t x)
+{
+	return (__force uint16_t)CPU_TO_FDT16(x);
+}
+static inline fdt16_t cpu_to_fdt16(uint16_t x)
+{
+	return (__force fdt16_t)CPU_TO_FDT16(x);
+}
+
+static inline uint32_t fdt32_to_cpu(fdt32_t x)
+{
+	return (__force uint32_t)CPU_TO_FDT32(x);
+}
+static inline fdt32_t cpu_to_fdt32(uint32_t x)
+{
+	return (__force fdt32_t)CPU_TO_FDT32(x);
+}
+
+static inline uint64_t fdt64_to_cpu(fdt64_t x)
+{
+	return (__force uint64_t)CPU_TO_FDT64(x);
+}
+static inline fdt64_t cpu_to_fdt64(uint64_t x)
+{
+	return (__force fdt64_t)CPU_TO_FDT64(x);
+}
+#undef CPU_TO_FDT64
+#undef CPU_TO_FDT32
+#undef CPU_TO_FDT16
+#undef EXTRACT_BYTE
+
+#endif /* _LIBFDT_ENV_H */
diff --git a/core/lib/libfdt/libfdt_internal.h b/core/lib/libfdt/libfdt_internal.h
new file mode 100644
index 0000000..02cfa6f
--- /dev/null
+++ b/core/lib/libfdt/libfdt_internal.h
@@ -0,0 +1,95 @@
+#ifndef _LIBFDT_INTERNAL_H
+#define _LIBFDT_INTERNAL_H
+/*
+ * libfdt - Flat Device Tree manipulation
+ * Copyright (C) 2006 David Gibson, IBM Corporation.
+ *
+ * libfdt is dual licensed: you can use it either under the terms of
+ * the GPL, or the BSD license, at your option.
+ *
+ *  a) This library is free software; you can redistribute it and/or
+ *     modify it under the terms of the GNU General Public License as
+ *     published by the Free Software Foundation; either version 2 of the
+ *     License, or (at your option) any later version.
+ *
+ *     This library is distributed in the hope that it will be useful,
+ *     but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *     GNU General Public License for more details.
+ *
+ *     You should have received a copy of the GNU General Public
+ *     License along with this library; if not, write to the Free
+ *     Software Foundation, Inc., 51 Franklin St, Fifth Floor, Boston,
+ *     MA 02110-1301 USA
+ *
+ * Alternatively,
+ *
+ *  b) 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 OWNER 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 <fdt.h>
+
+#define FDT_ALIGN(x, a)		(((x) + (a) - 1) & ~((a) - 1))
+#define FDT_TAGALIGN(x)		(FDT_ALIGN((x), FDT_TAGSIZE))
+
+#define FDT_CHECK_HEADER(fdt) \
+	{ \
+		int __err; \
+		if ((__err = fdt_check_header(fdt)) != 0) \
+			return __err; \
+	}
+
+int _fdt_check_node_offset(const void *fdt, int offset);
+int _fdt_check_prop_offset(const void *fdt, int offset);
+const char *_fdt_find_string(const char *strtab, int tabsize, const char *s);
+int _fdt_node_end_offset(void *fdt, int nodeoffset);
+
+static inline const void *_fdt_offset_ptr(const void *fdt, int offset)
+{
+	return (const char *)fdt + fdt_off_dt_struct(fdt) + offset;
+}
+
+static inline void *_fdt_offset_ptr_w(void *fdt, int offset)
+{
+	return (void *)(uintptr_t)_fdt_offset_ptr(fdt, offset);
+}
+
+static inline const struct fdt_reserve_entry *_fdt_mem_rsv(const void *fdt, int n)
+{
+	const struct fdt_reserve_entry *rsv_table =
+		(const struct fdt_reserve_entry *)
+		((const char *)fdt + fdt_off_mem_rsvmap(fdt));
+
+	return rsv_table + n;
+}
+static inline struct fdt_reserve_entry *_fdt_mem_rsv_w(void *fdt, int n)
+{
+	return (void *)(uintptr_t)_fdt_mem_rsv(fdt, n);
+}
+
+#define FDT_SW_MAGIC		(~FDT_MAGIC)
+
+#endif /* _LIBFDT_INTERNAL_H */
diff --git a/core/lib/libfdt/sub.mk b/core/lib/libfdt/sub.mk
new file mode 100644
index 0000000..b136ed0
--- /dev/null
+++ b/core/lib/libfdt/sub.mk
@@ -0,0 +1,6 @@
+global-incdirs-y += include
+cflags-y += -Wno-cast-align
+cflags-y += -Wno-sign-compare
+cflags-y += -Wno-switch-default
+srcs-y += fdt.c fdt_ro.c fdt_wip.c fdt_sw.c fdt_rw.c fdt_strerror.c
+srcs-y += fdt_empty_tree.c fdt_addresses.c
diff --git a/core/lib/libtomcrypt/include/tomcrypt.h b/core/lib/libtomcrypt/include/tomcrypt.h
new file mode 100644
index 0000000..6864eb7
--- /dev/null
+++ b/core/lib/libtomcrypt/include/tomcrypt.h
@@ -0,0 +1,117 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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 TOMCRYPT_H_
+#define TOMCRYPT_H_
+#include <assert.h>
+#include <stdio.h>
+#include <string.h>
+#include <stdlib.h>
+#include <time.h>
+#include <ctype.h>
+#include <limits.h>
+#include <stddef.h>  // for ptrdiff_t
+
+/* use configuration data */
+#include <tomcrypt_custom.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* version */
+#define CRYPT   0x0117
+#define SCRYPT  "1.17"
+
+/* max size of either a cipher/hash block or symmetric key [largest of the two] */
+#define MAXBLOCKSIZE  128
+
+/* descriptor table size */
+#define TAB_SIZE      32
+
+/* error codes [will be expanded in future releases] */
+enum {
+   CRYPT_OK=0,             /* Result OK */
+   CRYPT_ERROR,            /* Generic Error */
+   CRYPT_NOP,              /* Not a failure but no operation was performed */
+
+   CRYPT_INVALID_KEYSIZE,  /* Invalid key size given */
+   CRYPT_INVALID_ROUNDS,   /* Invalid number of rounds */
+   CRYPT_FAIL_TESTVECTOR,  /* Algorithm failed test vectors */
+
+   CRYPT_BUFFER_OVERFLOW,  /* Not enough space for output */
+   CRYPT_INVALID_PACKET,   /* Invalid input packet given */
+
+   CRYPT_INVALID_PRNGSIZE, /* Invalid number of bits for a PRNG */
+   CRYPT_ERROR_READPRNG,   /* Could not read enough from PRNG */
+
+   CRYPT_INVALID_CIPHER,   /* Invalid cipher specified */
+   CRYPT_INVALID_HASH,     /* Invalid hash specified */
+   CRYPT_INVALID_PRNG,     /* Invalid PRNG specified */
+
+   CRYPT_MEM,              /* Out of memory */
+
+   CRYPT_PK_TYPE_MISMATCH, /* Not equivalent types of PK keys */
+   CRYPT_PK_NOT_PRIVATE,   /* Requires a private PK key */
+
+   CRYPT_INVALID_ARG,      /* Generic invalid argument */
+   CRYPT_FILE_NOTFOUND,    /* File Not Found */
+
+   CRYPT_PK_INVALID_TYPE,  /* Invalid type of PK key */
+   CRYPT_PK_INVALID_SYSTEM,/* Invalid PK system specified */
+   CRYPT_PK_DUP,           /* Duplicate key already in key ring */
+   CRYPT_PK_NOT_FOUND,     /* Key not found in keyring */
+   CRYPT_PK_INVALID_SIZE,  /* Invalid size input for PK parameters */
+
+   CRYPT_INVALID_PRIME_SIZE,/* Invalid size of prime requested */
+   CRYPT_PK_INVALID_PADDING, /* Invalid padding on input */
+
+   CRYPT_HASH_OVERFLOW      /* Hash applied to too many bits */
+};
+
+#include <tomcrypt_cfg.h>
+#include <tomcrypt_macros.h>
+#include <tomcrypt_cipher.h>
+#include <tomcrypt_hash.h>
+#include <tomcrypt_mac.h>
+#include <tomcrypt_prng.h>
+#include <tomcrypt_pk.h>
+#include <tomcrypt_math.h>
+#include <tomcrypt_misc.h>
+#include <tomcrypt_argchk.h>
+#include <tomcrypt_pkcs.h>
+
+#ifdef __cplusplus
+   }
+#endif
+
+#endif /* TOMCRYPT_H_ */
+
+
+/* $Source: /cvs/libtom/libtomcrypt/src/headers/tomcrypt.h,v $ */
+/* $Revision: 1.21 $ */
+/* $Date: 2006/12/16 19:34:05 $ */
diff --git a/core/lib/libtomcrypt/include/tomcrypt_argchk.h b/core/lib/libtomcrypt/include/tomcrypt_argchk.h
new file mode 100644
index 0000000..5476f3e
--- /dev/null
+++ b/core/lib/libtomcrypt/include/tomcrypt_argchk.h
@@ -0,0 +1,71 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* Defines the LTC_ARGCHK macro used within the library */
+/* ARGTYPE is defined in mycrypt_cfg.h */
+#if ARGTYPE == 0
+
+#include <signal.h>
+
+/* this is the default LibTomCrypt macro  */
+#if defined(__clang__) || defined(__GNUC_MINOR__)
+#define NORETURN __attribute__ ((noreturn))
+#else
+#define NORETURN
+#endif
+
+void crypt_argchk(char *v, char *s, int d) NORETURN;
+#define LTC_ARGCHK(x) do { if (!(x)) { crypt_argchk(#x, __FILE__, __LINE__); } }while(0)
+#define LTC_ARGCHKVD(x) do { if (!(x)) { crypt_argchk(#x, __FILE__, __LINE__); } }while(0)
+
+#elif ARGTYPE == 1
+
+/* fatal type of error */
+#define LTC_ARGCHK(x) assert((x))
+#define LTC_ARGCHKVD(x) LTC_ARGCHK(x)
+
+#elif ARGTYPE == 2
+
+#define LTC_ARGCHK(x) if (!(x)) { fprintf(stderr, "\nwarning: ARGCHK failed at %s:%d\n", __FILE__, __LINE__); }
+#define LTC_ARGCHKVD(x) LTC_ARGCHK(x)
+
+#elif ARGTYPE == 3
+
+#define LTC_ARGCHK(x) 
+#define LTC_ARGCHKVD(x) LTC_ARGCHK(x)
+
+#elif ARGTYPE == 4
+
+#define LTC_ARGCHK(x)   if (!(x)) return CRYPT_INVALID_ARG;
+#define LTC_ARGCHKVD(x) if (!(x)) return;
+
+#endif
+
+
+/* $Source: /cvs/libtom/libtomcrypt/src/headers/tomcrypt_argchk.h,v $ */
+/* $Revision: 1.5 $ */
+/* $Date: 2006/08/27 20:50:21 $ */
diff --git a/core/lib/libtomcrypt/include/tomcrypt_arm_neon.h b/core/lib/libtomcrypt/include/tomcrypt_arm_neon.h
new file mode 100644
index 0000000..61a3041
--- /dev/null
+++ b/core/lib/libtomcrypt/include/tomcrypt_arm_neon.h
@@ -0,0 +1,41 @@
+/*
+ * 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.
+ */
+#ifndef TOMCRYPT_ARM_NEON_H
+#define TOMCRYPT_ARM_NEON_H
+
+#include <tomcrypt_macros.h>
+
+struct tomcrypt_arm_neon_state {
+	ulong32 state;
+};
+
+/* Temporarily enables neon instructions */
+void tomcrypt_arm_neon_enable(struct tomcrypt_arm_neon_state *state);
+/* Disables neon instructions after a call to tomcrypt_arm_neon_enable() */
+void tomcrypt_arm_neon_disable(struct tomcrypt_arm_neon_state *state);
+
+#endif /*TOMCRYPT_ARM_NEON_H*/
diff --git a/core/lib/libtomcrypt/include/tomcrypt_cfg.h b/core/lib/libtomcrypt/include/tomcrypt_cfg.h
new file mode 100644
index 0000000..2338971
--- /dev/null
+++ b/core/lib/libtomcrypt/include/tomcrypt_cfg.h
@@ -0,0 +1,197 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/* This is the build config file.
+ *
+ * With this you can setup what to inlcude/exclude automatically during any build.  Just comment
+ * out the line that #define's the word for the thing you want to remove.  phew!
+ */
+
+#ifndef TOMCRYPT_CFG_H
+#define TOMCRYPT_CFG_H
+
+#if defined(_WIN32) || defined(_MSC_VER)
+#define LTC_CALL __cdecl
+#else
+#ifndef LTC_CALL
+   #define LTC_CALL
+#endif
+#endif
+
+#ifndef LTC_EXPORT
+#define LTC_EXPORT
+#endif
+
+/* certain platforms use macros for these, making the prototypes broken */
+#ifndef LTC_NO_PROTOTYPES
+
+/* you can change how memory allocation works ... */
+LTC_EXPORT void * LTC_CALL XMALLOC(size_t n);
+LTC_EXPORT void * LTC_CALL XREALLOC(void *p, size_t n);
+LTC_EXPORT void * LTC_CALL XCALLOC(size_t n, size_t s);
+LTC_EXPORT void LTC_CALL XFREE(void *p);
+
+LTC_EXPORT void LTC_CALL XQSORT(void *base, size_t nmemb, size_t size, int(*compar)(const void *, const void *));
+
+
+/* change the clock function too */
+LTC_EXPORT clock_t LTC_CALL XCLOCK(void);
+
+/* various other functions */
+LTC_EXPORT void * LTC_CALL XMEMCPY(void *dest, const void *src, size_t n);
+LTC_EXPORT int   LTC_CALL XMEMCMP(const void *s1, const void *s2, size_t n);
+LTC_EXPORT void * LTC_CALL XMEMSET(void *s, int c, size_t n);
+
+LTC_EXPORT int   LTC_CALL XSTRCMP(const char *s1, const char *s2);
+
+#endif
+
+/*
+ * with ARGTYPE==4, LTC_ARGCHK() returns an error when an argument is not correct
+ */
+#define ARGTYPE  4
+
+/* type of argument checking, 0=default, 1=fatal and 2=error+continue, 3=nothing */
+#ifndef ARGTYPE
+   #define ARGTYPE  0
+#endif
+
+/* Controls endianess and size of registers.  Leave uncommented to get platform neutral [slower] code 
+ * 
+ * Note: in order to use the optimized macros your platform must support unaligned 32 and 64 bit read/writes.
+ * The x86 platforms allow this but some others [ARM for instance] do not.  On those platforms you **MUST**
+ * use the portable [slower] macros.
+ */
+
+/* detect x86-32 machines somewhat */
+#if !defined(__STRICT_ANSI__) && !defined(__x86_64__) && !defined(_WIN64) && ((defined(_MSC_VER) && defined(WIN32)) || (defined(__GNUC__) && (defined(__DJGPP__) || defined(__CYGWIN__) || defined(__MINGW32__) || defined(__i386__))))
+   #define ENDIAN_LITTLE
+   #define ENDIAN_32BITWORD
+   #define LTC_FAST
+#endif
+
+/* detects MIPS R5900 processors (PS2) */
+#if (defined(__R5900) || defined(R5900) || defined(__R5900__)) && (defined(_mips) || defined(__mips__) || defined(mips))
+   #define ENDIAN_LITTLE
+   #define ENDIAN_64BITWORD
+#endif
+
+/* detect amd64 */
+#if !defined(__STRICT_ANSI__) && defined(__x86_64__)
+   #define ENDIAN_LITTLE
+   #define ENDIAN_64BITWORD
+   #define LTC_FAST
+#endif
+
+/* detect PPC32 */
+#if !defined(__STRICT_ANSI__) && defined(LTC_PPC32)
+   #define ENDIAN_BIG
+   #define ENDIAN_32BITWORD
+   #define LTC_FAST
+#endif   
+
+/* detect sparc and sparc64 */
+#if defined(__sparc__)
+  #define ENDIAN_BIG
+  #if defined(__arch64__)
+    #define ENDIAN_64BITWORD
+  #else
+    #define ENDIAN_32BITWORD
+  #endif
+#endif
+
+
+#ifdef LTC_NO_FAST
+   #ifdef LTC_FAST
+      #undef LTC_FAST
+   #endif
+#endif
+
+#ifdef LTC_FAST
+#if __GNUC__ < 4 /* if the compiler does not support gnu extensions, i.e. its neither clang nor gcc nor icc */
+#error the LTC_FAST hack is only available on compilers that support __attribute__((may_alias)) - disable it for your compiler, and dont worry, it won`t buy you much anyway
+#else
+#ifdef ENDIAN_64BITWORD
+typedef ulong64 __attribute__((__may_alias__)) LTC_FAST_TYPE;
+#else
+typedef ulong32 __attribute__((__may_alias__)) LTC_FAST_TYPE;
+#endif
+#endif
+#endif /* LTC_FAST */
+
+
+/* No asm is a quick way to disable anything "not portable" */
+#ifdef LTC_NO_ASM
+   #undef ENDIAN_LITTLE
+   #undef ENDIAN_BIG
+   #undef ENDIAN_32BITWORD
+   #undef ENDIAN_64BITWORD
+   #undef LTC_FAST
+   #undef LTC_FAST_TYPE
+   #define LTC_NO_ROLC
+	#define LTC_NO_BSWAP
+#endif
+
+/* #define ENDIAN_LITTLE */
+/* #define ENDIAN_BIG */
+
+/* #define ENDIAN_32BITWORD */
+/* #define ENDIAN_64BITWORD */
+
+#if (defined(ENDIAN_BIG) || defined(ENDIAN_LITTLE)) && !(defined(ENDIAN_32BITWORD) || defined(ENDIAN_64BITWORD))
+    #error You must specify a word size as well as endianess in tomcrypt_cfg.h
+#endif
+
+#if !(defined(ENDIAN_BIG) || defined(ENDIAN_LITTLE))
+   #define ENDIAN_NEUTRAL
+#endif
+
+#if (defined(ENDIAN_32BITWORD) && defined(ENDIAN_64BITWORD))
+    #error Can not be 32 and 64 bit words...
+#endif
+
+/* gcc 4.3 and up has a bswap builtin; detect it by gcc version.
+ * clang also supports the bswap builtin, and although clang pretends
+ * to be gcc (macro-wise, anyway), clang pretends to be a version
+ * prior to gcc 4.3, so we can't detect bswap that way.  Instead,
+ * clang has a __has_builtin mechanism that can be used to check
+ * for builtins:
+ * http://clang.llvm.org/docs/LanguageExtensions.html#feature_check */
+#ifndef __has_builtin
+   #define __has_builtin(x) 0
+#endif
+#if !defined(LTC_NO_BSWAP) && defined(__GNUC__) &&                      \
+   ((__GNUC__ * 100 + __GNUC_MINOR__ >= 403) ||                         \
+    (__has_builtin(__builtin_bswap32) && __has_builtin(__builtin_bswap64)))
+   #define LTC_HAVE_BSWAP_BUILTIN
+#endif
+
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/headers/tomcrypt_cfg.h,v $ */
+/* $Revision: 1.19 $ */
+/* $Date: 2006/12/04 02:19:48 $ */
diff --git a/core/lib/libtomcrypt/include/tomcrypt_cipher.h b/core/lib/libtomcrypt/include/tomcrypt_cipher.h
new file mode 100644
index 0000000..9a2647e
--- /dev/null
+++ b/core/lib/libtomcrypt/include/tomcrypt_cipher.h
@@ -0,0 +1,618 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* ---- SYMMETRIC KEY STUFF -----
+ *
+ * We put each of the ciphers scheduled keys in their own structs then we put all of 
+ * the key formats in one union.  This makes the function prototypes easier to use.
+ */
+
+#ifdef LTC_RIJNDAEL
+struct rijndael_key {
+   ulong32 eK[60], dK[60];
+   int Nr;
+};
+#endif
+
+
+#ifdef LTC_DES
+struct des_key {
+    ulong32 ek[32], dk[32];
+};
+
+struct des3_key {
+    ulong32 ek[3][32], dk[3][32];
+};
+#endif
+
+
+typedef union Symmetric_key {
+#ifdef LTC_DES
+   struct des_key des;
+   struct des3_key des3;
+#endif
+#ifdef LTC_RIJNDAEL
+   struct rijndael_key rijndael;
+#endif
+   void   *data;
+} symmetric_key;
+
+#ifdef LTC_ECB_MODE
+/** A block cipher ECB structure */
+typedef struct {
+   /** The index of the cipher chosen */
+   int                 cipher, 
+   /** The block size of the given cipher */
+                       blocklen;
+   /** The scheduled key */                       
+   symmetric_key       key;
+} symmetric_ECB;
+#endif
+
+#ifdef LTC_CFB_MODE
+/** A block cipher CFB structure */
+typedef struct {
+   /** The index of the cipher chosen */
+   int                 cipher, 
+   /** The block size of the given cipher */                        
+                       blocklen, 
+   /** The padding offset */
+                       padlen;
+   /** The current IV */
+   unsigned char       IV[MAXBLOCKSIZE], 
+   /** The pad used to encrypt/decrypt */ 
+                       pad[MAXBLOCKSIZE];
+   /** The scheduled key */
+   symmetric_key       key;
+} symmetric_CFB;
+#endif
+
+#ifdef LTC_OFB_MODE
+/** A block cipher OFB structure */
+typedef struct {
+   /** The index of the cipher chosen */
+   int                 cipher, 
+   /** The block size of the given cipher */                        
+                       blocklen, 
+   /** The padding offset */
+                       padlen;
+   /** The current IV */
+   unsigned char       IV[MAXBLOCKSIZE];
+   /** The scheduled key */
+   symmetric_key       key;
+} symmetric_OFB;
+#endif
+
+#ifdef LTC_CBC_MODE
+/** A block cipher CBC structure */
+typedef struct {
+   /** The index of the cipher chosen */
+   int                 cipher, 
+   /** The block size of the given cipher */                        
+                       blocklen;
+   /** The current IV */
+   unsigned char       IV[MAXBLOCKSIZE];
+   /** The scheduled key */
+   symmetric_key       key;
+} symmetric_CBC;
+#endif
+
+
+#ifdef LTC_CTR_MODE
+/** A block cipher CTR structure */
+typedef struct {
+   /** The index of the cipher chosen */
+   int                 cipher,
+   /** The block size of the given cipher */                        
+                       blocklen, 
+   /** The padding offset */
+                       padlen, 
+   /** The mode (endianess) of the CTR, 0==little, 1==big */
+                       mode,
+   /** counter width */
+                       ctrlen;
+
+   /** The counter */                       
+   unsigned char       ctr[MAXBLOCKSIZE], 
+   /** The pad used to encrypt/decrypt */                       
+                       pad[MAXBLOCKSIZE];
+   /** The scheduled key */
+   symmetric_key       key;
+} symmetric_CTR;
+#endif
+
+
+#ifdef LTC_LRW_MODE
+/** A LRW structure */
+typedef struct {
+    /** The index of the cipher chosen (must be a 128-bit block cipher) */
+    int               cipher;
+
+    /** The current IV */
+    unsigned char     IV[16],
+ 
+    /** the tweak key */
+                      tweak[16],
+
+    /** The current pad, it's the product of the first 15 bytes against the tweak key */
+                      pad[16];
+
+    /** The scheduled symmetric key */
+    symmetric_key     key;
+
+#ifdef LTC_LRW_TABLES
+    /** The pre-computed multiplication table */
+    unsigned char     PC[16][256][16];
+#endif
+} symmetric_LRW;
+#endif
+
+#ifdef LTC_F8_MODE
+/** A block cipher F8 structure */
+typedef struct {
+   /** The index of the cipher chosen */
+   int                 cipher, 
+   /** The block size of the given cipher */                        
+                       blocklen, 
+   /** The padding offset */
+                       padlen;
+   /** The current IV */
+   unsigned char       IV[MAXBLOCKSIZE],
+                       MIV[MAXBLOCKSIZE];
+   /** Current block count */
+   ulong32             blockcnt;
+   /** The scheduled key */
+   symmetric_key       key;
+} symmetric_F8;
+#endif
+
+
+/** cipher descriptor table, last entry has "name == NULL" to mark the end of table */
+extern const struct ltc_cipher_descriptor {
+   /** name of cipher */
+   const char *name;
+   /** internal ID */
+   unsigned char ID;
+   /** min keysize (octets) */
+   int  min_key_length, 
+   /** max keysize (octets) */
+        max_key_length, 
+   /** block size (octets) */
+        block_length, 
+   /** default number of rounds */
+        default_rounds;
+   /** Setup the cipher 
+      @param key         The input symmetric key
+      @param keylen      The length of the input key (octets)
+      @param num_rounds  The requested number of rounds (0==default)
+      @param skey        [out] The destination of the scheduled key
+      @return CRYPT_OK if successful
+   */
+   int  (*setup)(const unsigned char *key, int keylen, int num_rounds, symmetric_key *skey);
+   /** Encrypt a block
+      @param pt      The plaintext
+      @param ct      [out] The ciphertext
+      @param skey    The scheduled key
+      @return CRYPT_OK if successful
+   */
+   int (*ecb_encrypt)(const unsigned char *pt, unsigned char *ct, symmetric_key *skey);
+   /** Decrypt a block
+      @param ct      The ciphertext
+      @param pt      [out] The plaintext
+      @param skey    The scheduled key
+      @return CRYPT_OK if successful
+   */
+   int (*ecb_decrypt)(const unsigned char *ct, unsigned char *pt, symmetric_key *skey);
+   /** Test the block cipher
+       @return CRYPT_OK if successful, CRYPT_NOP if self-testing has been disabled
+   */
+   int (*test)(void);
+
+   /** Terminate the context 
+      @param skey    The scheduled key
+   */
+   void (*done)(symmetric_key *skey);      
+
+   /** Determine a key size
+       @param keysize    [in/out] The size of the key desired and the suggested size
+       @return CRYPT_OK if successful
+   */
+   int  (*keysize)(int *keysize);
+
+/** Accelerators **/
+   /** Accelerated ECB encryption 
+       @param pt      Plaintext
+       @param ct      Ciphertext
+       @param blocks  The number of complete blocks to process
+       @param skey    The scheduled key context
+       @return CRYPT_OK if successful
+   */
+   int (*accel_ecb_encrypt)(const unsigned char *pt, unsigned char *ct, unsigned long blocks, symmetric_key *skey);
+
+   /** Accelerated ECB decryption 
+       @param pt      Plaintext
+       @param ct      Ciphertext
+       @param blocks  The number of complete blocks to process
+       @param skey    The scheduled key context
+       @return CRYPT_OK if successful
+   */
+   int (*accel_ecb_decrypt)(const unsigned char *ct, unsigned char *pt, unsigned long blocks, symmetric_key *skey);
+
+   /** Accelerated CBC encryption 
+       @param pt      Plaintext
+       @param ct      Ciphertext
+       @param blocks  The number of complete blocks to process
+       @param IV      The initial value (input/output)
+       @param skey    The scheduled key context
+       @return CRYPT_OK if successful
+   */
+   int (*accel_cbc_encrypt)(const unsigned char *pt, unsigned char *ct, unsigned long blocks, unsigned char *IV, symmetric_key *skey);
+
+   /** Accelerated CBC decryption 
+       @param pt      Plaintext
+       @param ct      Ciphertext
+       @param blocks  The number of complete blocks to process
+       @param IV      The initial value (input/output)
+       @param skey    The scheduled key context
+       @return CRYPT_OK if successful
+   */
+   int (*accel_cbc_decrypt)(const unsigned char *ct, unsigned char *pt, unsigned long blocks, unsigned char *IV, symmetric_key *skey);
+
+   /** Accelerated CTR encryption 
+       @param pt      Plaintext
+       @param ct      Ciphertext
+       @param blocks  The number of complete blocks to process
+       @param IV      The initial value (input/output)
+       @param mode    little or big endian counter (mode=0 or mode=1)
+       @param skey    The scheduled key context
+       @return CRYPT_OK if successful
+   */
+   int (*accel_ctr_encrypt)(const unsigned char *pt, unsigned char *ct, unsigned long blocks, unsigned char *IV, int mode, symmetric_key *skey);
+
+   /** Accelerated LRW 
+       @param pt      Plaintext
+       @param ct      Ciphertext
+       @param blocks  The number of complete blocks to process
+       @param IV      The initial value (input/output)
+       @param tweak   The LRW tweak
+       @param skey    The scheduled key context
+       @return CRYPT_OK if successful
+   */
+   int (*accel_lrw_encrypt)(const unsigned char *pt, unsigned char *ct, unsigned long blocks, unsigned char *IV, const unsigned char *tweak, symmetric_key *skey);
+
+   /** Accelerated LRW 
+       @param ct      Ciphertext
+       @param pt      Plaintext
+       @param blocks  The number of complete blocks to process
+       @param IV      The initial value (input/output)
+       @param tweak   The LRW tweak
+       @param skey    The scheduled key context
+       @return CRYPT_OK if successful
+   */
+   int (*accel_lrw_decrypt)(const unsigned char *ct, unsigned char *pt, unsigned long blocks, unsigned char *IV, const unsigned char *tweak, symmetric_key *skey);
+
+   /** Accelerated CCM packet (one-shot)
+       @param key        The secret key to use
+       @param keylen     The length of the secret key (octets)
+       @param uskey      A previously scheduled key [optional can be NULL]
+       @param nonce      The session nonce [use once]
+       @param noncelen   The length of the nonce
+       @param header     The header for the session
+       @param headerlen  The length of the header (octets)
+       @param pt         [out] The plaintext
+       @param ptlen      The length of the plaintext (octets)
+       @param ct         [out] The ciphertext
+       @param tag        [out] The destination tag
+       @param taglen     [in/out] The max size and resulting size of the authentication tag
+       @param direction  Encrypt or Decrypt direction (0 or 1)
+       @return CRYPT_OK if successful
+   */
+   int (*accel_ccm_memory)(
+       const unsigned char *key,    unsigned long keylen,
+       symmetric_key       *uskey,
+       const unsigned char *nonce,  unsigned long noncelen,
+       const unsigned char *header, unsigned long headerlen,
+             unsigned char *pt,     unsigned long ptlen,
+             unsigned char *ct,
+             unsigned char *tag,    unsigned long *taglen,
+                       int  direction);
+
+   /** Accelerated GCM packet (one shot)
+       @param key        The secret key
+       @param keylen     The length of the secret key
+       @param IV         The initial vector 
+       @param IVlen      The length of the initial vector
+       @param adata      The additional authentication data (header)
+       @param adatalen   The length of the adata
+       @param pt         The plaintext
+       @param ptlen      The length of the plaintext (ciphertext length is the same)
+       @param ct         The ciphertext
+       @param tag        [out] The MAC tag
+       @param taglen     [in/out] The MAC tag length
+       @param direction  Encrypt or Decrypt mode (GCM_ENCRYPT or GCM_DECRYPT)
+       @return CRYPT_OK on success
+   */
+   int (*accel_gcm_memory)(
+       const unsigned char *key,    unsigned long keylen,
+       const unsigned char *IV,     unsigned long IVlen,
+       const unsigned char *adata,  unsigned long adatalen,
+             unsigned char *pt,     unsigned long ptlen,
+             unsigned char *ct, 
+             unsigned char *tag,    unsigned long *taglen,
+                       int direction);
+
+   /** Accelerated one shot LTC_OMAC 
+       @param key            The secret key
+       @param keylen         The key length (octets) 
+       @param in             The message 
+       @param inlen          Length of message (octets)
+       @param out            [out] Destination for tag
+       @param outlen         [in/out] Initial and final size of out
+       @return CRYPT_OK on success
+   */
+   int (*omac_memory)(
+       const unsigned char *key, unsigned long keylen,
+       const unsigned char *in,  unsigned long inlen,
+             unsigned char *out, unsigned long *outlen);
+
+   /** Accelerated one shot XCBC 
+       @param key            The secret key
+       @param keylen         The key length (octets) 
+       @param in             The message 
+       @param inlen          Length of message (octets)
+       @param out            [out] Destination for tag
+       @param outlen         [in/out] Initial and final size of out
+       @return CRYPT_OK on success
+   */
+   int (*xcbc_memory)(
+       const unsigned char *key, unsigned long keylen,
+       const unsigned char *in,  unsigned long inlen,
+             unsigned char *out, unsigned long *outlen);
+
+   /** Accelerated one shot F9 
+       @param key            The secret key
+       @param keylen         The key length (octets) 
+       @param in             The message 
+       @param inlen          Length of message (octets)
+       @param out            [out] Destination for tag
+       @param outlen         [in/out] Initial and final size of out
+       @return CRYPT_OK on success
+       @remark Requires manual padding
+   */
+   int (*f9_memory)(
+       const unsigned char *key, unsigned long keylen,
+       const unsigned char *in,  unsigned long inlen,
+             unsigned char *out, unsigned long *outlen);
+
+   /** Accelerated XTS encryption
+       @param pt      Plaintext
+       @param ct      Ciphertext
+       @param blocks  The number of complete blocks to process
+       @param tweak   The 128-bit encryption tweak (input/output).
+                      The tweak should not be encrypted on input, but
+                      next tweak will be copied encrypted on output.
+       @param skey1   The first scheduled key context
+       @param skey2   The second scheduled key context
+       @return CRYPT_OK if successful
+    */
+    int (*accel_xts_encrypt)(const unsigned char *pt, unsigned char *ct,
+        unsigned long blocks, unsigned char *tweak, symmetric_key *skey1,
+        symmetric_key *skey2);
+
+    /** Accelerated XTS decryption
+        @param ct      Ciphertext
+        @param pt      Plaintext
+        @param blocks  The number of complete blocks to process
+        @param tweak   The 128-bit encryption tweak (input/output).
+                       The tweak should not be encrypted on input, but
+                       next tweak will be copied encrypted on output.
+        @param skey1   The first scheduled key context
+        @param skey2   The second scheduled key context
+        @return CRYPT_OK if successful
+     */
+     int (*accel_xts_decrypt)(const unsigned char *ct, unsigned char *pt,
+         unsigned long blocks, unsigned char *tweak, symmetric_key *skey1,
+         symmetric_key *skey2);
+} *cipher_descriptor[];
+
+
+/* make aes an alias */
+#define aes_setup           rijndael_setup
+#define aes_ecb_encrypt     rijndael_ecb_encrypt
+#define aes_ecb_decrypt     rijndael_ecb_decrypt
+#define aes_test            rijndael_test
+#define aes_done            rijndael_done
+#define aes_keysize         rijndael_keysize
+
+#define aes_enc_setup           rijndael_enc_setup
+#define aes_enc_ecb_encrypt     rijndael_enc_ecb_encrypt
+#define aes_enc_keysize         rijndael_enc_keysize
+
+int rijndael_setup(const unsigned char *key, int keylen, int num_rounds, symmetric_key *skey);
+int rijndael_ecb_encrypt(const unsigned char *pt, unsigned char *ct, symmetric_key *skey);
+int rijndael_ecb_decrypt(const unsigned char *ct, unsigned char *pt, symmetric_key *skey);
+int rijndael_test(void);
+void rijndael_done(symmetric_key *skey);
+int rijndael_keysize(int *keysize);
+int rijndael_enc_setup(const unsigned char *key, int keylen, int num_rounds, symmetric_key *skey);
+int rijndael_enc_ecb_encrypt(const unsigned char *pt, unsigned char *ct, symmetric_key *skey);
+void rijndael_enc_done(symmetric_key *skey);
+int rijndael_enc_keysize(int *keysize);
+extern const struct ltc_cipher_descriptor rijndael_desc, aes_desc;
+extern const struct ltc_cipher_descriptor rijndael_enc_desc, aes_enc_desc;
+
+
+
+
+int des_setup(const unsigned char *key, int keylen, int num_rounds, symmetric_key *skey);
+int des_ecb_encrypt(const unsigned char *pt, unsigned char *ct, symmetric_key *skey);
+int des_ecb_decrypt(const unsigned char *ct, unsigned char *pt, symmetric_key *skey);
+int des_test(void);
+void des_done(symmetric_key *skey);
+int des_keysize(int *keysize);
+int des3_setup(const unsigned char *key, int keylen, int num_rounds, symmetric_key *skey);
+int des3_ecb_encrypt(const unsigned char *pt, unsigned char *ct, symmetric_key *skey);
+int des3_ecb_decrypt(const unsigned char *ct, unsigned char *pt, symmetric_key *skey);
+int des3_test(void);
+void des3_done(symmetric_key *skey);
+int des3_keysize(int *keysize);
+extern const struct ltc_cipher_descriptor des_desc, des3_desc;
+
+
+#ifdef LTC_ECB_MODE
+int ecb_start(int cipher, const unsigned char *key, 
+              int keylen, int num_rounds, symmetric_ECB *ecb);
+int ecb_encrypt(const unsigned char *pt, unsigned char *ct, unsigned long len, symmetric_ECB *ecb);
+int ecb_decrypt(const unsigned char *ct, unsigned char *pt, unsigned long len, symmetric_ECB *ecb);
+int ecb_done(symmetric_ECB *ecb);
+#endif
+
+#ifdef LTC_CFB_MODE
+int cfb_start(int cipher, const unsigned char *IV, const unsigned char *key, 
+              int keylen, int num_rounds, symmetric_CFB *cfb);
+int cfb_encrypt(const unsigned char *pt, unsigned char *ct, unsigned long len, symmetric_CFB *cfb);
+int cfb_decrypt(const unsigned char *ct, unsigned char *pt, unsigned long len, symmetric_CFB *cfb);
+int cfb_getiv(unsigned char *IV, unsigned long *len, symmetric_CFB *cfb);
+int cfb_setiv(const unsigned char *IV, unsigned long len, symmetric_CFB *cfb);
+int cfb_done(symmetric_CFB *cfb);
+#endif
+
+#ifdef LTC_OFB_MODE
+int ofb_start(int cipher, const unsigned char *IV, const unsigned char *key, 
+              int keylen, int num_rounds, symmetric_OFB *ofb);
+int ofb_encrypt(const unsigned char *pt, unsigned char *ct, unsigned long len, symmetric_OFB *ofb);
+int ofb_decrypt(const unsigned char *ct, unsigned char *pt, unsigned long len, symmetric_OFB *ofb);
+int ofb_getiv(unsigned char *IV, unsigned long *len, symmetric_OFB *ofb);
+int ofb_setiv(const unsigned char *IV, unsigned long len, symmetric_OFB *ofb);
+int ofb_done(symmetric_OFB *ofb);
+#endif
+
+#ifdef LTC_CBC_MODE
+int cbc_start(int cipher, const unsigned char *IV, const unsigned char *key,
+               int keylen, int num_rounds, symmetric_CBC *cbc);
+int cbc_encrypt(const unsigned char *pt, unsigned char *ct, unsigned long len, symmetric_CBC *cbc);
+int cbc_decrypt(const unsigned char *ct, unsigned char *pt, unsigned long len, symmetric_CBC *cbc);
+int cbc_getiv(unsigned char *IV, unsigned long *len, symmetric_CBC *cbc);
+int cbc_setiv(const unsigned char *IV, unsigned long len, symmetric_CBC *cbc);
+int cbc_done(symmetric_CBC *cbc);
+#endif
+
+#ifdef LTC_CTR_MODE
+
+#define CTR_COUNTER_LITTLE_ENDIAN    0x0000
+#define CTR_COUNTER_BIG_ENDIAN       0x1000
+#define LTC_CTR_RFC3686              0x2000
+
+int ctr_start(               int   cipher,
+              const unsigned char *IV,
+              const unsigned char *key,       int keylen,
+                             int  num_rounds, int ctr_mode,
+                   symmetric_CTR *ctr);
+int ctr_encrypt(const unsigned char *pt, unsigned char *ct, unsigned long len, symmetric_CTR *ctr);
+int ctr_decrypt(const unsigned char *ct, unsigned char *pt, unsigned long len, symmetric_CTR *ctr);
+int ctr_getiv(unsigned char *IV, unsigned long *len, symmetric_CTR *ctr);
+int ctr_setiv(const unsigned char *IV, unsigned long len, symmetric_CTR *ctr);
+int ctr_done(symmetric_CTR *ctr);
+int ctr_test(void);
+#endif
+
+#ifdef LTC_LRW_MODE
+
+#define LRW_ENCRYPT 0
+#define LRW_DECRYPT 1
+
+int lrw_start(               int   cipher,
+              const unsigned char *IV,
+              const unsigned char *key,       int keylen,
+              const unsigned char *tweak,
+                             int  num_rounds, 
+                   symmetric_LRW *lrw);
+int lrw_encrypt(const unsigned char *pt, unsigned char *ct, unsigned long len, symmetric_LRW *lrw);
+int lrw_decrypt(const unsigned char *ct, unsigned char *pt, unsigned long len, symmetric_LRW *lrw);
+int lrw_getiv(unsigned char *IV, unsigned long *len, symmetric_LRW *lrw);
+int lrw_setiv(const unsigned char *IV, unsigned long len, symmetric_LRW *lrw);
+int lrw_done(symmetric_LRW *lrw);
+int lrw_test(void);
+
+/* don't call */
+int lrw_process(const unsigned char *pt, unsigned char *ct, unsigned long len, int mode, symmetric_LRW *lrw);
+#endif    
+
+#ifdef LTC_F8_MODE
+int f8_start(                int  cipher, const unsigned char *IV, 
+             const unsigned char *key,                    int  keylen, 
+             const unsigned char *salt_key,               int  skeylen,
+                             int  num_rounds,   symmetric_F8  *f8);
+int f8_encrypt(const unsigned char *pt, unsigned char *ct, unsigned long len, symmetric_F8 *f8);
+int f8_decrypt(const unsigned char *ct, unsigned char *pt, unsigned long len, symmetric_F8 *f8);
+int f8_getiv(unsigned char *IV, unsigned long *len, symmetric_F8 *f8);
+int f8_setiv(const unsigned char *IV, unsigned long len, symmetric_F8 *f8);
+int f8_done(symmetric_F8 *f8);
+int f8_test_mode(void);
+#endif
+
+#ifdef LTC_XTS_MODE
+typedef struct {
+   symmetric_key  key1, key2;
+   int            cipher;
+} symmetric_xts;
+
+int xts_start(                int  cipher,
+              const unsigned char *key1, 
+              const unsigned char *key2, 
+                    unsigned long  keylen,
+                              int  num_rounds, 
+                    symmetric_xts *xts);
+
+int xts_encrypt(
+   const unsigned char *pt, unsigned long ptlen,
+         unsigned char *ct,
+         unsigned char *tweak,
+         symmetric_xts *xts);
+int xts_decrypt(
+   const unsigned char *ct, unsigned long ptlen,
+         unsigned char *pt,
+         unsigned char *tweak,
+         symmetric_xts *xts);
+
+void xts_done(symmetric_xts *xts);
+int  xts_test(void);
+void xts_mult_x(unsigned char *I);
+#endif
+
+int find_cipher(const char *name);
+int find_cipher_any(const char *name, int blocklen, int keylen);
+int find_cipher_id(unsigned char ID);
+int register_cipher(const struct ltc_cipher_descriptor *cipher);
+int unregister_cipher(const struct ltc_cipher_descriptor *cipher);
+int cipher_is_valid(int idx);
+
+LTC_MUTEX_PROTO(ltc_cipher_mutex)
+
+/* $Source: /cvs/libtom/libtomcrypt/src/headers/tomcrypt_cipher.h,v $ */
+/* $Revision: 1.54 $ */
+/* $Date: 2007/05/12 14:37:41 $ */
diff --git a/core/lib/libtomcrypt/include/tomcrypt_custom.h b/core/lib/libtomcrypt/include/tomcrypt_custom.h
new file mode 100644
index 0000000..aadce56
--- /dev/null
+++ b/core/lib/libtomcrypt/include/tomcrypt_custom.h
@@ -0,0 +1,545 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+#ifndef TOMCRYPT_CUSTOM_H_
+#define TOMCRYPT_CUSTOM_H_
+
+#define LTC_NO_PROTOTYPES
+#define LTC_SOURCE
+#define LTC_NO_TABLES
+// #define LTC_VERBOSE
+#define LTC_NO_TEST
+
+/* macros for various libc functions you can change for embedded targets */
+#ifndef XMALLOC
+   #ifdef malloc 
+   #define LTC_NO_PROTOTYPES
+   #endif
+#define XMALLOC  malloc
+#endif
+#ifndef XREALLOC
+   #ifdef realloc 
+   #define LTC_NO_PROTOTYPES
+   #endif
+#define XREALLOC realloc
+#endif
+#ifndef XCALLOC
+   #ifdef calloc 
+   #define LTC_NO_PROTOTYPES
+   #endif
+#define XCALLOC  calloc
+#endif
+#ifndef XFREE
+   #ifdef free
+   #define LTC_NO_PROTOTYPES
+   #endif
+#define XFREE    free
+#endif
+
+#ifndef XMEMSET
+   #ifdef memset
+   #define LTC_NO_PROTOTYPES
+   #endif
+#define XMEMSET  memset
+#endif
+#ifndef XMEMCPY
+   #ifdef memcpy
+   #define LTC_NO_PROTOTYPES
+   #endif
+#define XMEMCPY  memcpy
+#endif
+#ifndef XMEMCMP
+   #ifdef memcmp 
+   #define LTC_NO_PROTOTYPES
+   #endif
+#define XMEMCMP  memcmp
+#endif
+#ifndef XMEM_NEQ
+#include <string_ext.h>
+#define XMEM_NEQ  buf_compare_ct
+#endif
+#ifndef XSTRCMP
+   #ifdef strcmp
+   #define LTC_NO_PROTOTYPES
+   #endif
+#define XSTRCMP strcmp
+#endif
+
+#ifndef XCLOCK
+#define XCLOCK   clock
+#endif
+#ifndef XCLOCKS_PER_SEC
+#define XCLOCKS_PER_SEC CLOCKS_PER_SEC
+#endif
+
+#ifndef XQSORT
+   #ifdef qsort
+   #define LTC_NO_PROTOTYPES
+   #endif
+#define XQSORT qsort
+#endif
+
+/* Easy button? */
+#ifdef LTC_EASY
+   #define LTC_NO_CIPHERS
+   #define LTC_RIJNDAEL
+   #define LTC_BLOWFISH
+   #define LTC_DES
+   #define LTC_CAST5
+   
+   #define LTC_NO_MODES
+   #define LTC_ECB_MODE
+   #define LTC_CBC_MODE
+   #define LTC_CTR_MODE
+   
+   #define LTC_NO_HASHES
+   #define LTC_SHA1
+   #define LTC_SHA512
+   #define LTC_SHA384
+   #define LTC_SHA256
+   #define LTC_SHA224
+
+
+   #define LTC_NO_MACS
+   #define LTC_HMAC
+   #define LTC_OMAC
+   #define LTC_CCM_MODE
+
+   #define LTC_NO_PRNGS
+   #define LTC_SPRNG
+   #define LTC_DEVRANDOM
+   #define LTC_TRY_URANDOM_FIRST
+      
+   #define LTC_NO_PK
+   #define LTC_MRSA
+   #define LTC_MECC
+#endif   
+
+/* Set LTC_ options based on OP-TEE configuration */
+
+#define LTC_NO_CIPHERS
+
+#ifdef CFG_CRYPTO_AES
+   #define LTC_RIJNDAEL
+#endif
+#ifdef CFG_CRYPTO_DES
+   #define LTC_DES
+#endif
+
+#define LTC_NO_MODES
+
+#ifdef CFG_CRYPTO_ECB
+   #define LTC_ECB_MODE
+#endif
+#if defined(CFG_CRYPTO_CBC) || defined(CFG_CRYPTO_CBC_MAC)
+   #define LTC_CBC_MODE
+#endif
+#ifdef CFG_CRYPTO_CTR
+   #define LTC_CTR_MODE
+#endif
+#ifdef CFG_CRYPTO_XTS
+   #define LTC_XTS_MODE
+#endif
+
+#define LTC_NO_HASHES
+
+#ifdef CFG_CRYPTO_MD5
+#define LTC_MD5
+#endif
+#ifdef CFG_CRYPTO_SHA1
+#define LTC_SHA1
+#endif
+#ifdef CFG_CRYPTO_SHA1_ARM32_CE
+#define LTC_SHA1_ARM32_CE
+#endif
+#ifdef CFG_CRYPTO_SHA1_ARM64_CE
+#define LTC_SHA1_ARM64_CE
+#endif
+#ifdef CFG_CRYPTO_SHA224
+#define LTC_SHA224
+#endif
+#ifdef CFG_CRYPTO_SHA256
+#define LTC_SHA256
+#endif
+#ifdef CFG_CRYPTO_SHA256_ARM32_CE
+#define LTC_SHA256_ARM32_CE
+#endif
+#ifdef CFG_CRYPTO_SHA256_ARM64_CE
+#define LTC_SHA256_ARM64_CE
+#endif
+#ifdef CFG_CRYPTO_SHA384
+#define LTC_SHA384
+#endif
+#ifdef CFG_CRYPTO_SHA512
+#define LTC_SHA512
+#endif
+
+#define LTC_NO_MACS
+
+#ifdef CFG_CRYPTO_HMAC
+   #define LTC_HMAC
+#endif
+#ifdef CFG_CRYPTO_CMAC
+   #define LTC_OMAC
+#endif
+#ifdef CFG_CRYPTO_CCM
+   #define LTC_CCM_MODE
+#endif
+#ifdef CFG_CRYPTO_GCM
+   #define LTC_GCM_MODE
+#endif
+
+#define LTC_NO_PK
+
+#ifdef CFG_CRYPTO_RSA
+   #define LTC_MRSA
+#endif
+#ifdef CFG_CRYPTO_DSA
+   #define LTC_MDSA
+#endif
+#ifdef CFG_CRYPTO_DH
+   #define LTC_MDH
+#endif
+#ifdef CFG_CRYPTO_ECC
+   #define LTC_MECC
+
+   /* use Shamir's trick for point mul (speeds up signature verification) */
+   #define LTC_ECC_SHAMIR
+
+   #if defined(TFM_LTC_DESC) && defined(LTC_MECC)
+   #define LTC_MECC_ACCEL
+   #endif
+
+   /* do we want fixed point ECC */
+   /* #define LTC_MECC_FP */
+
+   /* Timing Resistant */
+   #define LTC_ECC_TIMING_RESISTANT
+
+   #define LTC_ECC192
+   #define LTC_ECC224
+   #define LTC_ECC256
+   #define LTC_ECC384
+   #define LTC_ECC521
+
+   /* ECC 521 bits is the max supported key size */
+   #define LTC_MAX_ECC 521
+#endif
+
+#define LTC_NO_PKCS
+
+#if defined(CFG_CRYPTO_RSA) || defined(CFG_CRYPTO_DSA) || \
+	    defined(CFG_CRYPTO_ECC)
+   #define LTC_DER
+#endif
+
+/* Use small code where possible */
+/* #define LTC_SMALL_CODE */
+
+/* Enable self-test test vector checking */
+#ifndef LTC_NO_TEST
+   #define LTC_TEST
+#endif
+
+/* clean the stack of functions which put private information on stack */
+/* #define LTC_CLEAN_STACK */
+
+/* disable all file related functions */
+#define LTC_NO_FILE
+
+/* disable all forms of ASM */
+#define LTC_NO_ASM
+
+/* disable FAST mode */
+/* #define LTC_NO_FAST */
+
+/* disable BSWAP on x86 */
+/* #define LTC_NO_BSWAP */
+
+/* ---> Symmetric Block Ciphers <--- */
+
+#ifndef LTC_NO_CIPHERS
+
+#define LTC_RIJNDAEL
+
+/* LTC_DES includes EDE triple-LTC_DES */
+#define LTC_DES
+
+#endif
+
+/* ---> Block Cipher Modes of Operation <--- */
+#ifndef LTC_NO_MODES
+
+#define LTC_CFB_MODE
+#define LTC_OFB_MODE
+#define LTC_ECB_MODE
+#define LTC_CBC_MODE
+#define LTC_CTR_MODE
+
+/* F8 chaining mode */
+#define LTC_F8_MODE
+
+/* LRW mode */
+#define LTC_LRW_MODE
+#ifndef LTC_NO_TABLES
+   /* like GCM mode this will enable 16 8x128 tables [64KB] that make
+    * seeking very fast.  
+    */
+   #define LTC_LRW_TABLES
+#endif
+
+/* XTS mode */
+#define LTC_XTS_MODE
+
+#endif /* LTC_NO_MODES */
+
+/* ---> One-Way Hash Functions <--- */
+#ifndef LTC_NO_HASHES 
+
+#define LTC_SHA512
+#define LTC_SHA384
+#define LTC_SHA256
+#define LTC_SHA224
+#define LTC_SHA1
+#define LTC_MD5
+
+
+
+#endif /* LTC_NO_HASHES */
+
+/* ---> MAC functions <--- */
+#ifndef LTC_NO_MACS
+
+#define LTC_HMAC
+#define LTC_OMAC
+#define LTC_PMAC
+#define LTC_XCBC
+
+
+/* ---> Encrypt + Authenticate Modes <--- */
+
+#define LTC_EAX_MODE
+#if defined(LTC_EAX_MODE) && !(defined(LTC_CTR_MODE) && defined(LTC_OMAC))
+   #error LTC_EAX_MODE requires CTR and LTC_OMAC mode
+#endif
+
+#define LTC_OCB_MODE
+#define LTC_CCM_MODE
+#define LTC_GCM_MODE
+
+/* Use 64KiB tables */
+#ifndef LTC_NO_TABLES
+   #define LTC_GCM_TABLES 
+#endif
+
+/* USE SSE2? requires GCC works on x86_32 and x86_64*/
+#ifdef LTC_GCM_TABLES
+/* #define LTC_GCM_TABLES_SSE2 */
+#endif
+
+#endif /* LTC_NO_MACS */
+
+/* Various tidbits of modern neatoness */
+#define LTC_BASE64
+
+/* --> Pseudo Random Number Generators <--- */
+#ifndef LTC_NO_PRNGS
+
+/* a PRNG that simply reads from an available system source */
+#define LTC_SPRNG
+
+/* The LTC_RC4 stream cipher */
+#define LTC_RC4
+
+/* Fortuna PRNG */
+#define LTC_FORTUNA
+/* reseed every N calls to the read function */
+#define LTC_FORTUNA_WD    10
+/* number of pools (4..32) can save a bit of ram by lowering the count */
+#define LTC_FORTUNA_POOLS 32
+
+/* the *nix style /dev/random device */
+#define LTC_DEVRANDOM
+/* try /dev/urandom before trying /dev/random */
+#define LTC_TRY_URANDOM_FIRST
+
+#endif /* LTC_NO_PRNGS */
+
+/* ---> Public Key Crypto <--- */
+#ifndef LTC_NO_PK
+
+/* Include RSA support */
+#define LTC_MRSA
+
+/* Include Diffie-Hellman support */
+/*
+ * From libtomcrypt.org:
+ *     DH vanished because nobody used it and it was a pain to support
+ *     DH support rewritten by ST
+ */
+#define LTC_MDH
+
+/* Include Katja (a Rabin variant like RSA) */
+/* #define LTC_MKAT */
+
+/* Digital Signature Algorithm */
+#define LTC_MDSA
+
+/* ECC */
+#define LTC_MECC
+
+/* use Shamir's trick for point mul (speeds up signature verification) */
+#define LTC_ECC_SHAMIR
+
+#if defined(TFM_LTC_DESC) && defined(LTC_MECC)
+   #define LTC_MECC_ACCEL
+#endif   
+
+/* do we want fixed point ECC */
+/* #define LTC_MECC_FP */
+
+/* Timing Resistant? */
+/* #define LTC_ECC_TIMING_RESISTANT */
+
+#endif /* LTC_NO_PK */
+
+/* in cases where you want ASN.1/DER functionality, but no
+ * RSA, you can define this externally if 1024 is not enough
+ */
+#if defined(LTC_MRSA)
+#define LTC_DER_MAX_PUBKEY_SIZE MAX_RSA_SIZE
+#elif !defined(LTC_DER_MAX_PUBKEY_SIZE)
+/* this includes DSA */
+#define LTC_DER_MAX_PUBKEY_SIZE 1024
+#endif
+
+/* LTC_PKCS #1 (RSA) and #5 (Password Handling) stuff */
+#ifndef LTC_NO_PKCS
+
+#define LTC_PKCS_1
+#define LTC_PKCS_5
+
+/* Include ASN.1 DER (required by DSA/RSA) */
+#define LTC_DER
+
+#endif /* LTC_NO_PKCS */
+
+/* cleanup */
+
+#if defined(LTC_MECC) || defined(LTC_MRSA) || defined(LTC_MDSA) || \
+	defined(MKATJA) || defined(LTC_MDH)
+   /* Include the MPI functionality?  (required by the PK algorithms) */
+   #define LTC_MPI
+#endif
+
+#ifdef LTC_MRSA
+   #define LTC_PKCS_1
+#endif   
+
+#if defined(LTC_DER) && !defined(LTC_MPI)
+   #error ASN.1 DER requires MPI functionality
+#endif
+
+#if (defined(LTC_MDSA) || defined(LTC_MRSA) || defined(LTC_MECC) || defined(MKATJA)) && !defined(LTC_DER)
+   #error PK requires ASN.1 DER functionality, make sure LTC_DER is enabled
+#endif
+
+
+/* THREAD management */
+#if defined(CFG_LTC_OPTEE_THREAD)
+
+#include <kernel/mutex.h>
+
+#define LTC_MUTEX_GLOBAL(x)   struct mutex x = MUTEX_INITIALIZER;
+#define LTC_MUTEX_PROTO(x)    extern struct mutex x;
+#define LTC_MUTEX_TYPE(x)     struct mutex x;
+#define LTC_MUTEX_INIT(x)     mutex_init(x);
+#define LTC_MUTEX_LOCK(x)     mutex_lock(x);
+#define LTC_MUTEX_UNLOCK(x)   mutex_unlock(x);
+
+#elif defined(LTC_PTHREAD)
+
+#include <pthread.h>
+
+#define LTC_MUTEX_GLOBAL(x)   pthread_mutex_t x = PTHREAD_MUTEX_INITIALIZER;
+#define LTC_MUTEX_PROTO(x)    extern pthread_mutex_t x;
+#define LTC_MUTEX_TYPE(x)     pthread_mutex_t x;
+#define LTC_MUTEX_INIT(x)     pthread_mutex_init(x, NULL);
+#define LTC_MUTEX_LOCK(x)     pthread_mutex_lock(x);
+#define LTC_MUTEX_UNLOCK(x)   pthread_mutex_unlock(x);
+
+#else
+
+/* default no functions */
+#define LTC_MUTEX_GLOBAL(x)
+#define LTC_MUTEX_PROTO(x)
+#define LTC_MUTEX_TYPE(x)
+#define LTC_MUTEX_INIT(x)
+#define LTC_MUTEX_LOCK(x)
+#define LTC_MUTEX_UNLOCK(x)
+
+#endif
+
+/*
+ * Here are a list of fixes required in libtomcrypt
+ */
+
+#define LTC_LINARO_FIX_RSAWITHOUTCRT
+
+/*
+ * From libtomcrypt.org:
+ *     DH vanished because nobody used it and it was a pain to support
+ * DH support was adapted from the master branch of libtomcrypt that can be
+ * found at
+ *     http://dev.openaos.org/browser/trunk/buildroot/gen7/buildroot/package/libtomcrypt/libtomcrypt-dh.patch
+ * The original version was not taken as it makes use of static const array
+ * containing base and prime, and did not include subprime and x-bits
+ * constraints
+ */
+#define LTC_LINARO_FIX_DH
+
+/*
+ * XTS encryption / decryption does not update the tweak when successive
+ * operations are performed.
+ * Defining LTC_LINARO_FIX_XTS fixes this.
+ */
+#define LTC_LINARO_FIX_XTS
+
+/* Debuggers */
+
+/* define this if you use Valgrind, note: it CHANGES the way SOBER-128 and LTC_RC4 work (see the code) */
+/* #define LTC_VALGRIND */
+
+#endif
+
+
+
+/* $Source: /cvs/libtom/libtomcrypt/src/headers/tomcrypt_custom.h,v $ */
+/* $Revision: 1.73 $ */
+/* $Date: 2007/05/12 14:37:41 $ */
diff --git a/core/lib/libtomcrypt/include/tomcrypt_hash.h b/core/lib/libtomcrypt/include/tomcrypt_hash.h
new file mode 100644
index 0000000..18ff4a2
--- /dev/null
+++ b/core/lib/libtomcrypt/include/tomcrypt_hash.h
@@ -0,0 +1,427 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* ---- HASH FUNCTIONS ---- */
+#ifdef LTC_SHA512
+struct sha512_state {
+    ulong64  length, state[8];
+    unsigned long curlen;
+    unsigned char buf[128];
+};
+#endif
+
+#if defined(LTC_SHA256) || defined(LTC_SHA256_ARM32_CE)
+struct sha256_state {
+    ulong64 length;
+    ulong32 state[8], curlen;
+    unsigned char buf[64];
+};
+#endif
+
+#if defined(LTC_SHA1) || defined(LTC_SHA1_ARM32_CE) || defined(LTC_SHA1_ARM64_CE)
+struct sha1_state {
+    ulong64 length;
+    ulong32 state[5], curlen;
+    unsigned char buf[64];
+};
+#endif
+
+#ifdef LTC_MD5
+struct md5_state {
+    ulong64 length;
+    ulong32 state[4], curlen;
+    unsigned char buf[64];
+};
+#endif
+
+#ifdef LTC_MD4
+struct md4_state {
+    ulong64 length;
+    ulong32 state[4], curlen;
+    unsigned char buf[64];
+};
+#endif
+
+#ifdef LTC_TIGER
+struct tiger_state {
+    ulong64 state[3], length;
+    unsigned long curlen;
+    unsigned char buf[64];
+};
+#endif
+
+#ifdef LTC_MD2
+struct md2_state {
+    unsigned char chksum[16], X[48], buf[16];
+    unsigned long curlen;
+};
+#endif
+
+#ifdef LTC_RIPEMD128
+struct rmd128_state {
+    ulong64 length;
+    unsigned char buf[64];
+    ulong32 curlen, state[4];
+};
+#endif
+
+#ifdef LTC_RIPEMD160
+struct rmd160_state {
+    ulong64 length;
+    unsigned char buf[64];
+    ulong32 curlen, state[5];
+};
+#endif
+
+#ifdef LTC_RIPEMD256
+struct rmd256_state {
+    ulong64 length;
+    unsigned char buf[64];
+    ulong32 curlen, state[8];
+};
+#endif
+
+#ifdef LTC_RIPEMD320
+struct rmd320_state {
+    ulong64 length;
+    unsigned char buf[64];
+    ulong32 curlen, state[10];
+};
+#endif
+
+#ifdef LTC_WHIRLPOOL
+struct whirlpool_state {
+    ulong64 length, state[8];
+    unsigned char buf[64];
+    ulong32 curlen;
+};
+#endif
+
+#ifdef LTC_CHC_HASH
+struct chc_state {
+    ulong64 length;
+    unsigned char state[MAXBLOCKSIZE], buf[MAXBLOCKSIZE];
+    ulong32 curlen;
+};
+#endif
+
+typedef union Hash_state {
+    char dummy[1];
+#ifdef LTC_CHC_HASH
+    struct chc_state chc;
+#endif
+#ifdef LTC_WHIRLPOOL
+    struct whirlpool_state whirlpool;
+#endif
+#ifdef LTC_SHA512
+    struct sha512_state sha512;
+#endif
+#if defined(LTC_SHA256) || defined(LTC_SHA256_ARM32_CE)
+    struct sha256_state sha256;
+#endif
+#if defined(LTC_SHA1) || defined(LTC_SHA1_ARM32_CE)
+    struct sha1_state   sha1;
+#endif
+#ifdef LTC_MD5
+    struct md5_state    md5;
+#endif
+#ifdef LTC_MD4
+    struct md4_state    md4;
+#endif
+#ifdef LTC_MD2
+    struct md2_state    md2;
+#endif
+#ifdef LTC_TIGER
+    struct tiger_state  tiger;
+#endif
+#ifdef LTC_RIPEMD128
+    struct rmd128_state rmd128;
+#endif
+#ifdef LTC_RIPEMD160
+    struct rmd160_state rmd160;
+#endif
+#ifdef LTC_RIPEMD256
+    struct rmd256_state rmd256;
+#endif
+#ifdef LTC_RIPEMD320
+    struct rmd320_state rmd320;
+#endif
+    void *data;
+} hash_state;
+
+/** hash descriptor */
+extern  const struct ltc_hash_descriptor {
+    /** name of hash */
+    const char *name;
+    /** internal ID */
+    unsigned char ID;
+    /** Size of digest in octets */
+    unsigned long hashsize;
+    /** Input block size in octets */
+    unsigned long blocksize;
+    /** ASN.1 OID */
+    unsigned long OID[16];
+    /** Length of DER encoding */
+    unsigned long OIDlen;
+
+    /** Init a hash state
+      @param hash   The hash to initialize
+      @return CRYPT_OK if successful
+    */
+    int (*init)(hash_state *hash);
+    /** Process a block of data 
+      @param hash   The hash state
+      @param in     The data to hash
+      @param inlen  The length of the data (octets)
+      @return CRYPT_OK if successful
+    */
+    int (*process)(hash_state *hash, const unsigned char *in, unsigned long inlen);
+    /** Produce the digest and store it
+      @param hash   The hash state
+      @param out    [out] The destination of the digest
+      @return CRYPT_OK if successful
+    */
+    int (*done)(hash_state *hash, unsigned char *out);
+    /** Self-test
+      @return CRYPT_OK if successful, CRYPT_NOP if self-tests have been disabled
+    */
+    int (*test)(void);
+
+    /* accelerated hmac callback: if you need to-do multiple packets just use the generic hmac_memory and provide a hash callback */
+    int  (*hmac_block)(const unsigned char *key, unsigned long  keylen,
+                       const unsigned char *in,  unsigned long  inlen, 
+                             unsigned char *out, unsigned long *outlen);
+
+} *hash_descriptor[];
+
+#ifdef LTC_CHC_HASH
+int chc_register(int cipher);
+int chc_init(hash_state * md);
+int chc_process(hash_state * md, const unsigned char *in, unsigned long inlen);
+int chc_done(hash_state * md, unsigned char *hash);
+int chc_test(void);
+extern const struct ltc_hash_descriptor chc_desc;
+#endif
+
+#ifdef LTC_WHIRLPOOL
+int whirlpool_init(hash_state * md);
+int whirlpool_process(hash_state * md, const unsigned char *in, unsigned long inlen);
+int whirlpool_done(hash_state * md, unsigned char *hash);
+int whirlpool_test(void);
+extern const struct ltc_hash_descriptor whirlpool_desc;
+#endif
+
+#ifdef LTC_SHA512
+int sha512_init(hash_state * md);
+int sha512_process(hash_state * md, const unsigned char *in, unsigned long inlen);
+int sha512_done(hash_state * md, unsigned char *hash);
+int sha512_test(void);
+extern const struct ltc_hash_descriptor sha512_desc;
+#endif
+
+#ifdef LTC_SHA384
+#ifndef LTC_SHA512
+   #error LTC_SHA512 is required for LTC_SHA384
+#endif
+int sha384_init(hash_state * md);
+#define sha384_process sha512_process
+int sha384_done(hash_state * md, unsigned char *hash);
+int sha384_test(void);
+extern const struct ltc_hash_descriptor sha384_desc;
+#endif
+
+#if defined(LTC_SHA256) || defined(LTC_SHA256_ARM32_CE)
+int sha256_init(hash_state * md);
+int sha256_process(hash_state * md, const unsigned char *in, unsigned long inlen);
+int sha256_done(hash_state * md, unsigned char *hash);
+int sha256_test(void);
+extern const struct ltc_hash_descriptor sha256_desc;
+
+#ifdef LTC_SHA224
+#ifndef LTC_SHA256
+   #error LTC_SHA256 is required for LTC_SHA224
+#endif
+int sha224_init(hash_state * md);
+#define sha224_process sha256_process
+int sha224_done(hash_state * md, unsigned char *hash);
+int sha224_test(void);
+extern const struct ltc_hash_descriptor sha224_desc;
+#endif
+#endif
+
+#if defined(LTC_SHA1) || defined(LTC_SHA1_ARM32_CE)
+int sha1_init(hash_state * md);
+int sha1_process(hash_state * md, const unsigned char *in, unsigned long inlen);
+int sha1_done(hash_state * md, unsigned char *hash);
+int sha1_test(void);
+extern const struct ltc_hash_descriptor sha1_desc;
+#endif
+
+#ifdef LTC_MD5
+int md5_init(hash_state * md);
+int md5_process(hash_state * md, const unsigned char *in, unsigned long inlen);
+int md5_done(hash_state * md, unsigned char *hash);
+int md5_test(void);
+extern const struct ltc_hash_descriptor md5_desc;
+#endif
+
+#ifdef LTC_MD4
+int md4_init(hash_state * md);
+int md4_process(hash_state * md, const unsigned char *in, unsigned long inlen);
+int md4_done(hash_state * md, unsigned char *hash);
+int md4_test(void);
+extern const struct ltc_hash_descriptor md4_desc;
+#endif
+
+#ifdef LTC_MD2
+int md2_init(hash_state * md);
+int md2_process(hash_state * md, const unsigned char *in, unsigned long inlen);
+int md2_done(hash_state * md, unsigned char *hash);
+int md2_test(void);
+extern const struct ltc_hash_descriptor md2_desc;
+#endif
+
+#ifdef LTC_TIGER
+int tiger_init(hash_state * md);
+int tiger_process(hash_state * md, const unsigned char *in, unsigned long inlen);
+int tiger_done(hash_state * md, unsigned char *hash);
+int tiger_test(void);
+extern const struct ltc_hash_descriptor tiger_desc;
+#endif
+
+#ifdef LTC_RIPEMD128
+int rmd128_init(hash_state * md);
+int rmd128_process(hash_state * md, const unsigned char *in, unsigned long inlen);
+int rmd128_done(hash_state * md, unsigned char *hash);
+int rmd128_test(void);
+extern const struct ltc_hash_descriptor rmd128_desc;
+#endif
+
+#ifdef LTC_RIPEMD160
+int rmd160_init(hash_state * md);
+int rmd160_process(hash_state * md, const unsigned char *in, unsigned long inlen);
+int rmd160_done(hash_state * md, unsigned char *hash);
+int rmd160_test(void);
+extern const struct ltc_hash_descriptor rmd160_desc;
+#endif
+
+#ifdef LTC_RIPEMD256
+int rmd256_init(hash_state * md);
+int rmd256_process(hash_state * md, const unsigned char *in, unsigned long inlen);
+int rmd256_done(hash_state * md, unsigned char *hash);
+int rmd256_test(void);
+extern const struct ltc_hash_descriptor rmd256_desc;
+#endif
+
+#ifdef LTC_RIPEMD320
+int rmd320_init(hash_state * md);
+int rmd320_process(hash_state * md, const unsigned char *in, unsigned long inlen);
+int rmd320_done(hash_state * md, unsigned char *hash);
+int rmd320_test(void);
+extern const struct ltc_hash_descriptor rmd320_desc;
+#endif
+
+
+int find_hash(const char *name);
+int find_hash_id(unsigned char ID);
+int find_hash_oid(const unsigned long *ID, unsigned long IDlen);
+int find_hash_any(const char *name, int digestlen);
+int register_hash(const struct ltc_hash_descriptor *hash);
+int unregister_hash(const struct ltc_hash_descriptor *hash);
+int hash_is_valid(int idx);
+
+LTC_MUTEX_PROTO(ltc_hash_mutex)
+
+int hash_memory(int hash, 
+                const unsigned char *in,  unsigned long inlen, 
+                      unsigned char *out, unsigned long *outlen);
+int hash_memory_multi(int hash, unsigned char *out, unsigned long *outlen,
+                      const unsigned char *in, unsigned long inlen, ...);
+int hash_filehandle(int hash, FILE *in, unsigned char *out, unsigned long *outlen);
+int hash_file(int hash, const char *fname, unsigned char *out, unsigned long *outlen);
+
+/* a macro for making hash "process" functions */
+#define HASH_PROCESS_(func_name, compress_name, compress_n_name, state_var, block_size)     \
+int func_name (hash_state * md, const unsigned char *in, unsigned long inlen)               \
+{                                                                                           \
+    unsigned long n, blocks;                                                                \
+    int           err;                                                                      \
+    int           (*compress)(hash_state *, unsigned char *) = compress_name;               \
+    int           (*compress_n)(hash_state *, unsigned char *, int) = compress_n_name;      \
+    LTC_ARGCHK(md != NULL);                                                                 \
+    LTC_ARGCHK(in != NULL);                                                                 \
+    if (md-> state_var .curlen > sizeof(md-> state_var .buf)) {                             \
+       return CRYPT_INVALID_ARG;                                                            \
+    }                                                                                       \
+    while (inlen > 0) {                                                                     \
+        if (md-> state_var .curlen == 0 && inlen >= block_size) {                           \
+           if (compress_n) {                                                                \
+              blocks = inlen / block_size;                                                  \
+              err = compress_n (md, (unsigned char *)in, blocks);                           \
+           } else {                                                                         \
+              blocks = 1;                                                                   \
+              err = compress (md, (unsigned char *)in);                                     \
+           }                                                                                \
+           if (err != CRYPT_OK)                                                             \
+              return err;                                                                   \
+           md-> state_var .length += blocks * block_size * 8;                               \
+           in             += blocks * block_size;                                           \
+           inlen          -= blocks * block_size;                                           \
+        } else {                                                                            \
+           n = MIN(inlen, (block_size - md-> state_var .curlen));                           \
+           memcpy(md-> state_var .buf + md-> state_var.curlen, in, (size_t)n);              \
+           md-> state_var .curlen += n;                                                     \
+           in             += n;                                                             \
+           inlen          -= n;                                                             \
+           if (md-> state_var .curlen == block_size) {                                      \
+              if (compress_n) {                                                             \
+                 err = compress_n (md, md-> state_var .buf, 1);                             \
+              } else {                                                                      \
+                 err = compress (md, md-> state_var .buf);                                  \
+              }                                                                             \
+              if (err != CRYPT_OK) {                                                        \
+                 return err;                                                                \
+              }                                                                             \
+              md-> state_var .length += 8*block_size;                                       \
+              md-> state_var .curlen = 0;                                                   \
+           }                                                                                \
+       }                                                                                    \
+    }                                                                                       \
+    return CRYPT_OK;                                                                        \
+}
+
+/* define a hash "process" function based on a 1-block compress function */
+#define HASH_PROCESS(func_name, compress_name, state_var, block_size)                       \
+        HASH_PROCESS_(func_name, compress_name, NULL, state_var, block_size)
+
+/* define a hash "process" function based on a n-block compress function */
+#define HASH_PROCESS_NBLOCKS(func_name, compress_n_name, state_var, block_size)             \
+	HASH_PROCESS_(func_name, NULL, compress_n_name, state_var, block_size)
+
+/* $Source: /cvs/libtom/libtomcrypt/src/headers/tomcrypt_hash.h,v $ */
+/* $Revision: 1.22 $ */
+/* $Date: 2007/05/12 14:32:35 $ */
diff --git a/core/lib/libtomcrypt/include/tomcrypt_mac.h b/core/lib/libtomcrypt/include/tomcrypt_mac.h
new file mode 100644
index 0000000..f5843fe
--- /dev/null
+++ b/core/lib/libtomcrypt/include/tomcrypt_mac.h
@@ -0,0 +1,449 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+#ifdef LTC_HMAC
+typedef struct Hmac_state {
+     hash_state     md;
+     int            hash;
+     hash_state     hashstate;
+     unsigned char  key[MAXBLOCKSIZE];
+} hmac_state;
+
+int hmac_init(hmac_state *hmac, int hash, const unsigned char *key, unsigned long keylen);
+int hmac_process(hmac_state *hmac, const unsigned char *in, unsigned long inlen);
+int hmac_done(hmac_state *hmac, unsigned char *out, unsigned long *outlen);
+int hmac_test(void);
+int hmac_memory(int hash, 
+                const unsigned char *key, unsigned long keylen,
+                const unsigned char *in,  unsigned long inlen, 
+                      unsigned char *out, unsigned long *outlen);
+int hmac_memory_multi(int hash, 
+                const unsigned char *key,  unsigned long keylen,
+                      unsigned char *out,  unsigned long *outlen,
+                const unsigned char *in,   unsigned long inlen, ...);
+int hmac_file(int hash, const char *fname, const unsigned char *key,
+              unsigned long keylen, 
+              unsigned char *dst, unsigned long *dstlen);
+#endif
+
+#ifdef LTC_OMAC
+
+typedef struct {
+   int             cipher_idx, 
+                   buflen,
+                   blklen;
+   unsigned char   block[MAXBLOCKSIZE],
+                   prev[MAXBLOCKSIZE],
+                   Lu[2][MAXBLOCKSIZE];
+   symmetric_key   key;
+} omac_state;
+
+int omac_init(omac_state *omac, int cipher, const unsigned char *key, unsigned long keylen);
+int omac_process(omac_state *omac, const unsigned char *in, unsigned long inlen);
+int omac_done(omac_state *omac, unsigned char *out, unsigned long *outlen);
+int omac_memory(int cipher, 
+               const unsigned char *key, unsigned long keylen,
+               const unsigned char *in,  unsigned long inlen,
+                     unsigned char *out, unsigned long *outlen);
+int omac_memory_multi(int cipher, 
+                const unsigned char *key, unsigned long keylen,
+                      unsigned char *out, unsigned long *outlen,
+                const unsigned char *in,  unsigned long inlen, ...);
+int omac_file(int cipher, 
+              const unsigned char *key, unsigned long keylen,
+              const          char *filename, 
+                    unsigned char *out, unsigned long *outlen);
+int omac_test(void);
+#endif /* LTC_OMAC */
+
+#ifdef LTC_PMAC
+
+typedef struct {
+   unsigned char     Ls[32][MAXBLOCKSIZE],    /* L shifted by i bits to the left */
+                     Li[MAXBLOCKSIZE],        /* value of Li [current value, we calc from previous recall] */
+                     Lr[MAXBLOCKSIZE],        /* L * x^-1 */
+                     block[MAXBLOCKSIZE],     /* currently accumulated block */
+                     checksum[MAXBLOCKSIZE];  /* current checksum */
+
+   symmetric_key     key;                     /* scheduled key for cipher */
+   unsigned long     block_index;             /* index # for current block */
+   int               cipher_idx,              /* cipher idx */
+                     block_len,               /* length of block */
+                     buflen;                  /* number of bytes in the buffer */
+} pmac_state;
+
+int pmac_init(pmac_state *pmac, int cipher, const unsigned char *key, unsigned long keylen);
+int pmac_process(pmac_state *pmac, const unsigned char *in, unsigned long inlen);
+int pmac_done(pmac_state *pmac, unsigned char *out, unsigned long *outlen);
+
+int pmac_memory(int cipher, 
+               const unsigned char *key, unsigned long keylen,
+               const unsigned char *msg, unsigned long msglen,
+                     unsigned char *out, unsigned long *outlen);
+
+int pmac_memory_multi(int cipher, 
+                const unsigned char *key, unsigned long keylen,
+                      unsigned char *out, unsigned long *outlen,
+                const unsigned char *in, unsigned long inlen, ...);
+
+int pmac_file(int cipher, 
+             const unsigned char *key, unsigned long keylen,
+             const          char *filename, 
+                   unsigned char *out, unsigned long *outlen);
+
+int pmac_test(void);
+
+/* internal functions */
+int pmac_ntz(unsigned long x);
+void pmac_shift_xor(pmac_state *pmac);
+
+#endif /* PMAC */
+
+#ifdef LTC_EAX_MODE
+
+#if !(defined(LTC_OMAC) && defined(LTC_CTR_MODE))
+   #error LTC_EAX_MODE requires LTC_OMAC and CTR
+#endif
+
+typedef struct {
+   unsigned char N[MAXBLOCKSIZE];
+   symmetric_CTR ctr;
+   omac_state    headeromac, ctomac;
+} eax_state;
+
+int eax_init(eax_state *eax, int cipher, const unsigned char *key, unsigned long keylen,
+             const unsigned char *nonce, unsigned long noncelen,
+             const unsigned char *header, unsigned long headerlen);
+
+int eax_encrypt(eax_state *eax, const unsigned char *pt, unsigned char *ct, unsigned long length);
+int eax_decrypt(eax_state *eax, const unsigned char *ct, unsigned char *pt, unsigned long length);
+int eax_addheader(eax_state *eax, const unsigned char *header, unsigned long length);
+int eax_done(eax_state *eax, unsigned char *tag, unsigned long *taglen);
+
+int eax_encrypt_authenticate_memory(int cipher,
+    const unsigned char *key,    unsigned long keylen,
+    const unsigned char *nonce,  unsigned long noncelen,
+    const unsigned char *header, unsigned long headerlen,
+    const unsigned char *pt,     unsigned long ptlen,
+          unsigned char *ct,
+          unsigned char *tag,    unsigned long *taglen);
+
+int eax_decrypt_verify_memory(int cipher,
+    const unsigned char *key,    unsigned long keylen,
+    const unsigned char *nonce,  unsigned long noncelen,
+    const unsigned char *header, unsigned long headerlen,
+    const unsigned char *ct,     unsigned long ctlen,
+          unsigned char *pt,
+          unsigned char *tag,    unsigned long taglen,
+          int           *stat);
+
+ int eax_test(void);
+#endif /* EAX MODE */
+
+#ifdef LTC_OCB_MODE
+typedef struct {
+   unsigned char     L[MAXBLOCKSIZE],         /* L value */
+                     Ls[32][MAXBLOCKSIZE],    /* L shifted by i bits to the left */
+                     Li[MAXBLOCKSIZE],        /* value of Li [current value, we calc from previous recall] */
+                     Lr[MAXBLOCKSIZE],        /* L * x^-1 */
+                     R[MAXBLOCKSIZE],         /* R value */
+                     checksum[MAXBLOCKSIZE];  /* current checksum */
+
+   symmetric_key     key;                     /* scheduled key for cipher */
+   unsigned long     block_index;             /* index # for current block */
+   int               cipher,                  /* cipher idx */
+                     block_len;               /* length of block */
+} ocb_state;
+
+int ocb_init(ocb_state *ocb, int cipher, 
+             const unsigned char *key, unsigned long keylen, const unsigned char *nonce);
+
+int ocb_encrypt(ocb_state *ocb, const unsigned char *pt, unsigned char *ct);
+int ocb_decrypt(ocb_state *ocb, const unsigned char *ct, unsigned char *pt);
+
+int ocb_done_encrypt(ocb_state *ocb, 
+                     const unsigned char *pt,  unsigned long ptlen,
+                           unsigned char *ct, 
+                           unsigned char *tag, unsigned long *taglen);
+
+int ocb_done_decrypt(ocb_state *ocb, 
+                     const unsigned char *ct,  unsigned long ctlen,
+                           unsigned char *pt, 
+                     const unsigned char *tag, unsigned long taglen, int *stat);
+
+int ocb_encrypt_authenticate_memory(int cipher,
+    const unsigned char *key,    unsigned long keylen,
+    const unsigned char *nonce,  
+    const unsigned char *pt,     unsigned long ptlen,
+          unsigned char *ct,
+          unsigned char *tag,    unsigned long *taglen);
+
+int ocb_decrypt_verify_memory(int cipher,
+    const unsigned char *key,    unsigned long keylen,
+    const unsigned char *nonce,  
+    const unsigned char *ct,     unsigned long ctlen,
+          unsigned char *pt,
+    const unsigned char *tag,    unsigned long taglen,
+          int           *stat);
+
+int ocb_test(void);
+
+/* internal functions */
+void ocb_shift_xor(ocb_state *ocb, unsigned char *Z);
+int ocb_ntz(unsigned long x);
+int s_ocb_done(ocb_state *ocb, const unsigned char *pt, unsigned long ptlen,
+               unsigned char *ct, unsigned char *tag, unsigned long *taglen, int mode);
+
+#endif /* LTC_OCB_MODE */
+
+#ifdef LTC_CCM_MODE
+
+#define CCM_ENCRYPT 0
+#define CCM_DECRYPT 1
+
+typedef struct {
+   symmetric_key       K;
+   int                 cipher,               /* which cipher */
+                       taglen,               /* length of the tag */
+                       x;                    /* index in PAD */
+
+   unsigned long       L,                    /* L value */
+                       ptlen,                /* length that will be enc / dec */
+                       current_ptlen,        /* current processed length */
+                       aadlen,               /* length of the aad */
+                       current_aadlen,       /* length of the currently provided add */
+                       noncelen;             /* length of the nonce */
+
+   unsigned char       PAD[16],
+                       ctr[16],
+                       CTRPAD[16],
+                       CTRlen;
+} ccm_state;
+
+int ccm_init(ccm_state *ccm, int cipher,
+             const unsigned char *key, int keylen, int ptlen, int taglen, int aad_len);
+
+int ccm_reset(ccm_state *ccm);
+
+int ccm_add_nonce(ccm_state *ccm,
+                  const unsigned char *nonce,     unsigned long noncelen);
+
+int ccm_add_aad(ccm_state *ccm,
+                const unsigned char *adata,  unsigned long adatalen);
+
+int ccm_process(ccm_state *ccm,
+                unsigned char *pt,     unsigned long ptlen,
+                unsigned char *ct,
+                int direction);
+
+int ccm_done(ccm_state *ccm,
+             unsigned char *tag,    unsigned long *taglen);
+
+int ccm_memory(int cipher,
+    const unsigned char *key,    unsigned long keylen,
+    symmetric_key       *uskey,
+    const unsigned char *nonce,  unsigned long noncelen,
+    const unsigned char *header, unsigned long headerlen,
+          unsigned char *pt,     unsigned long ptlen,
+          unsigned char *ct,
+          unsigned char *tag,    unsigned long *taglen,
+                    int  direction);
+
+int ccm_test(void);
+
+#endif /* LTC_CCM_MODE */
+
+#if defined(LRW_MODE) || defined(LTC_GCM_MODE)
+void gcm_gf_mult(const unsigned char *a, const unsigned char *b, unsigned char *c);
+#endif
+
+
+/* table shared between GCM and LRW */
+#if defined(LTC_GCM_TABLES) || defined(LTC_LRW_TABLES) || ((defined(LTC_GCM_MODE) || defined(LTC_GCM_MODE)) && defined(LTC_FAST))
+extern const unsigned char gcm_shift_table[];
+#endif
+
+#ifdef LTC_GCM_MODE
+
+#define GCM_ENCRYPT 0
+#define GCM_DECRYPT 1
+
+#define LTC_GCM_MODE_IV    0
+#define LTC_GCM_MODE_AAD   1
+#define LTC_GCM_MODE_TEXT  2
+
+typedef struct { 
+   symmetric_key       K;
+   unsigned char       H[16],        /* multiplier */
+                       X[16],        /* accumulator */
+                       Y[16],        /* counter */
+                       Y_0[16],      /* initial counter */
+                       buf[16];      /* buffer for stuff */
+
+   int                 cipher,       /* which cipher */
+                       ivmode,       /* Which mode is the IV in? */
+                       mode,         /* mode the GCM code is in */
+                       buflen;       /* length of data in buf */
+
+   ulong64             totlen,       /* 64-bit counter used for IV and AAD */
+                       pttotlen;     /* 64-bit counter for the PT */
+
+#ifdef LTC_GCM_TABLES
+   unsigned char       PC[16][256][16]  /* 16 tables of 8x128 */
+#ifdef LTC_GCM_TABLES_SSE2
+__attribute__ ((aligned (16)))
+#endif
+;
+#endif  
+} gcm_state;
+
+void gcm_mult_h(gcm_state *gcm, unsigned char *I);
+
+int gcm_init(gcm_state *gcm, int cipher,
+             const unsigned char *key, int keylen);
+
+int gcm_reset(gcm_state *gcm);
+
+int gcm_add_iv(gcm_state *gcm, 
+               const unsigned char *IV,     unsigned long IVlen);
+
+int gcm_add_aad(gcm_state *gcm,
+               const unsigned char *adata,  unsigned long adatalen);
+
+int gcm_process(gcm_state *gcm,
+                     unsigned char *pt,     unsigned long ptlen,
+                     unsigned char *ct,
+                     int direction);
+
+int gcm_done(gcm_state *gcm, 
+                     unsigned char *tag,    unsigned long *taglen);
+
+int gcm_memory(      int           cipher,
+               const unsigned char *key,    unsigned long keylen,
+               const unsigned char *IV,     unsigned long IVlen,
+               const unsigned char *adata,  unsigned long adatalen,
+                     unsigned char *pt,     unsigned long ptlen,
+                     unsigned char *ct, 
+                     unsigned char *tag,    unsigned long *taglen,
+                               int direction);
+int gcm_test(void);
+
+#endif /* LTC_GCM_MODE */
+
+#ifdef LTC_PELICAN
+
+typedef struct pelican_state
+{
+    symmetric_key K;
+    unsigned char state[16];
+    int           buflen;
+} pelican_state;
+
+int pelican_init(pelican_state *pelmac, const unsigned char *key, unsigned long keylen);
+int pelican_process(pelican_state *pelmac, const unsigned char *in, unsigned long inlen);
+int pelican_done(pelican_state *pelmac, unsigned char *out);
+int pelican_test(void);
+
+int pelican_memory(const unsigned char *key, unsigned long keylen,
+                   const unsigned char *in, unsigned long inlen,
+                         unsigned char *out);
+
+#endif
+
+#ifdef LTC_XCBC
+
+/* add this to "keylen" to xcbc_init to use a pure three-key XCBC MAC */
+#define LTC_XCBC_PURE  0x8000UL
+
+typedef struct {
+   unsigned char K[3][MAXBLOCKSIZE],
+                 IV[MAXBLOCKSIZE];
+
+   symmetric_key key;
+
+             int cipher,
+                 buflen,
+                 blocksize;
+} xcbc_state;
+
+int xcbc_init(xcbc_state *xcbc, int cipher, const unsigned char *key, unsigned long keylen);
+int xcbc_process(xcbc_state *xcbc, const unsigned char *in, unsigned long inlen);
+int xcbc_done(xcbc_state *xcbc, unsigned char *out, unsigned long *outlen);
+int xcbc_memory(int cipher, 
+               const unsigned char *key, unsigned long keylen,
+               const unsigned char *in,  unsigned long inlen,
+                     unsigned char *out, unsigned long *outlen);
+int xcbc_memory_multi(int cipher, 
+                const unsigned char *key, unsigned long keylen,
+                      unsigned char *out, unsigned long *outlen,
+                const unsigned char *in,  unsigned long inlen, ...);
+int xcbc_file(int cipher, 
+              const unsigned char *key, unsigned long keylen,
+              const          char *filename, 
+                    unsigned char *out, unsigned long *outlen);
+int xcbc_test(void);
+
+#endif
+
+#ifdef LTC_F9_MODE
+
+typedef struct {
+   unsigned char akey[MAXBLOCKSIZE],
+                 ACC[MAXBLOCKSIZE],
+                 IV[MAXBLOCKSIZE];
+
+   symmetric_key key;
+
+             int cipher,
+                 buflen,
+                 keylen,
+                 blocksize;
+} f9_state;
+
+int f9_init(f9_state *f9, int cipher, const unsigned char *key, unsigned long keylen);
+int f9_process(f9_state *f9, const unsigned char *in, unsigned long inlen);
+int f9_done(f9_state *f9, unsigned char *out, unsigned long *outlen);
+int f9_memory(int cipher, 
+               const unsigned char *key, unsigned long keylen,
+               const unsigned char *in,  unsigned long inlen,
+                     unsigned char *out, unsigned long *outlen);
+int f9_memory_multi(int cipher, 
+                const unsigned char *key, unsigned long keylen,
+                      unsigned char *out, unsigned long *outlen,
+                const unsigned char *in,  unsigned long inlen, ...);
+int f9_file(int cipher, 
+              const unsigned char *key, unsigned long keylen,
+              const          char *filename, 
+                    unsigned char *out, unsigned long *outlen);
+int f9_test(void);
+
+#endif
+
+
+/* $Source: /cvs/libtom/libtomcrypt/src/headers/tomcrypt_mac.h,v $ */
+/* $Revision: 1.23 $ */
+/* $Date: 2007/05/12 14:37:41 $ */
diff --git a/core/lib/libtomcrypt/include/tomcrypt_macros.h b/core/lib/libtomcrypt/include/tomcrypt_macros.h
new file mode 100644
index 0000000..af6f7ca
--- /dev/null
+++ b/core/lib/libtomcrypt/include/tomcrypt_macros.h
@@ -0,0 +1,491 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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 TOMCRYPT_MACROS_H_
+#define TOMCRYPT_MACROS_H_
+
+/* fix for MSVC ...evil! */
+#ifdef _MSC_VER
+   #define CONST64(n) n ## ui64
+   typedef unsigned __int64 ulong64;
+#else
+   #define CONST64(n) n ## ULL
+   typedef unsigned long long ulong64;
+#endif
+
+/* this is the "32-bit at least" data type 
+ * Re-define it to suit your platform but it must be at least 32-bits 
+ */
+#if defined(__x86_64__) || (defined(__sparc__) && defined(__arch64__)) || \
+    defined(__LP64__)
+   typedef unsigned ulong32;
+#else
+   typedef unsigned long ulong32;
+#endif
+
+#ifdef ENDIAN_64BITWORD
+typedef ulong64 ltc_mp_digit;
+#else
+typedef ulong32 ltc_mp_digit;
+#endif
+
+/* ---- HELPER MACROS ---- */
+#ifdef ENDIAN_NEUTRAL
+
+#define STORE32L(x, y)                                                                     \
+  do { (y)[3] = (unsigned char)(((x)>>24)&255); (y)[2] = (unsigned char)(((x)>>16)&255);   \
+       (y)[1] = (unsigned char)(((x)>>8)&255); (y)[0] = (unsigned char)((x)&255); } while(0)
+
+#define LOAD32L(x, y)                            \
+  do { x = ((ulong32)((y)[3] & 255)<<24) | \
+           ((ulong32)((y)[2] & 255)<<16) | \
+           ((ulong32)((y)[1] & 255)<<8)  | \
+           ((ulong32)((y)[0] & 255)); } while(0)
+
+#define STORE64L(x, y)                                                                     \
+  do { (y)[7] = (unsigned char)(((x)>>56)&255); (y)[6] = (unsigned char)(((x)>>48)&255);   \
+       (y)[5] = (unsigned char)(((x)>>40)&255); (y)[4] = (unsigned char)(((x)>>32)&255);   \
+       (y)[3] = (unsigned char)(((x)>>24)&255); (y)[2] = (unsigned char)(((x)>>16)&255);   \
+       (y)[1] = (unsigned char)(((x)>>8)&255); (y)[0] = (unsigned char)((x)&255); } while(0)
+
+#define LOAD64L(x, y)                                                       \
+  do { x = (((ulong64)((y)[7] & 255))<<56)|(((ulong64)((y)[6] & 255))<<48)| \
+           (((ulong64)((y)[5] & 255))<<40)|(((ulong64)((y)[4] & 255))<<32)| \
+           (((ulong64)((y)[3] & 255))<<24)|(((ulong64)((y)[2] & 255))<<16)| \
+           (((ulong64)((y)[1] & 255))<<8)|(((ulong64)((y)[0] & 255))); } while(0)
+
+#define STORE32H(x, y)                                                                     \
+  do { (y)[0] = (unsigned char)(((x)>>24)&255); (y)[1] = (unsigned char)(((x)>>16)&255);   \
+       (y)[2] = (unsigned char)(((x)>>8)&255); (y)[3] = (unsigned char)((x)&255); } while(0)
+
+#define LOAD32H(x, y)                            \
+  do { x = ((ulong32)((y)[0] & 255)<<24) | \
+           ((ulong32)((y)[1] & 255)<<16) | \
+           ((ulong32)((y)[2] & 255)<<8)  | \
+           ((ulong32)((y)[3] & 255)); } while(0)
+
+#define STORE64H(x, y)                                                                     \
+do { (y)[0] = (unsigned char)(((x)>>56)&255); (y)[1] = (unsigned char)(((x)>>48)&255);     \
+     (y)[2] = (unsigned char)(((x)>>40)&255); (y)[3] = (unsigned char)(((x)>>32)&255);     \
+     (y)[4] = (unsigned char)(((x)>>24)&255); (y)[5] = (unsigned char)(((x)>>16)&255);     \
+     (y)[6] = (unsigned char)(((x)>>8)&255); (y)[7] = (unsigned char)((x)&255); } while(0)
+
+#define LOAD64H(x, y)                                                      \
+do { x = (((ulong64)((y)[0] & 255))<<56)|(((ulong64)((y)[1] & 255))<<48) | \
+         (((ulong64)((y)[2] & 255))<<40)|(((ulong64)((y)[3] & 255))<<32) | \
+         (((ulong64)((y)[4] & 255))<<24)|(((ulong64)((y)[5] & 255))<<16) | \
+         (((ulong64)((y)[6] & 255))<<8)|(((ulong64)((y)[7] & 255))); } while(0)
+
+#endif /* ENDIAN_NEUTRAL */
+
+#ifdef ENDIAN_LITTLE
+
+#ifdef LTC_HAVE_BSWAP_BUILTIN
+
+#define STORE32H(x, y)                          \
+do { ulong32 __t = __builtin_bswap32 ((x));     \
+      XMEMCPY ((y), &__t, 4); } while(0)
+
+#define LOAD32H(x, y)                           \
+do { XMEMCPY (&(x), (y), 4);                    \
+      (x) = __builtin_bswap32 ((x)); } while(0)
+
+#elif !defined(LTC_NO_BSWAP) && (defined(INTEL_CC) || (defined(__GNUC__) && (defined(__DJGPP__) || defined(__CYGWIN__) || defined(__MINGW32__) || defined(__i386__) || defined(__x86_64__))))
+
+#define STORE32H(x, y)           \
+asm __volatile__ (               \
+   "bswapl %0     \n\t"          \
+   "movl   %0,(%1)\n\t"          \
+   "bswapl %0     \n\t"          \
+      ::"r"(x), "r"(y));
+
+#define LOAD32H(x, y)          \
+asm __volatile__ (             \
+   "movl (%1),%0\n\t"          \
+   "bswapl %0\n\t"             \
+   :"=r"(x): "r"(y));
+
+#else
+
+#define STORE32H(x, y)                                                                     \
+  do { (y)[0] = (unsigned char)(((x)>>24)&255); (y)[1] = (unsigned char)(((x)>>16)&255);   \
+       (y)[2] = (unsigned char)(((x)>>8)&255); (y)[3] = (unsigned char)((x)&255); } while(0)
+
+#define LOAD32H(x, y)                            \
+  do { x = ((ulong32)((y)[0] & 255)<<24) | \
+           ((ulong32)((y)[1] & 255)<<16) | \
+           ((ulong32)((y)[2] & 255)<<8)  | \
+           ((ulong32)((y)[3] & 255)); } while(0)
+
+#endif
+
+#ifdef LTC_HAVE_BSWAP_BUILTIN
+
+#define STORE64H(x, y)                          \
+do { ulong64 __t = __builtin_bswap64 ((x));     \
+      XMEMCPY ((y), &__t, 8); } while(0)
+
+#define LOAD64H(x, y)                           \
+do { XMEMCPY (&(x), (y), 8);                    \
+      (x) = __builtin_bswap64 ((x)); } while(0)
+
+/* x86_64 processor */
+#elif !defined(LTC_NO_BSWAP) && (defined(__GNUC__) && defined(__x86_64__))
+
+#define STORE64H(x, y)           \
+asm __volatile__ (               \
+   "bswapq %0     \n\t"          \
+   "movq   %0,(%1)\n\t"          \
+   "bswapq %0     \n\t"          \
+   ::"r"(x), "r"(y): "memory");
+
+#define LOAD64H(x, y)          \
+asm __volatile__ (             \
+   "movq (%1),%0\n\t"          \
+   "bswapq %0\n\t"             \
+   :"=r"(x): "r"(y): "memory");
+
+#else
+
+#define STORE64H(x, y)                                                                     \
+do { (y)[0] = (unsigned char)(((x)>>56)&255); (y)[1] = (unsigned char)(((x)>>48)&255);     \
+     (y)[2] = (unsigned char)(((x)>>40)&255); (y)[3] = (unsigned char)(((x)>>32)&255);     \
+     (y)[4] = (unsigned char)(((x)>>24)&255); (y)[5] = (unsigned char)(((x)>>16)&255);     \
+     (y)[6] = (unsigned char)(((x)>>8)&255); (y)[7] = (unsigned char)((x)&255); } while(0)
+
+#define LOAD64H(x, y)                                                      \
+do { x = (((ulong64)((y)[0] & 255))<<56)|(((ulong64)((y)[1] & 255))<<48) | \
+         (((ulong64)((y)[2] & 255))<<40)|(((ulong64)((y)[3] & 255))<<32) | \
+         (((ulong64)((y)[4] & 255))<<24)|(((ulong64)((y)[5] & 255))<<16) | \
+         (((ulong64)((y)[6] & 255))<<8)|(((ulong64)((y)[7] & 255))); } while(0)
+
+#endif
+
+#ifdef ENDIAN_32BITWORD 
+
+#define STORE32L(x, y)        \
+  do { ulong32  __t = (x); XMEMCPY(y, &__t, 4); } while(0)
+
+#define LOAD32L(x, y)         \
+  do { XMEMCPY(&(x), y, 4); } while(0)
+
+#define STORE64L(x, y)                                                                     \
+  do { (y)[7] = (unsigned char)(((x)>>56)&255); (y)[6] = (unsigned char)(((x)>>48)&255);   \
+       (y)[5] = (unsigned char)(((x)>>40)&255); (y)[4] = (unsigned char)(((x)>>32)&255);   \
+       (y)[3] = (unsigned char)(((x)>>24)&255); (y)[2] = (unsigned char)(((x)>>16)&255);   \
+       (y)[1] = (unsigned char)(((x)>>8)&255); (y)[0] = (unsigned char)((x)&255); } while(0)
+
+#define LOAD64L(x, y)                                                       \
+  do { x = (((ulong64)((y)[7] & 255))<<56)|(((ulong64)((y)[6] & 255))<<48)| \
+           (((ulong64)((y)[5] & 255))<<40)|(((ulong64)((y)[4] & 255))<<32)| \
+           (((ulong64)((y)[3] & 255))<<24)|(((ulong64)((y)[2] & 255))<<16)| \
+           (((ulong64)((y)[1] & 255))<<8)|(((ulong64)((y)[0] & 255))); } while(0)
+
+#else /* 64-bit words then  */
+
+#define STORE32L(x, y)        \
+  do { ulong32 __t = (x); XMEMCPY(y, &__t, 4); } while(0)
+
+#define LOAD32L(x, y)         \
+  do { XMEMCPY(&(x), y, 4); x &= 0xFFFFFFFF; } while(0)
+
+#define STORE64L(x, y)        \
+  do { ulong64 __t = (x); XMEMCPY(y, &__t, 8); } while(0)
+
+#define LOAD64L(x, y)         \
+  do { XMEMCPY(&(x), y, 8); } while(0)
+
+#endif /* ENDIAN_64BITWORD */
+
+#endif /* ENDIAN_LITTLE */
+
+#ifdef ENDIAN_BIG
+#define STORE32L(x, y)                                                                     \
+  do { (y)[3] = (unsigned char)(((x)>>24)&255); (y)[2] = (unsigned char)(((x)>>16)&255);   \
+       (y)[1] = (unsigned char)(((x)>>8)&255); (y)[0] = (unsigned char)((x)&255); } while(0)
+
+#define LOAD32L(x, y)                            \
+  do { x = ((ulong32)((y)[3] & 255)<<24) | \
+           ((ulong32)((y)[2] & 255)<<16) | \
+           ((ulong32)((y)[1] & 255)<<8)  | \
+           ((ulong32)((y)[0] & 255)); } while(0)
+
+#define STORE64L(x, y)                                                                     \
+do { (y)[7] = (unsigned char)(((x)>>56)&255); (y)[6] = (unsigned char)(((x)>>48)&255);     \
+     (y)[5] = (unsigned char)(((x)>>40)&255); (y)[4] = (unsigned char)(((x)>>32)&255);     \
+     (y)[3] = (unsigned char)(((x)>>24)&255); (y)[2] = (unsigned char)(((x)>>16)&255);     \
+     (y)[1] = (unsigned char)(((x)>>8)&255); (y)[0] = (unsigned char)((x)&255); } while(0)
+
+#define LOAD64L(x, y)                                                      \
+do { x = (((ulong64)((y)[7] & 255))<<56)|(((ulong64)((y)[6] & 255))<<48) | \
+         (((ulong64)((y)[5] & 255))<<40)|(((ulong64)((y)[4] & 255))<<32) | \
+         (((ulong64)((y)[3] & 255))<<24)|(((ulong64)((y)[2] & 255))<<16) | \
+         (((ulong64)((y)[1] & 255))<<8)|(((ulong64)((y)[0] & 255))); } while(0)
+
+#ifdef ENDIAN_32BITWORD 
+
+#define STORE32H(x, y)        \
+  do { ulong32 __t = (x); XMEMCPY(y, &__t, 4); } while(0)
+
+#define LOAD32H(x, y)         \
+  do { XMEMCPY(&(x), y, 4); } while(0)
+
+#define STORE64H(x, y)                                                                     \
+  do { (y)[0] = (unsigned char)(((x)>>56)&255); (y)[1] = (unsigned char)(((x)>>48)&255);   \
+       (y)[2] = (unsigned char)(((x)>>40)&255); (y)[3] = (unsigned char)(((x)>>32)&255);   \
+       (y)[4] = (unsigned char)(((x)>>24)&255); (y)[5] = (unsigned char)(((x)>>16)&255);   \
+       (y)[6] = (unsigned char)(((x)>>8)&255);  (y)[7] = (unsigned char)((x)&255); } while(0)
+
+#define LOAD64H(x, y)                                                       \
+  do { x = (((ulong64)((y)[0] & 255))<<56)|(((ulong64)((y)[1] & 255))<<48)| \
+           (((ulong64)((y)[2] & 255))<<40)|(((ulong64)((y)[3] & 255))<<32)| \
+           (((ulong64)((y)[4] & 255))<<24)|(((ulong64)((y)[5] & 255))<<16)| \
+           (((ulong64)((y)[6] & 255))<<8)| (((ulong64)((y)[7] & 255))); } while(0)
+
+#else /* 64-bit words then  */
+
+#define STORE32H(x, y)        \
+  do { ulong32 __t = (x); XMEMCPY(y, &__t, 4); } while(0)
+
+#define LOAD32H(x, y)         \
+  do { XMEMCPY(&(x), y, 4); x &= 0xFFFFFFFF; } while(0)
+
+#define STORE64H(x, y)        \
+  do { ulong64 __t = (x); XMEMCPY(y, &__t, 8); } while(0)
+
+#define LOAD64H(x, y)         \
+  do { XMEMCPY(&(x), y, 8); } while(0)
+
+#endif /* ENDIAN_64BITWORD */
+#endif /* ENDIAN_BIG */
+
+#define BSWAP(x)  ( ((x>>24)&0x000000FFUL) | ((x<<24)&0xFF000000UL)  | \
+                    ((x>>8)&0x0000FF00UL)  | ((x<<8)&0x00FF0000UL) )
+
+
+/* 32-bit Rotates */
+#if defined(_MSC_VER)
+#define LTC_ROx_ASM
+
+/* instrinsic rotate */
+#include <stdlib.h>
+#pragma intrinsic(_lrotr,_lrotl)
+#define ROR(x,n) _lrotr(x,n)
+#define ROL(x,n) _lrotl(x,n)
+#define RORc(x,n) _lrotr(x,n)
+#define ROLc(x,n) _lrotl(x,n)
+
+#elif !defined(__STRICT_ANSI__) && defined(__GNUC__) && (defined(__i386__) || defined(__x86_64__)) && !defined(INTEL_CC) && !defined(LTC_NO_ASM)
+#define LTC_ROx_ASM
+
+static inline ulong32 ROL(ulong32 word, int i)
+{
+   asm ("roll %%cl,%0"
+      :"=r" (word)
+      :"0" (word),"c" (i));
+   return word;
+}
+
+static inline ulong32 ROR(ulong32 word, int i)
+{
+   asm ("rorl %%cl,%0"
+      :"=r" (word)
+      :"0" (word),"c" (i));
+   return word;
+}
+
+#ifndef LTC_NO_ROLC
+
+#define ROLc(word,i) ({ \
+   ulong32 __ROLc_tmp = word; \
+   __asm__ ("roll %2, %0" : \
+            "=r" (__ROLc_tmp) : \
+            "0" (__ROLc_tmp), \
+            "I" (i)); \
+            __ROLc_tmp; \
+   })
+#define RORc(word,i) ({ \
+   ulong32 __RORc_tmp = word; \
+   __asm__ ("rorl %2, %0" : \
+            "=r" (__RORc_tmp) : \
+            "0" (__RORc_tmp), \
+            "I" (i)); \
+            __RORc_tmp; \
+   })
+
+#else
+
+#define ROLc ROL
+#define RORc ROR
+
+#endif
+
+#elif !defined(__STRICT_ANSI__) && defined(LTC_PPC32)
+#define LTC_ROx_ASM
+
+static inline ulong32 ROL(ulong32 word, int i)
+{
+   asm ("rotlw %0,%0,%2"
+      :"=r" (word)
+      :"0" (word),"r" (i));
+   return word;
+}
+
+static inline ulong32 ROR(ulong32 word, int i)
+{
+   asm ("rotlw %0,%0,%2"
+      :"=r" (word)
+      :"0" (word),"r" (32-i));
+   return word;
+}
+
+#ifndef LTC_NO_ROLC
+
+static inline ulong32 ROLc(ulong32 word, const int i)
+{
+   asm ("rotlwi %0,%0,%2"
+      :"=r" (word)
+      :"0" (word),"I" (i));
+   return word;
+}
+
+static inline ulong32 RORc(ulong32 word, const int i)
+{
+   asm ("rotrwi %0,%0,%2"
+      :"=r" (word)
+      :"0" (word),"I" (i));
+   return word;
+}
+
+#else
+
+#define ROLc ROL
+#define RORc ROR
+
+#endif
+
+
+#else
+
+/* rotates the hard way */
+#define ROL(x, y) ( (((ulong32)(x)<<(ulong32)((y)&31)) | (((ulong32)(x)&0xFFFFFFFFUL)>>(ulong32)(32-((y)&31)))) & 0xFFFFFFFFUL)
+#define ROR(x, y) ( ((((ulong32)(x)&0xFFFFFFFFUL)>>(ulong32)((y)&31)) | ((ulong32)(x)<<(ulong32)(32-((y)&31)))) & 0xFFFFFFFFUL)
+#define ROLc(x, y) ( (((ulong32)(x)<<(ulong32)((y)&31)) | (((ulong32)(x)&0xFFFFFFFFUL)>>(ulong32)(32-((y)&31)))) & 0xFFFFFFFFUL)
+#define RORc(x, y) ( ((((ulong32)(x)&0xFFFFFFFFUL)>>(ulong32)((y)&31)) | ((ulong32)(x)<<(ulong32)(32-((y)&31)))) & 0xFFFFFFFFUL)
+
+#endif
+
+
+/* 64-bit Rotates */
+#if !defined(__STRICT_ANSI__) && defined(__GNUC__) && defined(__x86_64__) && !defined(_WIN64) && !defined(LTC_NO_ASM)
+
+static inline ulong64 ROL64(ulong64 word, int i)
+{
+   asm("rolq %%cl,%0"
+      :"=r" (word)
+      :"0" (word),"c" (i));
+   return word;
+}
+
+static inline ulong64 ROR64(ulong64 word, int i)
+{
+   asm("rorq %%cl,%0"
+      :"=r" (word)
+      :"0" (word),"c" (i));
+   return word;
+}
+
+#ifndef LTC_NO_ROLC
+
+#define ROL64c(word,i) ({ \
+   ulong64 __ROL64c_tmp = word; \
+   __asm__ ("rolq %2, %0" : \
+            "=r" (__ROL64c_tmp) : \
+            "0" (__ROL64c_tmp), \
+            "J" (i)); \
+            __ROL64c_tmp; \
+   })
+#define ROR64c(word,i) ({ \
+   ulong64 __ROR64c_tmp = word; \
+   __asm__ ("rorq %2, %0" : \
+            "=r" (__ROR64c_tmp) : \
+            "0" (__ROR64c_tmp), \
+            "J" (i)); \
+            __ROR64c_tmp; \
+   })
+
+#else /* LTC_NO_ROLC */
+
+#define ROL64c ROL64
+#define ROR64c ROR64
+
+#endif
+
+#else /* Not x86_64  */
+
+#define ROL64(x, y) \
+    ( (((x)<<((ulong64)(y)&63)) | \
+      (((x)&CONST64(0xFFFFFFFFFFFFFFFF))>>((ulong64)64-((y)&63)))) & CONST64(0xFFFFFFFFFFFFFFFF))
+
+#define ROR64(x, y) \
+    ( ((((x)&CONST64(0xFFFFFFFFFFFFFFFF))>>((ulong64)(y)&CONST64(63))) | \
+      ((x)<<((ulong64)(64-((y)&CONST64(63)))))) & CONST64(0xFFFFFFFFFFFFFFFF))
+
+#define ROL64c(x, y) \
+    ( (((x)<<((ulong64)(y)&63)) | \
+      (((x)&CONST64(0xFFFFFFFFFFFFFFFF))>>((ulong64)64-((y)&63)))) & CONST64(0xFFFFFFFFFFFFFFFF))
+
+#define ROR64c(x, y) \
+    ( ((((x)&CONST64(0xFFFFFFFFFFFFFFFF))>>((ulong64)(y)&CONST64(63))) | \
+      ((x)<<((ulong64)(64-((y)&CONST64(63)))))) & CONST64(0xFFFFFFFFFFFFFFFF))
+
+#endif
+
+#ifndef MAX
+   #define MAX(x, y) ( ((x)>(y))?(x):(y) )
+#endif
+
+#ifndef MIN
+   #define MIN(x, y) ( ((x)<(y))?(x):(y) )
+#endif
+
+#ifndef LTC_UNUSED_PARAM
+   #define LTC_UNUSED_PARAM(x) (void)(x)
+#endif
+
+/* extract a byte portably */
+#ifdef _MSC_VER
+   #define byte(x, n) ((unsigned char)((x) >> (8 * (n))))
+#else
+   #define byte(x, n) (((x) >> (8 * (n))) & 255)
+#endif   
+
+
+#endif   /* TOMCRYPT_MACROS_H_ */
+/* $Source: /cvs/libtom/libtomcrypt/src/headers/tomcrypt_macros.h,v $ */
+/* $Revision: 1.15 $ */
+/* $Date: 2006/11/29 23:43:57 $ */
diff --git a/core/lib/libtomcrypt/include/tomcrypt_math.h b/core/lib/libtomcrypt/include/tomcrypt_math.h
new file mode 100644
index 0000000..b3e6308
--- /dev/null
+++ b/core/lib/libtomcrypt/include/tomcrypt_math.h
@@ -0,0 +1,547 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/** math functions **/
+
+#define LTC_MP_LT   -1
+#define LTC_MP_EQ    0
+#define LTC_MP_GT    1
+
+#define LTC_MP_NO    0
+#define LTC_MP_YES   1
+
+#ifndef LTC_MECC
+   typedef void ecc_point;
+#endif
+
+#ifndef LTC_MRSA
+   typedef void rsa_key;
+#endif
+
+/** math descriptor */
+typedef struct {
+   /** Name of the math provider */
+   const char *name;
+
+   /** Bits per digit, amount of bits must fit in an unsigned long */
+   int  bits_per_digit;
+
+/* ---- init/deinit functions ---- */
+
+   /** initialize a bignum
+     @param   a     The number to initialize
+     @return  CRYPT_OK on success
+   */
+   int (*init)(void **a);
+   
+   /** initialize a bignum
+     @param   size_bits   The size of the number we compute on
+     @param   a           The number to initialize
+     @return  CRYPT_OK on success
+   */
+   int (*init_size)(int size_bits, void **a);
+
+   /** init copy 
+     @param  dst    The number to initialize and write to
+     @param  src    The number to copy from
+     @return CRYPT_OK on success
+   */
+   int (*init_copy)(void **dst, void *src);
+
+   /** deinit 
+      @param   a    The number to free
+      @return CRYPT_OK on success
+   */
+   void (*deinit)(void *a);
+
+/* ---- data movement ---- */
+
+   /** negate
+      @param   src   The number to negate
+      @param   dst   The destination
+      @return CRYPT_OK on success
+   */
+   int (*neg)(void *src, void *dst);
+   
+   /** copy 
+      @param   src   The number to copy from
+      @param   dst   The number to write to 
+      @return CRYPT_OK on success
+   */
+   int (*copy)(void *src, void *dst);
+
+/* ---- trivial low level functions ---- */
+
+   /** set small constant 
+      @param a    Number to write to
+      @param n    Source upto bits_per_digit (actually meant for very small constants) 
+      @return CRYPT_OK on succcess
+   */
+   int (*set_int)(void *a, unsigned long n);
+
+   /** get small constant 
+      @param a    Number to read, only fetches upto bits_per_digit from the number
+      @return  The lower bits_per_digit of the integer (unsigned)
+   */
+   unsigned long (*get_int)(void *a);
+
+   /** get digit n 
+     @param a  The number to read from
+     @param n  The number of the digit to fetch
+     @return  The bits_per_digit  sized n'th digit of a
+   */
+   ltc_mp_digit (*get_digit)(void *a, int n);
+
+   /** Get the number of digits that represent the number
+     @param a   The number to count
+     @return The number of digits used to represent the number
+   */
+   int (*get_digit_count)(void *a);
+
+   /** compare two integers
+     @param a   The left side integer
+     @param b   The right side integer
+     @return LTC_MP_LT if a < b, LTC_MP_GT if a > b and LTC_MP_EQ otherwise.  (signed comparison)
+   */
+   int (*compare)(void *a, void *b);
+
+   /** compare against int 
+     @param a   The left side integer
+     @param b   The right side integer (upto bits_per_digit)
+     @return LTC_MP_LT if a < b, LTC_MP_GT if a > b and LTC_MP_EQ otherwise.  (signed comparison)
+   */
+   int (*compare_d)(void *a, unsigned long n);
+
+   /** Count the number of bits used to represent the integer
+     @param a   The integer to count
+     @return The number of bits required to represent the integer
+   */
+   int (*count_bits)(void * a);
+
+   /** Count the number of LSB bits which are zero 
+     @param a   The integer to count
+     @return The number of contiguous zero LSB bits
+   */
+   int (*count_lsb_bits)(void *a);
+
+   /** Compute a power of two
+     @param a  The integer to store the power in
+     @param n  The power of two you want to store (a = 2^n)
+     @return CRYPT_OK on success
+   */
+   int (*twoexpt)(void *a , int n);
+
+/* ---- radix conversions ---- */
+   
+   /** read ascii string 
+     @param a     The integer to store into
+     @param str   The string to read
+     @param radix The radix the integer has been represented in (2-64)
+     @return CRYPT_OK on success
+   */
+   int (*read_radix)(void *a, const char *str, int radix);
+
+   /** write number to string
+     @param a     The integer to store
+     @param str   The destination for the string
+     @param radix The radix the integer is to be represented in (2-64)
+     @return CRYPT_OK on success
+   */
+   int (*write_radix)(void *a, char *str, int radix);
+
+   /** get size as unsigned char string 
+     @param a     The integer to get the size (when stored in array of octets)
+     @return The length of the integer
+   */
+   unsigned long (*unsigned_size)(void *a);
+
+   /** store an integer as an array of octets 
+     @param src   The integer to store
+     @param dst   The buffer to store the integer in
+     @return CRYPT_OK on success
+   */
+   int (*unsigned_write)(void *src, unsigned char *dst);
+
+   /** read an array of octets and store as integer
+     @param dst   The integer to load
+     @param src   The array of octets 
+     @param len   The number of octets 
+     @return CRYPT_OK on success
+   */
+   int (*unsigned_read)(void *dst, unsigned char *src, unsigned long len);
+
+/* ---- basic math ---- */
+
+   /** add two integers 
+     @param a   The first source integer
+     @param b   The second source integer
+     @param c   The destination of "a + b"
+     @return CRYPT_OK on success
+   */
+   int (*add)(void *a, void *b, void *c);
+
+
+   /** add two integers 
+     @param a   The first source integer
+     @param b   The second source integer (single digit of upto bits_per_digit in length)
+     @param c   The destination of "a + b"
+     @return CRYPT_OK on success
+   */
+   int (*addi)(void *a, unsigned long b, void *c);
+
+   /** subtract two integers 
+     @param a   The first source integer
+     @param b   The second source integer
+     @param c   The destination of "a - b"
+     @return CRYPT_OK on success
+   */
+   int (*sub)(void *a, void *b, void *c);
+
+   /** subtract two integers 
+     @param a   The first source integer
+     @param b   The second source integer (single digit of upto bits_per_digit in length)
+     @param c   The destination of "a - b"
+     @return CRYPT_OK on success
+   */
+   int (*subi)(void *a, unsigned long b, void *c);
+
+   /** multiply two integers 
+     @param a   The first source integer
+     @param b   The second source integer (single digit of upto bits_per_digit in length)
+     @param c   The destination of "a * b"
+     @return CRYPT_OK on success
+   */
+   int (*mul)(void *a, void *b, void *c);
+
+   /** multiply two integers 
+     @param a   The first source integer
+     @param b   The second source integer (single digit of upto bits_per_digit in length)
+     @param c   The destination of "a * b"
+     @return CRYPT_OK on success
+   */
+   int (*muli)(void *a, unsigned long b, void *c);
+
+   /** Square an integer
+     @param a    The integer to square
+     @param b    The destination
+     @return CRYPT_OK on success
+   */
+   int (*sqr)(void *a, void *b);
+
+   /** Divide an integer
+     @param a    The dividend
+     @param b    The divisor
+     @param c    The quotient (can be NULL to signify don't care)
+     @param d    The remainder (can be NULL to signify don't care)
+     @return CRYPT_OK on success
+   */
+   int (*mpdiv)(void *a, void *b, void *c, void *d);
+
+   /** divide by two 
+      @param  a   The integer to divide (shift right)
+      @param  b   The destination 
+      @return CRYPT_OK on success
+   */
+   int (*div_2)(void *a, void *b);
+
+   /** Get remainder (small value)
+      @param  a    The integer to reduce
+      @param  b    The modulus (upto bits_per_digit in length)
+      @param  c    The destination for the residue
+      @return CRYPT_OK on success
+   */
+   int (*modi)(void *a, unsigned long b, unsigned long *c);
+
+   /** gcd 
+      @param  a     The first integer
+      @param  b     The second integer
+      @param  c     The destination for (a, b)
+      @return CRYPT_OK on success
+   */
+   int (*gcd)(void *a, void *b, void *c);
+
+   /** lcm 
+      @param  a     The first integer
+      @param  b     The second integer
+      @param  c     The destination for [a, b]
+      @return CRYPT_OK on success
+   */
+   int (*lcm)(void *a, void *b, void *c);
+
+   /** Modular reduction
+      @param  a     The source
+      @param  b     The modulus
+      @param  c     The destination (c = a mod b)
+      @return CRYPT_OK on success
+   */
+   int (*mod)(void *a, void *b, void *c);
+
+
+   /** Modular multiplication
+      @param  a     The first source
+      @param  b     The second source 
+      @param  c     The modulus
+      @param  d     The destination (a*b mod c)
+      @return CRYPT_OK on success
+   */
+   int (*mulmod)(void *a, void *b, void *c, void *d);
+
+   /** Modular squaring
+      @param  a     The first source
+      @param  b     The modulus
+      @param  c     The destination (a*a mod b)
+      @return CRYPT_OK on success
+   */
+   int (*sqrmod)(void *a, void *b, void *c);
+
+   /** Modular inversion
+      @param  a     The value to invert
+      @param  b     The modulus 
+      @param  c     The destination (1/a mod b)
+      @return CRYPT_OK on success
+   */
+   int (*invmod)(void *, void *, void *);
+
+/* ---- reduction ---- */
+
+   /** setup montgomery
+       @param a  The modulus 
+       @param b  The destination for the reduction digit 
+       @return CRYPT_OK on success
+   */
+   int (*montgomery_setup)(void *a, void **b);
+
+   /** get normalization value 
+       @param a   The destination for the normalization value
+       @param b   The modulus
+       @return  CRYPT_OK on success
+   */
+   int (*montgomery_normalization)(void *a, void *b);
+
+   /** reduce a number
+       @param a   The number [and dest] to reduce
+       @param b   The modulus
+       @param c   The value "b" from montgomery_setup()
+       @return CRYPT_OK on success
+   */
+   int (*montgomery_reduce)(void *a, void *b, void *c);
+
+   /** clean up  (frees memory)
+       @param a   The value "b" from montgomery_setup()
+       @return CRYPT_OK on success
+   */      
+   void (*montgomery_deinit)(void *a);
+
+/* ---- exponentiation ---- */
+
+   /** Modular exponentiation
+       @param a    The base integer
+       @param b    The power (can be negative) integer
+       @param c    The modulus integer
+       @param d    The destination
+       @return CRYPT_OK on success
+   */
+   int (*exptmod)(void *a, void *b, void *c, void *d);
+
+   /** Primality testing
+       @param a     The integer to test
+       @param b     The number of tests that shall be executed
+       @param c     The destination of the result (FP_YES if prime)
+       @return CRYPT_OK on success
+   */
+   int (*isprime)(void *a, int b, int *c);
+
+/* ----  (optional) ecc point math ---- */
+
+   /** ECC GF(p) point multiplication (from the NIST curves)
+       @param k   The integer to multiply the point by
+       @param G   The point to multiply
+       @param R   The destination for kG  
+       @param modulus  The modulus for the field
+       @param map Boolean indicated whether to map back to affine or not (can be ignored if you work in affine only)
+       @return CRYPT_OK on success
+   */
+   int (*ecc_ptmul)(void *k, ecc_point *G, ecc_point *R, void *modulus, int map);
+
+   /** ECC GF(p) point addition 
+       @param P    The first point
+       @param Q    The second point
+       @param R    The destination of P + Q
+       @param modulus  The modulus
+       @param mp   The "b" value from montgomery_setup()
+       @return CRYPT_OK on success
+   */
+   int (*ecc_ptadd)(ecc_point *P, ecc_point *Q, ecc_point *R, void *modulus, void *mp);
+
+   /** ECC GF(p) point double 
+       @param P    The first point
+       @param R    The destination of 2P
+       @param modulus  The modulus
+       @param mp   The "b" value from montgomery_setup()
+       @return CRYPT_OK on success
+   */
+   int (*ecc_ptdbl)(ecc_point *P, ecc_point *R, void *modulus, void *mp);
+
+   /** ECC mapping from projective to affine, currently uses (x,y,z) => (x/z^2, y/z^3, 1)
+       @param P     The point to map
+       @param modulus The modulus
+       @param mp    The "b" value from montgomery_setup()
+       @return CRYPT_OK on success
+       @remark  The mapping can be different but keep in mind a ecc_point only has three 
+                integers (x,y,z) so if you use a different mapping you have to make it fit.
+   */
+   int (*ecc_map)(ecc_point *P, void *modulus, void *mp);
+
+   /** Computes kA*A + kB*B = C using Shamir's Trick
+       @param A        First point to multiply
+       @param kA       What to multiple A by
+       @param B        Second point to multiply
+       @param kB       What to multiple B by
+       @param C        [out] Destination point (can overlap with A or B
+       @param modulus  Modulus for curve 
+       @return CRYPT_OK on success
+   */ 
+   int (*ecc_mul2add)(ecc_point *A, void *kA,
+                      ecc_point *B, void *kB,
+                      ecc_point *C,
+                           void *modulus);
+
+/* ---- (optional) rsa optimized math (for internal CRT) ---- */
+
+   /** RSA Key Generation 
+       @param prng     An active PRNG state
+       @param wprng    The index of the PRNG desired
+       @param size     The size of the modulus (key size) desired (octets)
+       @param e        The "e" value (public key).  e==65537 is a good choice
+       @param key      [out] Destination of a newly created private key pair
+       @return CRYPT_OK if successful, upon error all allocated ram is freed
+    */
+    int (*rsa_keygen)(prng_state *prng, int wprng, int size, long e, rsa_key *key);
+   
+
+   /** RSA exponentiation
+      @param in       The octet array representing the base
+      @param inlen    The length of the input
+      @param out      The destination (to be stored in an octet array format)
+      @param outlen   The length of the output buffer and the resulting size (zero padded to the size of the modulus)
+      @param which    PK_PUBLIC for public RSA and PK_PRIVATE for private RSA
+      @param key      The RSA key to use 
+      @return CRYPT_OK on success
+   */
+   int (*rsa_me)(const unsigned char *in,   unsigned long inlen,
+                       unsigned char *out,  unsigned long *outlen, int which,
+                       rsa_key *key);
+} ltc_math_descriptor;
+
+extern ltc_math_descriptor ltc_mp;
+
+int ltc_init_multi(void **a, ...);
+int ltc_init_multi_size(int size_bits, void **a, ...);
+void ltc_deinit_multi(void *a, ...);
+
+#ifdef LTM_DESC
+extern const ltc_math_descriptor ltm_desc;
+#endif
+
+#ifdef TFM_DESC
+extern const ltc_math_descriptor tfm_desc;
+#endif
+
+#ifdef GMP_DESC
+extern const ltc_math_descriptor gmp_desc;
+#endif
+
+#if !defined(DESC_DEF_ONLY) && defined(LTC_SOURCE)
+
+#define MP_DIGIT_BIT                 ltc_mp.bits_per_digit
+
+/* some handy macros */
+#define mp_init(a)                   ltc_mp.init(a)
+#define mp_init_multi                ltc_init_multi
+#define mp_init_size(a, b)           ltc_mp.init_size(a, b)
+#define mp_init_multi_size           ltc_init_multi_size
+#define mp_clear(a)                  ltc_mp.deinit(a)
+#define mp_clear_multi               ltc_deinit_multi
+#define mp_init_copy(a, b)           ltc_mp.init_copy(a, b)
+
+#define mp_neg(a, b)                 ltc_mp.neg(a, b)
+#define mp_copy(a, b)                ltc_mp.copy(a, b)
+
+#define mp_set(a, b)                 ltc_mp.set_int(a, b)
+#define mp_set_int(a, b)             ltc_mp.set_int(a, b)
+#define mp_get_int(a)                ltc_mp.get_int(a)
+#define mp_get_digit(a, n)           ltc_mp.get_digit(a, n)
+#define mp_get_digit_count(a)        ltc_mp.get_digit_count(a)
+#define mp_cmp(a, b)                 ltc_mp.compare(a, b)
+#define mp_cmp_d(a, b)               ltc_mp.compare_d(a, b)
+#define mp_count_bits(a)             ltc_mp.count_bits(a)
+#define mp_cnt_lsb(a)                ltc_mp.count_lsb_bits(a)
+#define mp_2expt(a, b)               ltc_mp.twoexpt(a, b)
+
+#define mp_read_radix(a, b, c)       ltc_mp.read_radix(a, b, c)
+#define mp_toradix(a, b, c)          ltc_mp.write_radix(a, b, c)
+#define mp_unsigned_bin_size(a)      ltc_mp.unsigned_size(a)
+#define mp_to_unsigned_bin(a, b)     ltc_mp.unsigned_write(a, b)
+#define mp_read_unsigned_bin(a, b, c) ltc_mp.unsigned_read(a, b, c)
+
+#define mp_add(a, b, c)              ltc_mp.add(a, b, c)
+#define mp_add_d(a, b, c)            ltc_mp.addi(a, b, c)
+#define mp_sub(a, b, c)              ltc_mp.sub(a, b, c)
+#define mp_sub_d(a, b, c)            ltc_mp.subi(a, b, c)
+#define mp_mul(a, b, c)              ltc_mp.mul(a, b, c)
+#define mp_mul_d(a, b, c)            ltc_mp.muli(a, b, c)
+#define mp_sqr(a, b)                 ltc_mp.sqr(a, b)
+#define mp_div(a, b, c, d)           ltc_mp.mpdiv(a, b, c, d)
+#define mp_div_2(a, b)               ltc_mp.div_2(a, b)
+#define mp_mod(a, b, c)              ltc_mp.mod(a, b, c)
+#define mp_mod_d(a, b, c)            ltc_mp.modi(a, b, c)
+#define mp_gcd(a, b, c)              ltc_mp.gcd(a, b, c)
+#define mp_lcm(a, b, c)              ltc_mp.lcm(a, b, c)
+
+#define mp_mulmod(a, b, c, d)        ltc_mp.mulmod(a, b, c, d)
+#define mp_sqrmod(a, b, c)           ltc_mp.sqrmod(a, b, c)
+#define mp_invmod(a, b, c)           ltc_mp.invmod(a, b, c)
+
+#define mp_montgomery_setup(a, b)    ltc_mp.montgomery_setup(a, b)
+#define mp_montgomery_normalization(a, b) ltc_mp.montgomery_normalization(a, b)
+#define mp_montgomery_reduce(a, b, c)   ltc_mp.montgomery_reduce(a, b, c)
+#define mp_montgomery_free(a)        ltc_mp.montgomery_deinit(a)
+
+#define mp_exptmod(a,b,c,d)          ltc_mp.exptmod(a,b,c,d)
+#define mp_prime_is_prime(a, b, c)   ltc_mp.isprime(a,b,c)
+
+#define mp_iszero(a)                 (mp_cmp_d(a, 0) == LTC_MP_EQ ? LTC_MP_YES : LTC_MP_NO)
+#define mp_isodd(a)                  (mp_get_digit_count(a) > 0 ? (mp_get_digit(a, 0) & 1 ? LTC_MP_YES : LTC_MP_NO) : LTC_MP_NO)
+#define mp_exch(a, b)                do { void *ABC__tmp = a; a = b; b = ABC__tmp; } while(0);
+
+#define mp_tohex(a, b)               mp_toradix(a, b, 16)
+
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/headers/tomcrypt_math.h,v $ */
+/* $Revision: 1.44 $ */
+/* $Date: 2007/05/12 14:32:35 $ */
diff --git a/core/lib/libtomcrypt/include/tomcrypt_misc.h b/core/lib/libtomcrypt/include/tomcrypt_misc.h
new file mode 100644
index 0000000..f346d19
--- /dev/null
+++ b/core/lib/libtomcrypt/include/tomcrypt_misc.h
@@ -0,0 +1,58 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* ---- LTC_BASE64 Routines ---- */
+#ifdef LTC_BASE64
+int base64_encode(const unsigned char *in,  unsigned long len, 
+                        unsigned char *out, unsigned long *outlen);
+
+int base64_decode(const unsigned char *in,  unsigned long len, 
+                        unsigned char *out, unsigned long *outlen);
+#endif
+
+#ifdef LTC_BASE64_URL
+int base64url_encode(const unsigned char *in,  unsigned long len,
+                        unsigned char *out, unsigned long *outlen);
+
+int base64url_decode(const unsigned char *in,  unsigned long len,
+                        unsigned char *out, unsigned long *outlen);
+#endif
+/* ---- MEM routines ---- */
+int mem_neq(const void *a, const void *b, size_t len);
+void zeromem(volatile void *dst, size_t len);
+void burn_stack(unsigned long len);
+
+const char *error_to_string(int err);
+
+extern const char *crypt_build_settings;
+
+/* ---- HMM ---- */
+int crypt_fsa(void *mp, ...);
+
+/* $Source: /cvs/libtom/libtomcrypt/src/headers/tomcrypt_misc.h,v $ */
+/* $Revision: 1.5 $ */
+/* $Date: 2007/05/12 14:32:35 $ */
diff --git a/core/lib/libtomcrypt/include/tomcrypt_mpa.h b/core/lib/libtomcrypt/include/tomcrypt_mpa.h
new file mode 100644
index 0000000..dba1206
--- /dev/null
+++ b/core/lib/libtomcrypt/include/tomcrypt_mpa.h
@@ -0,0 +1,38 @@
+/*
+ * 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.
+ */
+
+#ifndef TOMCRYPT_MPA_H_
+#define TOMCRYPT_MPA_H_
+
+#include <mpalib.h>
+#include "tomcrypt.h"
+
+extern mpa_scratch_mem external_mem_pool;
+
+void init_mpa_tomcrypt(mpa_scratch_mem pool);
+
+#endif /* TOMCRYPT_MPA_H_ */
diff --git a/core/lib/libtomcrypt/include/tomcrypt_pk.h b/core/lib/libtomcrypt/include/tomcrypt_pk.h
new file mode 100644
index 0000000..9b23cc0
--- /dev/null
+++ b/core/lib/libtomcrypt/include/tomcrypt_pk.h
@@ -0,0 +1,657 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* ---- NUMBER THEORY ---- */
+
+enum {
+   PK_PUBLIC=0,
+   PK_PRIVATE=1
+};
+
+#define PK_STD          0x1000
+
+int rand_prime(void *N, long len, prng_state *prng, int wprng);
+int rand_bn_bits(void *N, int bits, prng_state *prng, int wprng);
+int rand_bn_range(void *N, void *limit, prng_state *prng, int wprng);
+
+enum {
+   PKA_RSA,
+   PKA_DSA
+};
+
+typedef struct Oid {
+    unsigned long OID[16];
+    /** Length of DER encoding */
+    unsigned long OIDlen;
+} oid_st;
+
+int pk_get_oid(int pk, oid_st *st);
+
+/* ---- RSA ---- */
+#ifdef LTC_MRSA
+
+/* Min and Max RSA key sizes (in bits) */
+#define MIN_RSA_SIZE 256
+#define MAX_RSA_SIZE 4096
+
+/** RSA LTC_PKCS style key */
+typedef struct Rsa_key {
+    /** Type of key, PK_PRIVATE or PK_PUBLIC */
+    int type;
+    /** The public exponent */
+    void *e; 
+    /** The private exponent */
+    void *d; 
+    /** The modulus */
+    void *N; 
+    /** The p factor of N */
+    void *p; 
+    /** The q factor of N */
+    void *q; 
+    /** The 1/q mod p CRT param */
+    void *qP; 
+    /** The d mod (p - 1) CRT param */
+    void *dP; 
+    /** The d mod (q - 1) CRT param */
+    void *dQ;
+} rsa_key;
+
+int rsa_make_key(prng_state *prng, int wprng, int size, long e, rsa_key *key);
+
+int rsa_exptmod(const unsigned char *in,   unsigned long inlen,
+                      unsigned char *out,  unsigned long *outlen, int which,
+                      rsa_key *key);
+
+void rsa_free(rsa_key *key);
+
+/* These use LTC_PKCS #1 v2.0 padding */
+#define rsa_encrypt_key(_in, _inlen, _out, _outlen, _lparam, _lparamlen, _prng, _prng_idx, _hash_idx, _key) \
+  rsa_encrypt_key_ex(_in, _inlen, _out, _outlen, _lparam, _lparamlen, _prng, _prng_idx, _hash_idx, LTC_PKCS_1_OAEP, _key)
+
+#define rsa_decrypt_key(_in, _inlen, _out, _outlen, _lparam, _lparamlen, _hash_idx, _stat, _key) \
+  rsa_decrypt_key_ex(_in, _inlen, _out, _outlen, _lparam, _lparamlen, _hash_idx, LTC_PKCS_1_OAEP, _stat, _key)
+
+#define rsa_sign_hash(_in, _inlen, _out, _outlen, _prng, _prng_idx, _hash_idx, _saltlen, _key) \
+  rsa_sign_hash_ex(_in, _inlen, _out, _outlen, LTC_PKCS_1_PSS, _prng, _prng_idx, _hash_idx, _saltlen, _key)
+
+#define rsa_verify_hash(_sig, _siglen, _hash, _hashlen, _hash_idx, _saltlen, _stat, _key) \
+  rsa_verify_hash_ex(_sig, _siglen, _hash, _hashlen, LTC_PKCS_1_PSS, _hash_idx, _saltlen, _stat, _key)
+
+/* These can be switched between LTC_PKCS #1 v2.x and LTC_PKCS #1 v1.5 paddings */
+int rsa_encrypt_key_ex(const unsigned char *in,     unsigned long inlen,
+                             unsigned char *out,    unsigned long *outlen,
+                       const unsigned char *lparam, unsigned long lparamlen,
+                       prng_state *prng, int prng_idx, int hash_idx, int padding, rsa_key *key);
+
+int rsa_decrypt_key_ex(const unsigned char *in,       unsigned long  inlen,
+                             unsigned char *out,      unsigned long *outlen,
+                       const unsigned char *lparam,   unsigned long  lparamlen,
+                             int            hash_idx, int            padding,
+                             int           *stat,     rsa_key       *key);
+
+int rsa_sign_hash_ex(const unsigned char *in,       unsigned long  inlen,
+                           unsigned char *out,      unsigned long *outlen,
+                           int            padding,
+                           prng_state    *prng,     int            prng_idx,
+                           int            hash_idx, unsigned long  saltlen,
+                           rsa_key *key);
+
+int rsa_verify_hash_ex(const unsigned char *sig,      unsigned long siglen,
+                       const unsigned char *hash,     unsigned long hashlen,
+                             int            padding,
+                             int            hash_idx, unsigned long saltlen,
+                             int           *stat,     rsa_key      *key);
+
+/* LTC_PKCS #1 import/export */
+int rsa_export(unsigned char *out, unsigned long *outlen, int type, rsa_key *key);
+int rsa_import(const unsigned char *in, unsigned long inlen, rsa_key *key);
+                        
+#endif
+
+/* ---- Katja ---- */
+#ifdef LTC_MKAT
+
+/* Min and Max KAT key sizes (in bits) */
+#define MIN_KAT_SIZE 1024
+#define MAX_KAT_SIZE 4096
+
+/** Katja LTC_PKCS style key */
+typedef struct KAT_key {
+    /** Type of key, PK_PRIVATE or PK_PUBLIC */
+    int type;
+    /** The private exponent */
+    void *d; 
+    /** The modulus */
+    void *N; 
+    /** The p factor of N */
+    void *p; 
+    /** The q factor of N */
+    void *q; 
+    /** The 1/q mod p CRT param */
+    void *qP; 
+    /** The d mod (p - 1) CRT param */
+    void *dP; 
+    /** The d mod (q - 1) CRT param */
+    void *dQ;
+    /** The pq param */
+    void *pq;
+} katja_key;
+
+int katja_make_key(prng_state *prng, int wprng, int size, katja_key *key);
+
+int katja_exptmod(const unsigned char *in,   unsigned long inlen,
+                        unsigned char *out,  unsigned long *outlen, int which,
+                        katja_key *key);
+
+void katja_free(katja_key *key);
+
+/* These use LTC_PKCS #1 v2.0 padding */
+int katja_encrypt_key(const unsigned char *in,     unsigned long inlen,
+                            unsigned char *out,    unsigned long *outlen,
+                      const unsigned char *lparam, unsigned long lparamlen,
+                      prng_state *prng, int prng_idx, int hash_idx, katja_key *key);
+                                        
+int katja_decrypt_key(const unsigned char *in,       unsigned long inlen,
+                            unsigned char *out,      unsigned long *outlen, 
+                      const unsigned char *lparam,   unsigned long lparamlen,
+                            int            hash_idx, int *stat,
+                            katja_key       *key);
+
+/* LTC_PKCS #1 import/export */
+int katja_export(unsigned char *out, unsigned long *outlen, int type, katja_key *key);
+int katja_import(const unsigned char *in, unsigned long inlen, katja_key *key);
+                        
+#endif
+
+/*
+ * From libtomcrypt.org:
+ *     DH vanished because nobody used it and it was a pain to support
+ * DH support has been taken from
+ *     http://dev.openaos.org/browser/trunk/buildroot/gen7/buildroot/package/libtomcrypt/libtomcrypt-dh.patch
+ */
+#ifdef LTC_LINARO_FIX_DH
+/* ---- DH Routines ---- */
+#ifdef LTC_MDH
+
+typedef struct Dh_key {
+    int type;	/* Type of key, PK_PRIVATE or PK_PUBLIC */
+    void *x;	/* private key */
+    void *y;	/* public key - is g^x [p] */
+    void *g;	/* base */
+    void *p;	/* prime */
+} dh_key;
+
+int dh_make_key(prng_state *prng, int wprng, void *q, int xbits, dh_key *key);
+void dh_free(dh_key *key);
+int dh_shared_secret(dh_key *private_key, void *public_key, void *secret);
+#endif
+#endif
+
+/* ---- ECC Routines ---- */
+#ifdef LTC_MECC
+
+/* size of our temp buffers for exported keys */
+#define ECC_BUF_SIZE 256
+
+/* max private key size */
+#define ECC_MAXSIZE  66
+
+/** Structure defines a NIST GF(p) curve */
+typedef struct {
+   /** The size of the curve in octets */
+   int size;
+
+   /** name of curve */
+   const char *name;
+
+   /** The prime that defines the field the curve is in (encoded in hex) */
+   const char *prime;
+
+   /** The fields B param (hex) */
+   const char *B;
+
+   /** The order of the curve (hex) */
+   const char *order;
+  
+   /** The x co-ordinate of the base point on the curve (hex) */
+   const char *Gx;
+ 
+   /** The y co-ordinate of the base point on the curve (hex) */
+   const char *Gy;
+} ltc_ecc_set_type;
+
+/** A point on a ECC curve, stored in Jacbobian format such that (x,y,z) => (x/z^2, y/z^3, 1) when interpretted as affine */
+typedef struct {
+    /** The x co-ordinate */
+    void *x;
+
+    /** The y co-ordinate */
+    void *y;
+
+    /** The z co-ordinate */
+    void *z;
+} ecc_point;
+
+/** An ECC key */
+typedef struct {
+    /** Type of key, PK_PRIVATE or PK_PUBLIC */
+    int type;
+
+    /** Index into the ltc_ecc_sets[] for the parameters of this curve; if -1, then this key is using user supplied curve in dp */
+    int idx;
+
+	/** pointer to domain parameters; either points to NIST curves (identified by idx >= 0) or user supplied curve */
+	const ltc_ecc_set_type *dp;
+
+    /** The public key */
+    ecc_point pubkey;
+
+    /** The private key */
+    void *k;
+} ecc_key;
+
+/** the ECC params provided */
+extern const ltc_ecc_set_type ltc_ecc_sets[];
+
+int  ecc_test(void);
+void ecc_sizes(int *low, int *high);
+int  ecc_get_size(ecc_key *key);
+
+int  ecc_make_key(prng_state *prng, int wprng, int keysize, ecc_key *key);
+int  ecc_make_key_ex(prng_state *prng, int wprng, ecc_key *key, const ltc_ecc_set_type *dp);
+void ecc_free(ecc_key *key);
+
+int  ecc_export(unsigned char *out, unsigned long *outlen, int type, ecc_key *key);
+int  ecc_import(const unsigned char *in, unsigned long inlen, ecc_key *key);
+int  ecc_import_ex(const unsigned char *in, unsigned long inlen, ecc_key *key, const ltc_ecc_set_type *dp);
+
+int ecc_ansi_x963_export(ecc_key *key, unsigned char *out, unsigned long *outlen);
+int ecc_ansi_x963_import(const unsigned char *in, unsigned long inlen, ecc_key *key);
+int ecc_ansi_x963_import_ex(const unsigned char *in, unsigned long inlen, ecc_key *key, ltc_ecc_set_type *dp);
+
+int  ecc_shared_secret(ecc_key *private_key, ecc_key *public_key, 
+                       unsigned char *out, unsigned long *outlen);
+
+int  ecc_encrypt_key(const unsigned char *in,   unsigned long inlen,
+                           unsigned char *out,  unsigned long *outlen, 
+                           prng_state *prng, int wprng, int hash, 
+                           ecc_key *key);
+
+int  ecc_decrypt_key(const unsigned char *in,  unsigned long  inlen,
+                           unsigned char *out, unsigned long *outlen, 
+                           ecc_key *key);
+
+int ecc_sign_hash_raw(const unsigned char *in,  unsigned long inlen,
+                            void   *r,   void *s,
+                            prng_state *prng, int wprng, ecc_key *key);
+
+int  ecc_sign_hash(const unsigned char *in,  unsigned long inlen, 
+                         unsigned char *out, unsigned long *outlen, 
+                         prng_state *prng, int wprng, ecc_key *key);
+
+int ecc_verify_hash_raw(      void   *r, void   *s,
+                        const unsigned char *hash, unsigned long hashlen,
+                        int *stat, ecc_key *key);
+
+int  ecc_verify_hash(const unsigned char *sig,  unsigned long siglen,
+                     const unsigned char *hash, unsigned long hashlen, 
+                     int *stat, ecc_key *key);
+
+/* low level functions */
+ecc_point *ltc_ecc_new_point(void);
+void       ltc_ecc_del_point(ecc_point *p);
+int        ltc_ecc_is_valid_idx(int n);
+
+/* point ops (mp == montgomery digit) */
+#if !defined(LTC_MECC_ACCEL) || defined(LTM_DESC) || defined(GMP_DESC)
+/* R = 2P */
+int ltc_ecc_projective_dbl_point(ecc_point *P, ecc_point *R, void *modulus, void *mp);
+
+/* R = P + Q */
+int ltc_ecc_projective_add_point(ecc_point *P, ecc_point *Q, ecc_point *R, void *modulus, void *mp);
+#endif
+
+#if defined(LTC_MECC_FP)
+/* optimized point multiplication using fixed point cache (HAC algorithm 14.117) */
+int ltc_ecc_fp_mulmod(void *k, ecc_point *G, ecc_point *R, void *modulus, int map);
+
+/* functions for saving/loading/freeing/adding to fixed point cache */
+int ltc_ecc_fp_save_state(unsigned char **out, unsigned long *outlen);
+int ltc_ecc_fp_restore_state(unsigned char *in, unsigned long inlen);
+void ltc_ecc_fp_free(void);
+int ltc_ecc_fp_add_point(ecc_point *g, void *modulus, int lock);
+
+/* lock/unlock all points currently in fixed point cache */
+void ltc_ecc_fp_tablelock(int lock);
+#endif
+
+/* R = kG */
+int ltc_ecc_mulmod(void *k, ecc_point *G, ecc_point *R, void *modulus, int map);
+
+#ifdef LTC_ECC_SHAMIR
+/* kA*A + kB*B = C */
+int ltc_ecc_mul2add(ecc_point *A, void *kA,
+                    ecc_point *B, void *kB,
+                    ecc_point *C,
+                         void *modulus);
+
+#ifdef LTC_MECC_FP
+/* Shamir's trick with optimized point multiplication using fixed point cache */
+int ltc_ecc_fp_mul2add(ecc_point *A, void *kA,
+                       ecc_point *B, void *kB,
+                       ecc_point *C, void *modulus);
+#endif
+
+#endif
+
+
+/* map P to affine from projective */
+int ltc_ecc_map(ecc_point *P, void *modulus, void *mp);
+
+#endif
+
+#ifdef LTC_MDSA
+
+/* Max diff between group and modulus size in bytes */
+#define LTC_MDSA_DELTA     512
+
+/* Max DSA group size in bytes (default allows 4k-bit groups) */
+#define LTC_MDSA_MAX_GROUP 512
+
+/** DSA key structure */
+typedef struct {
+   /** The key type, PK_PRIVATE or PK_PUBLIC */
+   int type; 
+
+   /** The order of the sub-group used in octets */
+   int qord;
+
+   /** The generator  */
+   void *g;
+
+   /** The prime used to generate the sub-group */
+   void *q;
+
+   /** The large prime that generats the field the contains the sub-group */
+   void *p;
+
+   /** The private key */
+   void *x;
+
+   /** The public key */
+   void *y;
+} dsa_key;
+
+int dsa_make_key(prng_state *prng, int wprng, int group_size, int modulus_size, dsa_key *key);
+void dsa_free(dsa_key *key);
+
+int dsa_sign_hash_raw(const unsigned char *in,  unsigned long inlen,
+                                   void *r,   void *s,
+                               prng_state *prng, int wprng, dsa_key *key);
+
+int dsa_sign_hash(const unsigned char *in,  unsigned long inlen,
+                        unsigned char *out, unsigned long *outlen,
+                        prng_state *prng, int wprng, dsa_key *key);
+
+int dsa_verify_hash_raw(         void *r,          void *s,
+                    const unsigned char *hash, unsigned long hashlen, 
+                                    int *stat,      dsa_key *key);
+
+int dsa_verify_hash(const unsigned char *sig,  unsigned long siglen,
+                    const unsigned char *hash, unsigned long hashlen, 
+                          int           *stat, dsa_key       *key);
+
+int dsa_encrypt_key(const unsigned char *in,   unsigned long inlen,
+                          unsigned char *out,  unsigned long *outlen, 
+                          prng_state *prng, int wprng, int hash, 
+                          dsa_key *key);
+                      
+int dsa_decrypt_key(const unsigned char *in,  unsigned long  inlen,
+                          unsigned char *out, unsigned long *outlen, 
+                          dsa_key *key);
+                          
+int dsa_import(const unsigned char *in, unsigned long inlen, dsa_key *key);
+int dsa_export(unsigned char *out, unsigned long *outlen, int type, dsa_key *key);
+int dsa_verify_key(dsa_key *key, int *stat);
+
+int dsa_shared_secret(void          *private_key, void *base,
+                      dsa_key       *public_key,
+                      unsigned char *out,         unsigned long *outlen);
+#endif
+
+#ifdef LTC_DER
+/* DER handling */
+
+typedef enum ltc_asn1_type_ {
+ /*  0 */
+ LTC_ASN1_EOL,
+ LTC_ASN1_BOOLEAN,
+ LTC_ASN1_INTEGER,
+ LTC_ASN1_SHORT_INTEGER,
+ LTC_ASN1_BIT_STRING,
+ /*  5 */
+ LTC_ASN1_OCTET_STRING,
+ LTC_ASN1_NULL,
+ LTC_ASN1_OBJECT_IDENTIFIER,
+ LTC_ASN1_IA5_STRING,
+ LTC_ASN1_PRINTABLE_STRING,
+ /* 10 */
+ LTC_ASN1_UTF8_STRING,
+ LTC_ASN1_UTCTIME,
+ LTC_ASN1_CHOICE,
+ LTC_ASN1_SEQUENCE,
+ LTC_ASN1_SET,
+ /* 15 */
+ LTC_ASN1_SETOF,
+ LTC_ASN1_RAW_BIT_STRING,
+ LTC_ASN1_TELETEX_STRING,
+ LTC_ASN1_CONSTRUCTED,
+ LTC_ASN1_CONTEXT_SPECIFIC,
+} ltc_asn1_type;
+
+/** A LTC ASN.1 list type */
+typedef struct ltc_asn1_list_ {
+   /** The LTC ASN.1 enumerated type identifier */
+   ltc_asn1_type type;
+   /** The data to encode or place for decoding */
+   void         *data;
+   /** The size of the input or resulting output */
+   unsigned long size;
+   /** The used flag, this is used by the CHOICE ASN.1 type to indicate which choice was made */
+   int           used;
+   /** prev/next entry in the list */
+   struct ltc_asn1_list_ *prev, *next, *child, *parent;
+} ltc_asn1_list;
+
+#define LTC_SET_ASN1(list, index, Type, Data, Size)  \
+   do {                                              \
+      int LTC_MACRO_temp            = (index);       \
+      ltc_asn1_list *LTC_MACRO_list = (list);        \
+      LTC_MACRO_list[LTC_MACRO_temp].type = (Type);  \
+      LTC_MACRO_list[LTC_MACRO_temp].data = (void*)(Data);  \
+      LTC_MACRO_list[LTC_MACRO_temp].size = (Size);  \
+      LTC_MACRO_list[LTC_MACRO_temp].used = 0;       \
+   } while (0)
+
+/* SEQUENCE */
+int der_encode_sequence_ex(ltc_asn1_list *list, unsigned long inlen,
+                           unsigned char *out,  unsigned long *outlen, int type_of);
+                          
+#define der_encode_sequence(list, inlen, out, outlen) der_encode_sequence_ex(list, inlen, out, outlen, LTC_ASN1_SEQUENCE)
+
+int der_decode_sequence_ex(const unsigned char *in, unsigned long  inlen,
+                           ltc_asn1_list *list,     unsigned long  outlen, int ordered);
+                              
+#define der_decode_sequence(in, inlen, list, outlen) der_decode_sequence_ex(in, inlen, list, outlen, 1)
+
+int der_length_sequence(ltc_asn1_list *list, unsigned long inlen,
+                        unsigned long *outlen);
+
+/* SUBJECT PUBLIC KEY INFO */
+int der_encode_subject_public_key_info(unsigned char *out, unsigned long *outlen,
+        unsigned int algorithm, void* public_key, unsigned long public_key_len,
+        unsigned long parameters_type, void* parameters, unsigned long parameters_len);
+
+int der_decode_subject_public_key_info(const unsigned char *in, unsigned long inlen,
+        unsigned int algorithm, void* public_key, unsigned long* public_key_len,
+        unsigned long parameters_type, ltc_asn1_list* parameters, unsigned long parameters_len);
+
+/* SET */
+#define der_decode_set(in, inlen, list, outlen) der_decode_sequence_ex(in, inlen, list, outlen, 0)
+#define der_length_set der_length_sequence
+int der_encode_set(ltc_asn1_list *list, unsigned long inlen,
+                   unsigned char *out,  unsigned long *outlen);
+
+int der_encode_setof(ltc_asn1_list *list, unsigned long inlen,
+                     unsigned char *out,  unsigned long *outlen);
+                        
+/* VA list handy helpers with triplets of <type, size, data> */
+int der_encode_sequence_multi(unsigned char *out, unsigned long *outlen, ...);
+int der_decode_sequence_multi(const unsigned char *in, unsigned long inlen, ...);
+
+/* FLEXI DECODER handle unknown list decoder */
+int  der_decode_sequence_flexi(const unsigned char *in, unsigned long *inlen, ltc_asn1_list **out);
+#define der_free_sequence_flexi         der_sequence_free
+void der_sequence_free(ltc_asn1_list *in);
+
+/* BOOLEAN */
+int der_length_boolean(unsigned long *outlen);
+int der_encode_boolean(int in, 
+                       unsigned char *out, unsigned long *outlen);
+int der_decode_boolean(const unsigned char *in, unsigned long inlen,
+                                       int *out);		       
+/* INTEGER */
+int der_encode_integer(void *num, unsigned char *out, unsigned long *outlen);
+int der_decode_integer(const unsigned char *in, unsigned long inlen, void *num);
+int der_length_integer(void *num, unsigned long *len);
+
+/* INTEGER -- handy for 0..2^32-1 values */
+int der_decode_short_integer(const unsigned char *in, unsigned long inlen, unsigned long *num);
+int der_encode_short_integer(unsigned long num, unsigned char *out, unsigned long *outlen);
+int der_length_short_integer(unsigned long num, unsigned long *outlen);
+
+/* BIT STRING */
+int der_encode_bit_string(const unsigned char *in, unsigned long inlen,
+                                unsigned char *out, unsigned long *outlen);
+int der_decode_bit_string(const unsigned char *in, unsigned long inlen,
+                                unsigned char *out, unsigned long *outlen);
+int der_encode_raw_bit_string(const unsigned char *in, unsigned long inlen,
+                                unsigned char *out, unsigned long *outlen);
+int der_decode_raw_bit_string(const unsigned char *in, unsigned long inlen,
+                                unsigned char *out, unsigned long *outlen);
+int der_length_bit_string(unsigned long nbits, unsigned long *outlen);
+
+/* OCTET STRING */
+int der_encode_octet_string(const unsigned char *in, unsigned long inlen,
+                                  unsigned char *out, unsigned long *outlen);
+int der_decode_octet_string(const unsigned char *in, unsigned long inlen,
+                                  unsigned char *out, unsigned long *outlen);
+int der_length_octet_string(unsigned long noctets, unsigned long *outlen);
+
+/* OBJECT IDENTIFIER */
+int der_encode_object_identifier(unsigned long *words, unsigned long  nwords,
+                                 unsigned char *out,   unsigned long *outlen);
+int der_decode_object_identifier(const unsigned char *in,    unsigned long  inlen,
+                                       unsigned long *words, unsigned long *outlen);
+int der_length_object_identifier(unsigned long *words, unsigned long nwords, unsigned long *outlen);
+unsigned long der_object_identifier_bits(unsigned long x);
+
+/* IA5 STRING */
+int der_encode_ia5_string(const unsigned char *in, unsigned long inlen,
+                                unsigned char *out, unsigned long *outlen);
+int der_decode_ia5_string(const unsigned char *in, unsigned long inlen,
+                                unsigned char *out, unsigned long *outlen);
+int der_length_ia5_string(const unsigned char *octets, unsigned long noctets, unsigned long *outlen);
+
+int der_ia5_char_encode(int c);
+int der_ia5_value_decode(int v);
+
+/* TELETEX STRING */
+int der_decode_teletex_string(const unsigned char *in, unsigned long inlen,
+                                unsigned char *out, unsigned long *outlen);
+int der_length_teletex_string(const unsigned char *octets, unsigned long noctets, unsigned long *outlen);
+
+int der_teletex_char_encode(int c);
+int der_teletex_value_decode(int v);
+
+/* PRINTABLE STRING */
+int der_encode_printable_string(const unsigned char *in, unsigned long inlen,
+                                unsigned char *out, unsigned long *outlen);
+int der_decode_printable_string(const unsigned char *in, unsigned long inlen,
+                                unsigned char *out, unsigned long *outlen);
+int der_length_printable_string(const unsigned char *octets, unsigned long noctets, unsigned long *outlen);
+
+int der_printable_char_encode(int c);
+int der_printable_value_decode(int v);
+
+/* UTF-8 */
+#if (defined(SIZE_MAX) || __STDC_VERSION__ >= 199901L || defined(WCHAR_MAX) || defined(_WCHAR_T) || defined(_WCHAR_T_DEFINED) || defined (__WCHAR_TYPE__)) && !defined(LTC_NO_WCHAR) 
+#include <wchar.h>
+#else
+typedef ulong32 wchar_t;
+#endif
+
+int der_encode_utf8_string(const wchar_t *in,  unsigned long inlen,
+                           unsigned char *out, unsigned long *outlen);
+
+int der_decode_utf8_string(const unsigned char *in,  unsigned long inlen,
+                                       wchar_t *out, unsigned long *outlen);
+unsigned long der_utf8_charsize(const wchar_t c);
+int der_length_utf8_string(const wchar_t *in, unsigned long noctets, unsigned long *outlen);
+
+
+/* CHOICE */
+int der_decode_choice(const unsigned char *in,   unsigned long *inlen,
+                            ltc_asn1_list *list, unsigned long  outlen);
+
+/* UTCTime */
+typedef struct {
+   unsigned YY, /* year */
+            MM, /* month */
+            DD, /* day */
+            hh, /* hour */
+            mm, /* minute */
+            ss, /* second */
+            off_dir, /* timezone offset direction 0 == +, 1 == - */
+            off_hh, /* timezone offset hours */
+            off_mm; /* timezone offset minutes */
+} ltc_utctime;
+
+int der_encode_utctime(ltc_utctime *utctime, 
+                       unsigned char *out,   unsigned long *outlen);
+
+int der_decode_utctime(const unsigned char *in, unsigned long *inlen,
+                             ltc_utctime   *out);
+
+int der_length_utctime(ltc_utctime *utctime, unsigned long *outlen);
+
+
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/headers/tomcrypt_pk.h,v $ */
+/* $Revision: 1.81 $ */
+/* $Date: 2007/05/12 14:32:35 $ */
diff --git a/core/lib/libtomcrypt/include/tomcrypt_pkcs.h b/core/lib/libtomcrypt/include/tomcrypt_pkcs.h
new file mode 100644
index 0000000..30cda91
--- /dev/null
+++ b/core/lib/libtomcrypt/include/tomcrypt_pkcs.h
@@ -0,0 +1,116 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LTC_PKCS Header Info */
+
+/* ===> LTC_PKCS #1 -- RSA Cryptography <=== */
+#ifdef LTC_PKCS_1
+
+enum ltc_pkcs_1_v1_5_blocks
+{
+  LTC_PKCS_1_EMSA   = 1,        /* Block type 1 (PKCS #1 v1.5 signature padding) */
+  LTC_PKCS_1_EME    = 2         /* Block type 2 (PKCS #1 v1.5 encryption padding) */
+};
+
+enum ltc_pkcs_1_paddings
+{
+  LTC_PKCS_1_V1_5   = 1,        /* PKCS #1 v1.5 padding (\sa ltc_pkcs_1_v1_5_blocks) */
+  LTC_PKCS_1_OAEP   = 2,        /* PKCS #1 v2.0 encryption padding */
+  LTC_PKCS_1_PSS    = 3         /* PKCS #1 v2.1 signature padding */
+};
+
+int pkcs_1_mgf1(      int            hash_idx,
+                const unsigned char *seed, unsigned long seedlen,
+                      unsigned char *mask, unsigned long masklen);
+
+int pkcs_1_i2osp(void *n, unsigned long modulus_len, unsigned char *out);
+int pkcs_1_os2ip(void *n, unsigned char *in, unsigned long inlen);
+
+/* *** v1.5 padding */
+int pkcs_1_v1_5_encode(const unsigned char *msg, 
+                             unsigned long  msglen,
+                             int            block_type,
+                             unsigned long  modulus_bitlen,
+                                prng_state *prng, 
+                                       int  prng_idx,
+                             unsigned char *out, 
+                             unsigned long *outlen);
+
+int pkcs_1_v1_5_decode(const unsigned char *msg, 
+                             unsigned long  msglen,
+                                       int  block_type,
+                             unsigned long  modulus_bitlen,
+                             unsigned char *out, 
+                             unsigned long *outlen,
+                                       int *is_valid);
+
+/* *** v2.1 padding */
+int pkcs_1_oaep_encode(const unsigned char *msg,    unsigned long msglen,
+                       const unsigned char *lparam, unsigned long lparamlen,
+                             unsigned long modulus_bitlen, prng_state *prng,
+                             int           prng_idx,         int  hash_idx,
+                             unsigned char *out,    unsigned long *outlen);
+
+int pkcs_1_oaep_decode(const unsigned char *msg,    unsigned long msglen,
+                       const unsigned char *lparam, unsigned long lparamlen,
+                             unsigned long modulus_bitlen, int hash_idx,
+                             unsigned char *out,    unsigned long *outlen,
+                             int           *res);
+
+int pkcs_1_pss_encode(const unsigned char *msghash, unsigned long msghashlen,
+                            unsigned long saltlen,  prng_state   *prng,     
+                            int           prng_idx, int           hash_idx,
+                            unsigned long modulus_bitlen,
+                            unsigned char *out,     unsigned long *outlen);
+
+int pkcs_1_pss_decode(const unsigned char *msghash, unsigned long msghashlen,
+                      const unsigned char *sig,     unsigned long siglen,
+                            unsigned long saltlen,  int           hash_idx,
+                            unsigned long modulus_bitlen, int    *res);
+
+#endif /* LTC_PKCS_1 */
+
+/* ===> LTC_PKCS #5 -- Password Based Cryptography <=== */
+#ifdef LTC_PKCS_5
+
+/* Algorithm #1 (old) */
+int pkcs_5_alg1(const unsigned char *password, unsigned long password_len, 
+                const unsigned char *salt, 
+                int iteration_count,  int hash_idx,
+                unsigned char *out,   unsigned long *outlen);
+
+/* Algorithm #2 (new) */
+int pkcs_5_alg2(const unsigned char *password, unsigned long password_len, 
+                const unsigned char *salt,     unsigned long salt_len,
+                int iteration_count,           int hash_idx,
+                unsigned char *out,            unsigned long *outlen);
+
+#endif  /* LTC_PKCS_5 */
+
+/* $Source: /cvs/libtom/libtomcrypt/src/headers/tomcrypt_pkcs.h,v $ */
+/* $Revision: 1.8 $ */
+/* $Date: 2007/05/12 14:32:35 $ */
diff --git a/core/lib/libtomcrypt/include/tomcrypt_prng.h b/core/lib/libtomcrypt/include/tomcrypt_prng.h
new file mode 100644
index 0000000..bc8b828
--- /dev/null
+++ b/core/lib/libtomcrypt/include/tomcrypt_prng.h
@@ -0,0 +1,226 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* ---- PRNG Stuff ---- */
+#ifdef LTC_YARROW
+struct yarrow_prng {
+    int                   cipher, hash;
+    unsigned char         pool[MAXBLOCKSIZE];
+    symmetric_CTR         ctr;
+    LTC_MUTEX_TYPE(prng_lock)
+};
+#endif
+
+#ifdef LTC_RC4
+struct rc4_prng {
+    int x, y;
+    unsigned char buf[256];
+};
+#endif
+
+#ifdef LTC_FORTUNA
+struct fortuna_prng {
+    hash_state pool[LTC_FORTUNA_POOLS];     /* the  pools */
+
+    symmetric_key skey;
+
+    unsigned char K[32],      /* the current key */
+                  IV[16];     /* IV for CTR mode */
+    
+    unsigned long pool_idx,   /* current pool we will add to */
+                  pool0_len,  /* length of 0'th pool */
+                  wd;            
+
+    ulong64       reset_cnt;  /* number of times we have reset */
+    LTC_MUTEX_TYPE(prng_lock)
+};
+#endif
+
+#ifdef LTC_SOBER128
+struct sober128_prng {
+    ulong32      R[17],          /* Working storage for the shift register */
+                 initR[17],      /* saved register contents */ 
+                 konst,          /* key dependent constant */
+                 sbuf;           /* partial word encryption buffer */
+
+    int          nbuf,           /* number of part-word stream bits buffered */
+                 flag,           /* first add_entropy call or not? */
+                 set;            /* did we call add_entropy to set key? */
+    
+};
+#endif
+
+typedef union Prng_state {
+    char dummy[1];
+#ifdef LTC_YARROW
+    struct yarrow_prng    yarrow;
+#endif
+#ifdef LTC_RC4
+    struct rc4_prng       rc4;
+#endif
+#ifdef LTC_FORTUNA
+    struct fortuna_prng   fortuna;
+#endif
+#ifdef LTC_SOBER128
+    struct sober128_prng  sober128;
+#endif
+} prng_state;
+
+/** PRNG descriptor */
+extern const struct ltc_prng_descriptor {
+    /** Name of the PRNG */
+    const char *name;
+    /** size in bytes of exported state */
+    int  export_size;
+    /** Start a PRNG state
+        @param prng   [out] The state to initialize
+        @return CRYPT_OK if successful
+    */
+    int (*start)(prng_state *prng);
+    /** Add entropy to the PRNG
+        @param in         The entropy
+        @param inlen      Length of the entropy (octets)\
+        @param prng       The PRNG state
+        @return CRYPT_OK if successful
+    */
+    int (*add_entropy)(const unsigned char *in, unsigned long inlen, prng_state *prng);
+    /** Ready a PRNG state to read from
+        @param prng       The PRNG state to ready
+        @return CRYPT_OK if successful
+    */
+    int (*ready)(prng_state *prng);
+    /** Read from the PRNG
+        @param out     [out] Where to store the data
+        @param outlen  Length of data desired (octets)
+        @param prng    The PRNG state to read from
+        @return Number of octets read
+    */
+    unsigned long (*read)(unsigned char *out, unsigned long outlen, prng_state *prng);
+    /** Terminate a PRNG state
+        @param prng   The PRNG state to terminate
+        @return CRYPT_OK if successful
+    */
+    int (*done)(prng_state *prng);
+    /** Export a PRNG state  
+        @param out     [out] The destination for the state
+        @param outlen  [in/out] The max size and resulting size of the PRNG state
+        @param prng    The PRNG to export
+        @return CRYPT_OK if successful
+    */
+    int (*pexport)(unsigned char *out, unsigned long *outlen, prng_state *prng);
+    /** Import a PRNG state
+        @param in      The data to import
+        @param inlen   The length of the data to import (octets)
+        @param prng    The PRNG to initialize/import
+        @return CRYPT_OK if successful
+    */
+    int (*pimport)(const unsigned char *in, unsigned long inlen, prng_state *prng);
+    /** Self-test the PRNG
+        @return CRYPT_OK if successful, CRYPT_NOP if self-testing has been disabled
+    */
+    int (*test)(void);
+} *prng_descriptor[];
+
+#ifdef LTC_YARROW
+int yarrow_start(prng_state *prng);
+int yarrow_add_entropy(const unsigned char *in, unsigned long inlen, prng_state *prng);
+int yarrow_ready(prng_state *prng);
+unsigned long yarrow_read(unsigned char *out, unsigned long outlen, prng_state *prng);
+int yarrow_done(prng_state *prng);
+int  yarrow_export(unsigned char *out, unsigned long *outlen, prng_state *prng);
+int  yarrow_import(const unsigned char *in, unsigned long inlen, prng_state *prng);
+int  yarrow_test(void);
+extern const struct ltc_prng_descriptor yarrow_desc;
+#endif
+
+#ifdef LTC_FORTUNA
+int fortuna_start(prng_state *prng);
+int fortuna_add_entropy(const unsigned char *in, unsigned long inlen, prng_state *prng);
+int fortuna_ready(prng_state *prng);
+unsigned long fortuna_read(unsigned char *out, unsigned long outlen, prng_state *prng);
+int fortuna_done(prng_state *prng);
+int  fortuna_export(unsigned char *out, unsigned long *outlen, prng_state *prng);
+int  fortuna_import(const unsigned char *in, unsigned long inlen, prng_state *prng);
+int  fortuna_test(void);
+extern const struct ltc_prng_descriptor fortuna_desc;
+#endif
+
+#ifdef LTC_RC4
+int rc4_start(prng_state *prng);
+int rc4_add_entropy(const unsigned char *in, unsigned long inlen, prng_state *prng);
+int rc4_ready(prng_state *prng);
+unsigned long rc4_read(unsigned char *out, unsigned long outlen, prng_state *prng);
+int  rc4_done(prng_state *prng);
+int  rc4_export(unsigned char *out, unsigned long *outlen, prng_state *prng);
+int  rc4_import(const unsigned char *in, unsigned long inlen, prng_state *prng);
+int  rc4_test(void);
+extern const struct ltc_prng_descriptor rc4_desc;
+#endif
+
+#ifdef LTC_SPRNG
+int sprng_start(prng_state *prng);
+int sprng_add_entropy(const unsigned char *in, unsigned long inlen, prng_state *prng);
+int sprng_ready(prng_state *prng);
+unsigned long sprng_read(unsigned char *out, unsigned long outlen, prng_state *prng);
+int sprng_done(prng_state *prng);
+int  sprng_export(unsigned char *out, unsigned long *outlen, prng_state *prng);
+int  sprng_import(const unsigned char *in, unsigned long inlen, prng_state *prng);
+int  sprng_test(void);
+extern const struct ltc_prng_descriptor sprng_desc;
+#endif
+
+#ifdef LTC_SOBER128
+int sober128_start(prng_state *prng);
+int sober128_add_entropy(const unsigned char *in, unsigned long inlen, prng_state *prng);
+int sober128_ready(prng_state *prng);
+unsigned long sober128_read(unsigned char *out, unsigned long outlen, prng_state *prng);
+int sober128_done(prng_state *prng);
+int  sober128_export(unsigned char *out, unsigned long *outlen, prng_state *prng);
+int  sober128_import(const unsigned char *in, unsigned long inlen, prng_state *prng);
+int  sober128_test(void);
+extern const struct ltc_prng_descriptor sober128_desc;
+#endif
+
+int find_prng(const char *name);
+int register_prng(const struct ltc_prng_descriptor *prng);
+int unregister_prng(const struct ltc_prng_descriptor *prng);
+int prng_is_valid(int idx);
+LTC_MUTEX_PROTO(ltc_prng_mutex)
+
+/* Slow RNG you **might** be able to use to seed a PRNG with.  Be careful as this
+ * might not work on all platforms as planned
+ */
+unsigned long rng_get_bytes(unsigned char *out, 
+                            unsigned long outlen, 
+                            void (*callback)(void));
+
+int rng_make_prng(int bits, int wprng, prng_state *prng, void (*callback)(void));
+
+
+/* $Source: /cvs/libtom/libtomcrypt/src/headers/tomcrypt_prng.h,v $ */
+/* $Revision: 1.9 $ */
+/* $Date: 2007/05/12 14:32:35 $ */
diff --git a/core/lib/libtomcrypt/src/ciphers/aes.c b/core/lib/libtomcrypt/src/ciphers/aes.c
new file mode 100644
index 0000000..e5d91de
--- /dev/null
+++ b/core/lib/libtomcrypt/src/ciphers/aes.c
@@ -0,0 +1,785 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+
+/* AES implementation by Tom St Denis
+ *
+ * Derived from the Public Domain source code by
+ 
+---  
+  * rijndael-alg-fst.c
+  *
+  * @version 3.0 (December 2000)
+  *
+  * Optimised ANSI C code for the Rijndael cipher (now AES)
+  *
+  * @author Vincent Rijmen <vincent.rijmen@esat.kuleuven.ac.be>
+  * @author Antoon Bosselaers <antoon.bosselaers@esat.kuleuven.ac.be>
+  * @author Paulo Barreto <paulo.barreto@terra.com.br>
+---
+ */
+/**
+  @file aes.c
+  Implementation of AES
+*/   
+
+#include "tomcrypt.h"
+
+#ifdef LTC_RIJNDAEL
+
+#ifndef ENCRYPT_ONLY 
+
+#define SETUP    rijndael_setup
+#define ECB_ENC  rijndael_ecb_encrypt
+#define ECB_DEC  rijndael_ecb_decrypt
+#define ECB_DONE rijndael_done
+#define ECB_TEST rijndael_test
+#define ECB_KS   rijndael_keysize
+
+const struct ltc_cipher_descriptor rijndael_desc =
+{
+    "rijndael",
+    6,
+    16, 32, 16, 10,
+    SETUP, ECB_ENC, ECB_DEC, ECB_TEST, ECB_DONE, ECB_KS,
+    NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL
+};
+
+const struct ltc_cipher_descriptor aes_desc =
+{
+    "aes",
+    6,
+    16, 32, 16, 10,
+    SETUP, ECB_ENC, ECB_DEC, ECB_TEST, ECB_DONE, ECB_KS,
+    NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL
+};
+
+#else
+
+#define SETUP    rijndael_enc_setup
+#define ECB_ENC  rijndael_enc_ecb_encrypt
+#define ECB_KS   rijndael_enc_keysize
+#define ECB_DONE rijndael_enc_done
+
+const struct ltc_cipher_descriptor rijndael_enc_desc =
+{
+    "rijndael",
+    6,
+    16, 32, 16, 10,
+    SETUP, ECB_ENC, NULL, NULL, ECB_DONE, ECB_KS,
+    NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL
+};
+
+const struct ltc_cipher_descriptor aes_enc_desc =
+{
+    "aes",
+    6,
+    16, 32, 16, 10,
+    SETUP, ECB_ENC, NULL, NULL, ECB_DONE, ECB_KS,
+    NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL
+};
+
+#endif
+
+#include "aes_tab.c"
+
+static ulong32 setup_mix(ulong32 temp)
+{
+   return (Te4_3[byte(temp, 2)]) ^
+          (Te4_2[byte(temp, 1)]) ^
+          (Te4_1[byte(temp, 0)]) ^
+          (Te4_0[byte(temp, 3)]);
+}
+
+#ifndef ENCRYPT_ONLY
+#ifdef LTC_SMALL_CODE
+static ulong32 setup_mix2(ulong32 temp)
+{
+   return Td0(255 & Te4[byte(temp, 3)]) ^
+          Td1(255 & Te4[byte(temp, 2)]) ^
+          Td2(255 & Te4[byte(temp, 1)]) ^
+          Td3(255 & Te4[byte(temp, 0)]);
+}
+#endif
+#endif
+
+ /**
+    Initialize the AES (Rijndael) block cipher
+    @param key The symmetric key you wish to pass
+    @param keylen The key length in bytes
+    @param num_rounds The number of rounds desired (0 for default)
+    @param skey The key in as scheduled by this function.
+    @return CRYPT_OK if successful
+ */
+int SETUP(const unsigned char *key, int keylen, int num_rounds, symmetric_key *skey)
+{
+    int i;
+    ulong32 temp, *rk;
+#ifndef ENCRYPT_ONLY
+    ulong32 *rrk;
+#endif    
+    LTC_ARGCHK(key  != NULL);
+    LTC_ARGCHK(skey != NULL);
+  
+    if (keylen != 16 && keylen != 24 && keylen != 32) {
+       return CRYPT_INVALID_KEYSIZE;
+    }
+    
+    if (num_rounds != 0 && num_rounds != (10 + ((keylen/8)-2)*2)) {
+       return CRYPT_INVALID_ROUNDS;
+    }
+    
+    skey->rijndael.Nr = 10 + ((keylen/8)-2)*2;
+        
+    /* setup the forward key */
+    i                 = 0;
+    rk                = skey->rijndael.eK;
+    LOAD32H(rk[0], key     );
+    LOAD32H(rk[1], key +  4);
+    LOAD32H(rk[2], key +  8);
+    LOAD32H(rk[3], key + 12);
+    if (keylen == 16) {
+        for (;;) {
+            temp  = rk[3];
+            rk[4] = rk[0] ^ setup_mix(temp) ^ rcon[i];
+            rk[5] = rk[1] ^ rk[4];
+            rk[6] = rk[2] ^ rk[5];
+            rk[7] = rk[3] ^ rk[6];
+            if (++i == 10) {
+               break;
+            }
+            rk += 4;
+        }
+    } else if (keylen == 24) {
+        LOAD32H(rk[4], key + 16);
+        LOAD32H(rk[5], key + 20);
+        for (;;) {
+        #ifdef _MSC_VER
+            temp = skey->rijndael.eK[rk - skey->rijndael.eK + 5]; 
+        #else
+            temp = rk[5];
+        #endif
+            rk[ 6] = rk[ 0] ^ setup_mix(temp) ^ rcon[i];
+            rk[ 7] = rk[ 1] ^ rk[ 6];
+            rk[ 8] = rk[ 2] ^ rk[ 7];
+            rk[ 9] = rk[ 3] ^ rk[ 8];
+            if (++i == 8) {
+                break;
+            }
+            rk[10] = rk[ 4] ^ rk[ 9];
+            rk[11] = rk[ 5] ^ rk[10];
+            rk += 6;
+        }
+    } else if (keylen == 32) {
+        LOAD32H(rk[4], key + 16);
+        LOAD32H(rk[5], key + 20);
+        LOAD32H(rk[6], key + 24);
+        LOAD32H(rk[7], key + 28);
+        for (;;) {
+        #ifdef _MSC_VER
+            temp = skey->rijndael.eK[rk - skey->rijndael.eK + 7]; 
+        #else
+            temp = rk[7];
+        #endif
+            rk[ 8] = rk[ 0] ^ setup_mix(temp) ^ rcon[i];
+            rk[ 9] = rk[ 1] ^ rk[ 8];
+            rk[10] = rk[ 2] ^ rk[ 9];
+            rk[11] = rk[ 3] ^ rk[10];
+            if (++i == 7) {
+                break;
+            }
+            temp = rk[11];
+            rk[12] = rk[ 4] ^ setup_mix(RORc(temp, 8));
+            rk[13] = rk[ 5] ^ rk[12];
+            rk[14] = rk[ 6] ^ rk[13];
+            rk[15] = rk[ 7] ^ rk[14];
+            rk += 8;
+        }
+    } else {
+       /* this can't happen */
+       return CRYPT_ERROR;
+    }
+
+#ifndef ENCRYPT_ONLY    
+    /* setup the inverse key now */
+    rk   = skey->rijndael.dK;
+    rrk  = skey->rijndael.eK + (28 + keylen) - 4;
+
+    /* apply the inverse MixColumn transform to all round keys but the first and the last: */
+    /* copy first */
+    *rk++ = *rrk++;
+    *rk++ = *rrk++;
+    *rk++ = *rrk++;
+    *rk   = *rrk;
+    rk -= 3; rrk -= 3;
+    
+    for (i = 1; i < skey->rijndael.Nr; i++) {
+        rrk -= 4;
+        rk  += 4;
+    #ifdef LTC_SMALL_CODE        
+        temp = rrk[0];
+        rk[0] = setup_mix2(temp);
+        temp = rrk[1];
+        rk[1] = setup_mix2(temp);
+        temp = rrk[2];
+        rk[2] = setup_mix2(temp);
+        temp = rrk[3];
+        rk[3] = setup_mix2(temp);
+     #else
+        temp = rrk[0];
+        rk[0] =
+            Tks0[byte(temp, 3)] ^
+            Tks1[byte(temp, 2)] ^
+            Tks2[byte(temp, 1)] ^
+            Tks3[byte(temp, 0)];
+        temp = rrk[1];
+        rk[1] =
+            Tks0[byte(temp, 3)] ^
+            Tks1[byte(temp, 2)] ^
+            Tks2[byte(temp, 1)] ^
+            Tks3[byte(temp, 0)];
+        temp = rrk[2];
+        rk[2] =
+            Tks0[byte(temp, 3)] ^
+            Tks1[byte(temp, 2)] ^
+            Tks2[byte(temp, 1)] ^
+            Tks3[byte(temp, 0)];
+        temp = rrk[3];
+        rk[3] =
+            Tks0[byte(temp, 3)] ^
+            Tks1[byte(temp, 2)] ^
+            Tks2[byte(temp, 1)] ^
+            Tks3[byte(temp, 0)];
+      #endif            
+     
+    }
+
+    /* copy last */
+    rrk -= 4;
+    rk  += 4;
+    *rk++ = *rrk++;
+    *rk++ = *rrk++;
+    *rk++ = *rrk++;
+    *rk   = *rrk;
+#endif /* ENCRYPT_ONLY */
+
+    return CRYPT_OK;   
+}
+
+/**
+  Encrypts a block of text with AES
+  @param pt The input plaintext (16 bytes)
+  @param ct The output ciphertext (16 bytes)
+  @param skey The key as scheduled
+  @return CRYPT_OK if successful
+*/
+#ifdef LTC_CLEAN_STACK
+static int _rijndael_ecb_encrypt(const unsigned char *pt, unsigned char *ct, symmetric_key *skey) 
+#else
+int ECB_ENC(const unsigned char *pt, unsigned char *ct, symmetric_key *skey)
+#endif
+{
+    ulong32 s0, s1, s2, s3, t0, t1, t2, t3, *rk;
+    int Nr, r;
+   
+    LTC_ARGCHK(pt != NULL);
+    LTC_ARGCHK(ct != NULL);
+    LTC_ARGCHK(skey != NULL);
+    
+    Nr = skey->rijndael.Nr;
+    rk = skey->rijndael.eK;
+    
+    /*
+     * map byte array block to cipher state
+     * and add initial round key:
+     */
+    LOAD32H(s0, pt      ); s0 ^= rk[0];
+    LOAD32H(s1, pt  +  4); s1 ^= rk[1];
+    LOAD32H(s2, pt  +  8); s2 ^= rk[2];
+    LOAD32H(s3, pt  + 12); s3 ^= rk[3];
+
+#ifdef LTC_SMALL_CODE
+
+    for (r = 0; ; r++) {
+        rk += 4;
+        t0 =
+            Te0(byte(s0, 3)) ^
+            Te1(byte(s1, 2)) ^
+            Te2(byte(s2, 1)) ^
+            Te3(byte(s3, 0)) ^
+            rk[0];
+        t1 =
+            Te0(byte(s1, 3)) ^
+            Te1(byte(s2, 2)) ^
+            Te2(byte(s3, 1)) ^
+            Te3(byte(s0, 0)) ^
+            rk[1];
+        t2 =
+            Te0(byte(s2, 3)) ^
+            Te1(byte(s3, 2)) ^
+            Te2(byte(s0, 1)) ^
+            Te3(byte(s1, 0)) ^
+            rk[2];
+        t3 =
+            Te0(byte(s3, 3)) ^
+            Te1(byte(s0, 2)) ^
+            Te2(byte(s1, 1)) ^
+            Te3(byte(s2, 0)) ^
+            rk[3];
+        if (r == Nr-2) { 
+           break;
+        }
+        s0 = t0; s1 = t1; s2 = t2; s3 = t3;
+    }
+    rk += 4;
+
+#else
+
+    /*
+     * Nr - 1 full rounds:
+     */
+    r = Nr >> 1;
+    for (;;) {
+        t0 =
+            Te0(byte(s0, 3)) ^
+            Te1(byte(s1, 2)) ^
+            Te2(byte(s2, 1)) ^
+            Te3(byte(s3, 0)) ^
+            rk[4];
+        t1 =
+            Te0(byte(s1, 3)) ^
+            Te1(byte(s2, 2)) ^
+            Te2(byte(s3, 1)) ^
+            Te3(byte(s0, 0)) ^
+            rk[5];
+        t2 =
+            Te0(byte(s2, 3)) ^
+            Te1(byte(s3, 2)) ^
+            Te2(byte(s0, 1)) ^
+            Te3(byte(s1, 0)) ^
+            rk[6];
+        t3 =
+            Te0(byte(s3, 3)) ^
+            Te1(byte(s0, 2)) ^
+            Te2(byte(s1, 1)) ^
+            Te3(byte(s2, 0)) ^
+            rk[7];
+
+        rk += 8;
+        if (--r == 0) {
+            break;
+        }
+
+        s0 =
+            Te0(byte(t0, 3)) ^
+            Te1(byte(t1, 2)) ^
+            Te2(byte(t2, 1)) ^
+            Te3(byte(t3, 0)) ^
+            rk[0];
+        s1 =
+            Te0(byte(t1, 3)) ^
+            Te1(byte(t2, 2)) ^
+            Te2(byte(t3, 1)) ^
+            Te3(byte(t0, 0)) ^
+            rk[1];
+        s2 =
+            Te0(byte(t2, 3)) ^
+            Te1(byte(t3, 2)) ^
+            Te2(byte(t0, 1)) ^
+            Te3(byte(t1, 0)) ^
+            rk[2];
+        s3 =
+            Te0(byte(t3, 3)) ^
+            Te1(byte(t0, 2)) ^
+            Te2(byte(t1, 1)) ^
+            Te3(byte(t2, 0)) ^
+            rk[3];
+    }
+
+#endif
+
+    /*
+     * apply last round and
+     * map cipher state to byte array block:
+     */
+    s0 =
+        (Te4_3[byte(t0, 3)]) ^
+        (Te4_2[byte(t1, 2)]) ^
+        (Te4_1[byte(t2, 1)]) ^
+        (Te4_0[byte(t3, 0)]) ^
+        rk[0];
+    STORE32H(s0, ct);
+    s1 =
+        (Te4_3[byte(t1, 3)]) ^
+        (Te4_2[byte(t2, 2)]) ^
+        (Te4_1[byte(t3, 1)]) ^
+        (Te4_0[byte(t0, 0)]) ^
+        rk[1];
+    STORE32H(s1, ct+4);
+    s2 =
+        (Te4_3[byte(t2, 3)]) ^
+        (Te4_2[byte(t3, 2)]) ^
+        (Te4_1[byte(t0, 1)]) ^
+        (Te4_0[byte(t1, 0)]) ^
+        rk[2];
+    STORE32H(s2, ct+8);
+    s3 =
+        (Te4_3[byte(t3, 3)]) ^
+        (Te4_2[byte(t0, 2)]) ^
+        (Te4_1[byte(t1, 1)]) ^
+        (Te4_0[byte(t2, 0)]) ^ 
+        rk[3];
+    STORE32H(s3, ct+12);
+
+    return CRYPT_OK;
+}
+
+#ifdef LTC_CLEAN_STACK
+int ECB_ENC(const unsigned char *pt, unsigned char *ct, symmetric_key *skey) 
+{
+   int err = _rijndael_ecb_encrypt(pt, ct, skey);
+   burn_stack(sizeof(unsigned long)*8 + sizeof(unsigned long*) + sizeof(int)*2);
+   return err;
+}
+#endif
+
+#ifndef ENCRYPT_ONLY 
+
+/**
+  Decrypts a block of text with AES
+  @param ct The input ciphertext (16 bytes)
+  @param pt The output plaintext (16 bytes)
+  @param skey The key as scheduled 
+  @return CRYPT_OK if successful
+*/
+#ifdef LTC_CLEAN_STACK
+static int _rijndael_ecb_decrypt(const unsigned char *ct, unsigned char *pt, symmetric_key *skey) 
+#else
+int ECB_DEC(const unsigned char *ct, unsigned char *pt, symmetric_key *skey)
+#endif
+{
+    ulong32 s0, s1, s2, s3, t0, t1, t2, t3, *rk;
+    int Nr, r;
+
+    LTC_ARGCHK(pt != NULL);
+    LTC_ARGCHK(ct != NULL);
+    LTC_ARGCHK(skey != NULL);
+    
+    Nr = skey->rijndael.Nr;
+    rk = skey->rijndael.dK;
+
+    /*
+     * map byte array block to cipher state
+     * and add initial round key:
+     */
+    LOAD32H(s0, ct      ); s0 ^= rk[0];
+    LOAD32H(s1, ct  +  4); s1 ^= rk[1];
+    LOAD32H(s2, ct  +  8); s2 ^= rk[2];
+    LOAD32H(s3, ct  + 12); s3 ^= rk[3];
+
+#ifdef LTC_SMALL_CODE
+    for (r = 0; ; r++) {
+        rk += 4;
+        t0 =
+            Td0(byte(s0, 3)) ^
+            Td1(byte(s3, 2)) ^
+            Td2(byte(s2, 1)) ^
+            Td3(byte(s1, 0)) ^
+            rk[0];
+        t1 =
+            Td0(byte(s1, 3)) ^
+            Td1(byte(s0, 2)) ^
+            Td2(byte(s3, 1)) ^
+            Td3(byte(s2, 0)) ^
+            rk[1];
+        t2 =
+            Td0(byte(s2, 3)) ^
+            Td1(byte(s1, 2)) ^
+            Td2(byte(s0, 1)) ^
+            Td3(byte(s3, 0)) ^
+            rk[2];
+        t3 =
+            Td0(byte(s3, 3)) ^
+            Td1(byte(s2, 2)) ^
+            Td2(byte(s1, 1)) ^
+            Td3(byte(s0, 0)) ^
+            rk[3];
+        if (r == Nr-2) {
+           break; 
+        }
+        s0 = t0; s1 = t1; s2 = t2; s3 = t3;
+    }
+    rk += 4;
+
+#else       
+
+    /*
+     * Nr - 1 full rounds:
+     */
+    r = Nr >> 1;
+    for (;;) {
+
+        t0 =
+            Td0(byte(s0, 3)) ^
+            Td1(byte(s3, 2)) ^
+            Td2(byte(s2, 1)) ^
+            Td3(byte(s1, 0)) ^
+            rk[4];
+        t1 =
+            Td0(byte(s1, 3)) ^
+            Td1(byte(s0, 2)) ^
+            Td2(byte(s3, 1)) ^
+            Td3(byte(s2, 0)) ^
+            rk[5];
+        t2 =
+            Td0(byte(s2, 3)) ^
+            Td1(byte(s1, 2)) ^
+            Td2(byte(s0, 1)) ^
+            Td3(byte(s3, 0)) ^
+            rk[6];
+        t3 =
+            Td0(byte(s3, 3)) ^
+            Td1(byte(s2, 2)) ^
+            Td2(byte(s1, 1)) ^
+            Td3(byte(s0, 0)) ^
+            rk[7];
+
+        rk += 8;
+        if (--r == 0) {
+            break;
+        }
+
+
+        s0 =
+            Td0(byte(t0, 3)) ^
+            Td1(byte(t3, 2)) ^
+            Td2(byte(t2, 1)) ^
+            Td3(byte(t1, 0)) ^
+            rk[0];
+        s1 =
+            Td0(byte(t1, 3)) ^
+            Td1(byte(t0, 2)) ^
+            Td2(byte(t3, 1)) ^
+            Td3(byte(t2, 0)) ^
+            rk[1];
+        s2 =
+            Td0(byte(t2, 3)) ^
+            Td1(byte(t1, 2)) ^
+            Td2(byte(t0, 1)) ^
+            Td3(byte(t3, 0)) ^
+            rk[2];
+        s3 =
+            Td0(byte(t3, 3)) ^
+            Td1(byte(t2, 2)) ^
+            Td2(byte(t1, 1)) ^
+            Td3(byte(t0, 0)) ^
+            rk[3];
+    }
+#endif
+
+    /*
+     * apply last round and
+     * map cipher state to byte array block:
+     */
+    s0 =
+        (Td4[byte(t0, 3)] & 0xff000000) ^
+        (Td4[byte(t3, 2)] & 0x00ff0000) ^
+        (Td4[byte(t2, 1)] & 0x0000ff00) ^
+        (Td4[byte(t1, 0)] & 0x000000ff) ^
+        rk[0];
+    STORE32H(s0, pt);
+    s1 =
+        (Td4[byte(t1, 3)] & 0xff000000) ^
+        (Td4[byte(t0, 2)] & 0x00ff0000) ^
+        (Td4[byte(t3, 1)] & 0x0000ff00) ^
+        (Td4[byte(t2, 0)] & 0x000000ff) ^
+        rk[1];
+    STORE32H(s1, pt+4);
+    s2 =
+        (Td4[byte(t2, 3)] & 0xff000000) ^
+        (Td4[byte(t1, 2)] & 0x00ff0000) ^
+        (Td4[byte(t0, 1)] & 0x0000ff00) ^
+        (Td4[byte(t3, 0)] & 0x000000ff) ^
+        rk[2];
+    STORE32H(s2, pt+8);
+    s3 =
+        (Td4[byte(t3, 3)] & 0xff000000) ^
+        (Td4[byte(t2, 2)] & 0x00ff0000) ^
+        (Td4[byte(t1, 1)] & 0x0000ff00) ^
+        (Td4[byte(t0, 0)] & 0x000000ff) ^
+        rk[3];
+    STORE32H(s3, pt+12);
+
+    return CRYPT_OK;
+}
+
+
+#ifdef LTC_CLEAN_STACK
+int ECB_DEC(const unsigned char *ct, unsigned char *pt, symmetric_key *skey) 
+{
+   int err = _rijndael_ecb_decrypt(ct, pt, skey);
+   burn_stack(sizeof(unsigned long)*8 + sizeof(unsigned long*) + sizeof(int)*2);
+   return err;
+}
+#endif
+
+/**
+  Performs a self-test of the AES block cipher
+  @return CRYPT_OK if functional, CRYPT_NOP if self-test has been disabled
+*/
+int ECB_TEST(void)
+{
+ #ifndef LTC_TEST
+    return CRYPT_NOP;
+ #else    
+ int err;
+ static const struct {
+     int keylen;
+     unsigned char key[32], pt[16], ct[16];
+ } tests[] = {
+    { 16,
+      { 0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 
+        0x08, 0x09, 0x0a, 0x0b, 0x0c, 0x0d, 0x0e, 0x0f }, 
+      { 0x00, 0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77,
+        0x88, 0x99, 0xaa, 0xbb, 0xcc, 0xdd, 0xee, 0xff },
+      { 0x69, 0xc4, 0xe0, 0xd8, 0x6a, 0x7b, 0x04, 0x30, 
+        0xd8, 0xcd, 0xb7, 0x80, 0x70, 0xb4, 0xc5, 0x5a }
+    }, { 
+      24,
+      { 0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 
+        0x08, 0x09, 0x0a, 0x0b, 0x0c, 0x0d, 0x0e, 0x0f,
+        0x10, 0x11, 0x12, 0x13, 0x14, 0x15, 0x16, 0x17 },
+      { 0x00, 0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77,
+        0x88, 0x99, 0xaa, 0xbb, 0xcc, 0xdd, 0xee, 0xff },
+      { 0xdd, 0xa9, 0x7c, 0xa4, 0x86, 0x4c, 0xdf, 0xe0, 
+        0x6e, 0xaf, 0x70, 0xa0, 0xec, 0x0d, 0x71, 0x91 }
+    }, {
+      32,
+      { 0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 
+        0x08, 0x09, 0x0a, 0x0b, 0x0c, 0x0d, 0x0e, 0x0f,
+        0x10, 0x11, 0x12, 0x13, 0x14, 0x15, 0x16, 0x17, 
+        0x18, 0x19, 0x1a, 0x1b, 0x1c, 0x1d, 0x1e, 0x1f },
+      { 0x00, 0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77,
+        0x88, 0x99, 0xaa, 0xbb, 0xcc, 0xdd, 0xee, 0xff },
+      { 0x8e, 0xa2, 0xb7, 0xca, 0x51, 0x67, 0x45, 0xbf, 
+        0xea, 0xfc, 0x49, 0x90, 0x4b, 0x49, 0x60, 0x89 }
+    }
+ };
+ 
+ symmetric_key key;
+ unsigned char tmp[2][16];
+ int i, y;
+ 
+ for (i = 0; i < (int)(sizeof(tests)/sizeof(tests[0])); i++) {
+    zeromem(&key, sizeof(key));
+    if ((err = rijndael_setup(tests[i].key, tests[i].keylen, 0, &key)) != CRYPT_OK) { 
+       return err;
+    }
+  
+    rijndael_ecb_encrypt(tests[i].pt, tmp[0], &key);
+    rijndael_ecb_decrypt(tmp[0], tmp[1], &key);
+    if (XMEMCMP(tmp[0], tests[i].ct, 16) || XMEMCMP(tmp[1], tests[i].pt, 16)) { 
+#if 0
+       printf("\n\nTest %d failed\n", i);
+       if (XMEMCMP(tmp[0], tests[i].ct, 16)) {
+          printf("CT: ");
+          for (i = 0; i < 16; i++) {
+             printf("%02x ", tmp[0][i]);
+          }
+          printf("\n");
+       } else {
+          printf("PT: ");
+          for (i = 0; i < 16; i++) {
+             printf("%02x ", tmp[1][i]);
+          }
+          printf("\n");
+       }
+#endif       
+        return CRYPT_FAIL_TESTVECTOR;
+    }
+
+      /* now see if we can encrypt all zero bytes 1000 times, decrypt and come back where we started */
+      for (y = 0; y < 16; y++) tmp[0][y] = 0;
+      for (y = 0; y < 1000; y++) rijndael_ecb_encrypt(tmp[0], tmp[0], &key);
+      for (y = 0; y < 1000; y++) rijndael_ecb_decrypt(tmp[0], tmp[0], &key);
+      for (y = 0; y < 16; y++) if (tmp[0][y] != 0) return CRYPT_FAIL_TESTVECTOR;
+ }       
+ return CRYPT_OK;
+ #endif
+}
+
+#endif /* ENCRYPT_ONLY */
+
+
+/** Terminate the context 
+   @param skey    The scheduled key
+*/
+void ECB_DONE(symmetric_key *skey)
+{
+  LTC_UNUSED_PARAM(skey);
+}
+
+
+/**
+  Gets suitable key size
+  @param keysize [in/out] The length of the recommended key (in bytes).  This function will store the suitable size back in this variable.
+  @return CRYPT_OK if the input key size is acceptable.
+*/
+int ECB_KS(int *keysize)
+{
+   LTC_ARGCHK(keysize != NULL);
+
+   if (*keysize < 16)
+      return CRYPT_INVALID_KEYSIZE;
+   if (*keysize < 24) {
+      *keysize = 16;
+      return CRYPT_OK;
+   } else if (*keysize < 32) {
+      *keysize = 24;
+      return CRYPT_OK;
+   } else {
+      *keysize = 32;
+      return CRYPT_OK;
+   }
+}
+
+#endif
+
+
+/* $Source: /cvs/libtom/libtomcrypt/src/ciphers/aes/aes.c,v $ */
+/* $Revision: 1.16 $ */
+/* $Date: 2007/05/12 14:13:00 $ */
diff --git a/core/lib/libtomcrypt/src/ciphers/aes_armv8a_ce.c b/core/lib/libtomcrypt/src/ciphers/aes_armv8a_ce.c
new file mode 100644
index 0000000..dc2a6f0
--- /dev/null
+++ b/core/lib/libtomcrypt/src/ciphers/aes_armv8a_ce.c
@@ -0,0 +1,393 @@
+/*
+ * Copyright (c) 2015, Linaro Limited
+ * All rights reserved.
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+
+/*
+ * AES cipher for ARMv8 with Crypto Extensions
+ *
+ * Copyright (C) 2013 Linaro Ltd <ard.biesheuvel@linaro.org>
+ */
+
+#include "tomcrypt.h"
+#include "tomcrypt_arm_neon.h"
+
+typedef unsigned int u32;
+typedef unsigned char u8;
+
+/* Prototypes for assembly functions */
+uint32_t ce_aes_sub(uint32_t in);
+void ce_aes_invert(void *dst, void *src);
+void ce_aes_ecb_encrypt(u8 out[], u8 const in[], u8 const rk[], int rounds,
+			int blocks, int first);
+void ce_aes_ecb_decrypt(u8 out[], u8 const in[], u8 const rk[], int rounds,
+			int blocks, int first);
+void ce_aes_cbc_encrypt(u8 out[], u8 const in[], u8 const rk[], int rounds,
+			int blocks, u8 iv[], int first);
+void ce_aes_cbc_decrypt(u8 out[], u8 const in[], u8 const rk[], int rounds,
+			int blocks, u8 iv[], int first);
+void ce_aes_ctr_encrypt(u8 out[], u8 const in[], u8 const rk[], int rounds,
+			int blocks, u8 ctr[], int first);
+void ce_aes_xts_encrypt(u8 out[], u8 const in[], u8 const rk1[], int rounds,
+			int blocks, u8 const rk2[], u8 iv[]);
+void ce_aes_xts_decrypt(u8 out[], u8 const in[], u8 const rk1[], int rounds,
+			int blocks, u8 const rk2[], u8 iv[]);
+
+
+struct aes_block {
+	u8 b[16];
+};
+
+static inline u32 ror32(u32 val, u32 shift)
+{
+	return (val >> shift) | (val << (32 - shift));
+}
+
+int rijndael_setup(const unsigned char *key, int keylen, int num_rounds,
+	      symmetric_key *skey)
+{
+	/* The AES key schedule round constants */
+	static u8 const rcon[] = {
+		0x01, 0x02, 0x04, 0x08, 0x10, 0x20, 0x40, 0x80, 0x1b, 0x36,
+	};
+	u32 kwords = keylen / sizeof(u32);
+	struct aes_block *key_enc, *key_dec;
+	struct tomcrypt_arm_neon_state state;
+	unsigned int i, j;
+	void *p;
+
+	LTC_ARGCHK(key);
+	LTC_ARGCHK(skey);
+
+	if (keylen != 16 && keylen != 24 && keylen != 32)
+		return CRYPT_INVALID_KEYSIZE;
+
+	if (num_rounds != 0 && num_rounds != (10 + ((keylen/8)-2)*2))
+		return CRYPT_INVALID_ROUNDS;
+
+	num_rounds = 10 + ((keylen/8)-2)*2;
+	skey->rijndael.Nr = num_rounds;
+
+	memcpy(skey->rijndael.eK, key, keylen);
+
+	tomcrypt_arm_neon_enable(&state);
+
+	for (i = 0; i < sizeof(rcon); i++) {
+		u32 *rki;
+		u32 *rko;
+
+		p = skey->rijndael.eK;
+		rki = (u32 *)p + (i * kwords);
+		rko = rki + kwords;
+
+		rko[0] = ror32(ce_aes_sub(rki[kwords - 1]), 8)
+				^ rcon[i] ^ rki[0];
+		rko[1] = rko[0] ^ rki[1];
+		rko[2] = rko[1] ^ rki[2];
+		rko[3] = rko[2] ^ rki[3];
+
+		if (keylen == 24) {
+			if (i >= 7)
+				break;
+			rko[4] = rko[3] ^ rki[4];
+			rko[5] = rko[4] ^ rki[5];
+		} else if (keylen == 32) {
+			if (i >= 6)
+				break;
+			rko[4] = ce_aes_sub(rko[3]) ^ rki[4];
+			rko[5] = rko[4] ^ rki[5];
+			rko[6] = rko[5] ^ rki[6];
+			rko[7] = rko[6] ^ rki[7];
+		}
+	}
+
+	/*
+	 * Generate the decryption keys for the Equivalent Inverse Cipher.
+	 * This involves reversing the order of the round keys, and applying
+	 * the Inverse Mix Columns transformation on all but the first and
+	 * the last one.
+	 */
+	p = skey->rijndael.eK;
+	key_enc = (struct aes_block *)p;
+	p = skey->rijndael.dK;
+	key_dec = (struct aes_block *)p;
+	j = num_rounds;
+
+	key_dec[0] = key_enc[j];
+	for (i = 1, j--; j > 0; i++, j--)
+		ce_aes_invert(key_dec + i, key_enc + j);
+	key_dec[i] = key_enc[0];
+
+	tomcrypt_arm_neon_disable(&state);
+
+	return CRYPT_OK;
+}
+
+void rijndael_done(symmetric_key *skey)
+{
+}
+
+int rijndael_keysize(int *keysize)
+{
+	LTC_ARGCHK(keysize);
+
+	if (*keysize < 16)
+		return CRYPT_INVALID_KEYSIZE;
+	else if (*keysize < 24)
+		*keysize = 16;
+	else if (*keysize < 32)
+		*keysize = 24;
+	else
+		*keysize = 32;
+
+	return CRYPT_OK;
+}
+
+static int aes_ecb_encrypt_nblocks(const unsigned char *pt, unsigned char *ct,
+				   unsigned long blocks, symmetric_key *skey)
+{
+	struct tomcrypt_arm_neon_state state;
+	u8 *rk;
+	int Nr;
+
+	LTC_ARGCHK(pt);
+	LTC_ARGCHK(ct);
+	LTC_ARGCHK(skey);
+
+	Nr = skey->rijndael.Nr;
+	rk = (u8 *)skey->rijndael.eK;
+
+	tomcrypt_arm_neon_enable(&state);
+	ce_aes_ecb_encrypt(ct, pt, rk, Nr, blocks, 1);
+	tomcrypt_arm_neon_disable(&state);
+
+	return CRYPT_OK;
+}
+
+static int aes_ecb_decrypt_nblocks(const unsigned char *ct, unsigned char *pt,
+				   unsigned long blocks, symmetric_key *skey)
+{
+	struct tomcrypt_arm_neon_state state;
+	u8 *rk;
+	int Nr;
+
+	LTC_ARGCHK(pt);
+	LTC_ARGCHK(ct);
+	LTC_ARGCHK(skey);
+
+	Nr = skey->rijndael.Nr;
+	rk = (u8 *)skey->rijndael.dK;
+
+	tomcrypt_arm_neon_enable(&state);
+	ce_aes_ecb_decrypt(pt, ct, rk, Nr, blocks, 1);
+	tomcrypt_arm_neon_disable(&state);
+
+	return CRYPT_OK;
+}
+
+int rijndael_ecb_encrypt(const unsigned char *pt, unsigned char *ct,
+		    symmetric_key *skey)
+{
+	return aes_ecb_encrypt_nblocks(pt, ct, 1, skey);
+}
+
+int rijndael_ecb_decrypt(const unsigned char *ct, unsigned char *pt,
+		    symmetric_key *skey)
+{
+	return aes_ecb_decrypt_nblocks(ct, pt, 1, skey);
+}
+
+static int aes_cbc_encrypt_nblocks(const unsigned char *pt, unsigned char *ct,
+				   unsigned long blocks, unsigned char *IV,
+				   symmetric_key *skey)
+{
+	struct tomcrypt_arm_neon_state state;
+	u8 *rk;
+	int Nr;
+
+	LTC_ARGCHK(pt);
+	LTC_ARGCHK(ct);
+	LTC_ARGCHK(IV);
+	LTC_ARGCHK(skey);
+
+	Nr = skey->rijndael.Nr;
+	rk = (u8 *)skey->rijndael.eK;
+
+	tomcrypt_arm_neon_enable(&state);
+	ce_aes_cbc_encrypt(ct, pt, rk, Nr, blocks, IV, 1);
+	tomcrypt_arm_neon_disable(&state);
+
+	return CRYPT_OK;
+}
+
+static int aes_cbc_decrypt_nblocks(const unsigned char *ct, unsigned char *pt,
+				   unsigned long blocks, unsigned char *IV,
+				   symmetric_key *skey)
+{
+	struct tomcrypt_arm_neon_state state;
+	u8 *rk;
+	int Nr;
+
+	LTC_ARGCHK(pt);
+	LTC_ARGCHK(ct);
+	LTC_ARGCHK(IV);
+	LTC_ARGCHK(skey);
+
+	Nr = skey->rijndael.Nr;
+	rk = (u8 *)skey->rijndael.dK;
+
+	tomcrypt_arm_neon_enable(&state);
+	ce_aes_cbc_decrypt(pt, ct, rk, Nr, blocks, IV, 1);
+	tomcrypt_arm_neon_disable(&state);
+
+	return CRYPT_OK;
+}
+
+/* Increment 128-bit counter */
+static void increment_ctr(unsigned char *val)
+{
+	int i;
+
+	for (i = 15; i >= 0; i--) {
+		val[i] = (val[i] + 1) & 0xff;
+		if (val[i])
+			break;
+	}
+}
+
+static int aes_ctr_encrypt_nblocks(const unsigned char *pt, unsigned char *ct,
+				   unsigned long blocks, unsigned char *IV,
+				   int mode, symmetric_key *skey)
+{
+	struct tomcrypt_arm_neon_state state;
+	u8 *rk;
+	int Nr;
+
+	LTC_ARGCHK(pt);
+	LTC_ARGCHK(ct);
+	LTC_ARGCHK(IV);
+	LTC_ARGCHK(skey);
+
+	if (mode == CTR_COUNTER_LITTLE_ENDIAN) {
+		/* Accelerated algorithm supports big endian only */
+		return CRYPT_ERROR;
+	}
+
+	Nr = skey->rijndael.Nr;
+	rk = (u8 *)skey->rijndael.eK;
+
+	increment_ctr(IV);
+	tomcrypt_arm_neon_enable(&state);
+	ce_aes_ctr_encrypt(ct, pt, rk, Nr, blocks, IV, 1);
+	tomcrypt_arm_neon_disable(&state);
+
+	return CRYPT_OK;
+}
+
+static int aes_xts_encrypt_nblocks(const unsigned char *pt, unsigned char *ct,
+				   unsigned long blocks, unsigned char *tweak,
+				   symmetric_key *skey1, symmetric_key *skey2)
+{
+	struct tomcrypt_arm_neon_state state;
+	u8 *rk1, *rk2;
+	int Nr;
+
+	LTC_ARGCHK(pt);
+	LTC_ARGCHK(ct);
+	LTC_ARGCHK(tweak);
+	LTC_ARGCHK(skey1);
+	LTC_ARGCHK(skey2);
+	LTC_ARGCHK(skey1->rijndael.Nr == skey2->rijndael.Nr);
+
+	Nr = skey1->rijndael.Nr;
+	rk1 = (u8 *)skey1->rijndael.eK;
+	rk2 = (u8 *)skey2->rijndael.eK;
+
+	tomcrypt_arm_neon_enable(&state);
+	ce_aes_xts_encrypt(ct, pt, rk1, Nr, blocks, rk2, tweak);
+	tomcrypt_arm_neon_disable(&state);
+
+	return CRYPT_OK;
+}
+
+static int aes_xts_decrypt_nblocks(const unsigned char *ct, unsigned char *pt,
+				   unsigned long blocks, unsigned char *tweak,
+				   symmetric_key *skey1, symmetric_key *skey2)
+{
+	struct tomcrypt_arm_neon_state state;
+	u8 *rk1, *rk2;
+	int Nr;
+
+	LTC_ARGCHK(pt);
+	LTC_ARGCHK(ct);
+	LTC_ARGCHK(tweak);
+	LTC_ARGCHK(skey1);
+	LTC_ARGCHK(skey2);
+	LTC_ARGCHK(skey1->rijndael.Nr == skey2->rijndael.Nr);
+
+	Nr = skey1->rijndael.Nr;
+	rk1 = (u8 *)skey1->rijndael.dK;
+	rk2 = (u8 *)skey2->rijndael.eK;
+
+	tomcrypt_arm_neon_enable(&state);
+	ce_aes_xts_decrypt(pt, ct, rk1, Nr, blocks, rk2, tweak);
+	tomcrypt_arm_neon_disable(&state);
+
+	return CRYPT_OK;
+}
+
+const struct ltc_cipher_descriptor aes_desc = {
+	.name = "aes",
+	.ID = 6,
+	.min_key_length = 16,
+	.max_key_length = 32,
+	.block_length = 16,
+	.default_rounds = 10,
+	.setup = rijndael_setup,
+	.ecb_encrypt = rijndael_ecb_encrypt,
+	.ecb_decrypt = rijndael_ecb_decrypt,
+	.done = rijndael_done,
+	.keysize = rijndael_keysize,
+	.accel_ecb_encrypt = aes_ecb_encrypt_nblocks,
+	.accel_ecb_decrypt = aes_ecb_decrypt_nblocks,
+	.accel_cbc_encrypt = aes_cbc_encrypt_nblocks,
+	.accel_cbc_decrypt = aes_cbc_decrypt_nblocks,
+	.accel_ctr_encrypt = aes_ctr_encrypt_nblocks,
+	.accel_xts_encrypt = aes_xts_encrypt_nblocks,
+	.accel_xts_decrypt = aes_xts_decrypt_nblocks,
+};
diff --git a/core/lib/libtomcrypt/src/ciphers/aes_modes_armv8a_ce_a32.S b/core/lib/libtomcrypt/src/ciphers/aes_modes_armv8a_ce_a32.S
new file mode 100644
index 0000000..f4c12ea
--- /dev/null
+++ b/core/lib/libtomcrypt/src/ciphers/aes_modes_armv8a_ce_a32.S
@@ -0,0 +1,548 @@
+/*
+ * Copyright (c) 2016, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/*
+ * aes-ce-core.S - AES in CBC/CTR/XTS mode using ARMv8 Crypto Extensions
+ *
+ * Copyright (C) 2015 Linaro Ltd <ard.biesheuvel@linaro.org>
+ */
+
+	.text
+	.fpu		crypto-neon-fp-armv8
+	.align		3
+
+	.macro		enc_round, state, key
+	aese.8		\state, \key
+	aesmc.8		\state, \state
+	.endm
+
+	.macro		dec_round, state, key
+	aesd.8		\state, \key
+	aesimc.8	\state, \state
+	.endm
+
+	.macro		enc_dround, key1, key2
+	enc_round	q0, \key1
+	enc_round	q0, \key2
+	.endm
+
+	.macro		dec_dround, key1, key2
+	dec_round	q0, \key1
+	dec_round	q0, \key2
+	.endm
+
+	.macro		enc_fround, key1, key2, key3
+	enc_round	q0, \key1
+	aese.8		q0, \key2
+	veor		q0, q0, \key3
+	.endm
+
+	.macro		dec_fround, key1, key2, key3
+	dec_round	q0, \key1
+	aesd.8		q0, \key2
+	veor		q0, q0, \key3
+	.endm
+
+	.macro		enc_dround_3x, key1, key2
+	enc_round	q0, \key1
+	enc_round	q1, \key1
+	enc_round	q2, \key1
+	enc_round	q0, \key2
+	enc_round	q1, \key2
+	enc_round	q2, \key2
+	.endm
+
+	.macro		dec_dround_3x, key1, key2
+	dec_round	q0, \key1
+	dec_round	q1, \key1
+	dec_round	q2, \key1
+	dec_round	q0, \key2
+	dec_round	q1, \key2
+	dec_round	q2, \key2
+	.endm
+
+	.macro		enc_fround_3x, key1, key2, key3
+	enc_round	q0, \key1
+	enc_round	q1, \key1
+	enc_round	q2, \key1
+	aese.8		q0, \key2
+	aese.8		q1, \key2
+	aese.8		q2, \key2
+	veor		q0, q0, \key3
+	veor		q1, q1, \key3
+	veor		q2, q2, \key3
+	.endm
+
+	.macro		dec_fround_3x, key1, key2, key3
+	dec_round	q0, \key1
+	dec_round	q1, \key1
+	dec_round	q2, \key1
+	aesd.8		q0, \key2
+	aesd.8		q1, \key2
+	aesd.8		q2, \key2
+	veor		q0, q0, \key3
+	veor		q1, q1, \key3
+	veor		q2, q2, \key3
+	.endm
+
+	.macro		do_block, dround, fround
+	cmp		r3, #12			@ which key size?
+	vld1.8		{q10-q11}, [ip]!
+	\dround		q8, q9
+	vld1.8		{q12-q13}, [ip]!
+	\dround		q10, q11
+	vld1.8		{q10-q11}, [ip]!
+	\dround		q12, q13
+	vld1.8		{q12-q13}, [ip]!
+	\dround		q10, q11
+	blo		0f			@ AES-128: 10 rounds
+	vld1.8		{q10-q11}, [ip]!
+	\dround		q12, q13
+	beq		1f			@ AES-192: 12 rounds
+	vld1.8		{q12-q13}, [ip]
+	\dround		q10, q11
+0:	\fround		q12, q13, q14
+	bx		lr
+
+1:	\fround		q10, q11, q14
+	bx		lr
+	.endm
+
+	/*
+	 * Internal, non-AAPCS compliant functions that implement the core AES
+	 * transforms. These should preserve all registers except q0 - q2 and ip
+	 * Arguments:
+	 *   q0        : first in/output block
+	 *   q1        : second in/output block (_3x version only)
+	 *   q2        : third in/output block (_3x version only)
+	 *   q8        : first round key
+	 *   q9        : secound round key
+	 *   q14       : final round key
+	 *   r2        : address of round key array
+	 *   r3        : number of rounds
+	 */
+	.align		6
+aes_encrypt:
+	add		ip, r2, #32		@ 3rd round key
+.Laes_encrypt_tweak:
+	do_block	enc_dround, enc_fround
+
+	.align		6
+aes_decrypt:
+	add		ip, r2, #32		@ 3rd round key
+	do_block	dec_dround, dec_fround
+
+	.align		6
+aes_encrypt_3x:
+	add		ip, r2, #32		@ 3rd round key
+	do_block	enc_dround_3x, enc_fround_3x
+
+	.align		6
+aes_decrypt_3x:
+	add		ip, r2, #32		@ 3rd round key
+	do_block	dec_dround_3x, dec_fround_3x
+
+	.macro		prepare_key, rk, rounds
+	add		ip, \rk, \rounds, lsl #4
+	vld1.8		{q8-q9}, [\rk]		@ load first 2 round keys
+	vld1.8		{q14}, [ip]		@ load last round key
+	.endm
+
+	/*
+	 * aes_ecb_encrypt(u8 out[], u8 const in[], u8 const rk[], int rounds,
+	 *		   int blocks)
+	 * aes_ecb_decrypt(u8 out[], u8 const in[], u8 const rk[], int rounds,
+	 *		   int blocks)
+	 */
+	.global		ce_aes_ecb_encrypt
+	.type		ce_aes_ecb_encrypt, %function
+ce_aes_ecb_encrypt:
+	push		{r4, lr}
+	ldr		r4, [sp, #8]
+	prepare_key	r2, r3
+.Lecbencloop3x:
+	subs		r4, r4, #3
+	bmi		.Lecbenc1x
+	vld1.8		{q0-q1}, [r1]!
+	vld1.8		{q2}, [r1]!
+	bl		aes_encrypt_3x
+	vst1.8		{q0-q1}, [r0]!
+	vst1.8		{q2}, [r0]!
+	b		.Lecbencloop3x
+.Lecbenc1x:
+	adds		r4, r4, #3
+	beq		.Lecbencout
+.Lecbencloop:
+	vld1.8		{q0}, [r1]!
+	bl		aes_encrypt
+	vst1.8		{q0}, [r0]!
+	subs		r4, r4, #1
+	bne		.Lecbencloop
+.Lecbencout:
+	pop		{r4, pc}
+
+	.global		ce_aes_ecb_decrypt
+	.type		ce_aes_ecb_decrypt, %function
+ce_aes_ecb_decrypt:
+	push		{r4, lr}
+	ldr		r4, [sp, #8]
+	prepare_key	r2, r3
+.Lecbdecloop3x:
+	subs		r4, r4, #3
+	bmi		.Lecbdec1x
+	vld1.8		{q0-q1}, [r1]!
+	vld1.8		{q2}, [r1]!
+	bl		aes_decrypt_3x
+	vst1.8		{q0-q1}, [r0]!
+	vst1.8		{q2}, [r0]!
+	b		.Lecbdecloop3x
+.Lecbdec1x:
+	adds		r4, r4, #3
+	beq		.Lecbdecout
+.Lecbdecloop:
+	vld1.8		{q0}, [r1]!
+	bl		aes_decrypt
+	vst1.8		{q0}, [r0]!
+	subs		r4, r4, #1
+	bne		.Lecbdecloop
+.Lecbdecout:
+	pop		{r4, pc}
+
+	/*
+	 * aes_cbc_encrypt(u8 out[], u8 const in[], u8 const rk[], int rounds,
+	 *		   int blocks, u8 iv[])
+	 * aes_cbc_decrypt(u8 out[], u8 const in[], u8 const rk[], int rounds,
+	 *		   int blocks, u8 iv[])
+	 */
+	.global		ce_aes_cbc_encrypt
+	.type		ce_aes_cbc_encrypt, %function
+ce_aes_cbc_encrypt:
+	push		{r4-r6, lr}
+	ldrd		r4, r5, [sp, #16]
+	vld1.8		{q0}, [r5]
+	prepare_key	r2, r3
+.Lcbcencloop:
+	vld1.8		{q1}, [r1]!		@ get next pt block
+	veor		q0, q0, q1		@ ..and xor with iv
+	bl		aes_encrypt
+	vst1.8		{q0}, [r0]!
+	subs		r4, r4, #1
+	bne		.Lcbcencloop
+	vst1.8		{q0}, [r5]
+	pop		{r4-r6, pc}
+
+	.global		ce_aes_cbc_decrypt
+	.type		ce_aes_cbc_decrypt, %function
+ce_aes_cbc_decrypt:
+	push		{r4-r6, lr}
+	ldrd		r4, r5, [sp, #16]
+	vld1.8		{q6}, [r5]		@ keep iv in q6
+	prepare_key	r2, r3
+.Lcbcdecloop3x:
+	subs		r4, r4, #3
+	bmi		.Lcbcdec1x
+	vld1.8		{q0-q1}, [r1]!
+	vld1.8		{q2}, [r1]!
+	vmov		q3, q0
+	vmov		q4, q1
+	vmov		q5, q2
+	bl		aes_decrypt_3x
+	veor		q0, q0, q6
+	veor		q1, q1, q3
+	veor		q2, q2, q4
+	vmov		q6, q5
+	vst1.8		{q0-q1}, [r0]!
+	vst1.8		{q2}, [r0]!
+	b		.Lcbcdecloop3x
+.Lcbcdec1x:
+	adds		r4, r4, #3
+	beq		.Lcbcdecout
+	vmov		q15, q14		@ preserve last round key
+.Lcbcdecloop:
+	vld1.8		{q0}, [r1]!		@ get next ct block
+	veor		q14, q15, q6		@ combine prev ct with last key
+	vmov		q6, q0
+	bl		aes_decrypt
+	vst1.8		{q0}, [r0]!
+	subs		r4, r4, #1
+	bne		.Lcbcdecloop
+.Lcbcdecout:
+	vst1.8		{q6}, [r5]		@ keep iv in q6
+	pop		{r4-r6, pc}
+
+	/*
+	 * aes_ctr_encrypt(u8 out[], u8 const in[], u8 const rk[], int rounds,
+	 *		   int blocks, u8 ctr[])
+	 */
+	/*
+	 * Note: ctr is not incremented before first block is encrypted. It is
+	 * not incremented either after the last block is done. The value used
+	 * for the last block is copied on return.
+	 */
+	.global		ce_aes_ctr_encrypt
+	.type		ce_aes_ctr_encrypt, %function
+ce_aes_ctr_encrypt:
+	push		{r4-r6, lr}
+	ldrd		r4, r5, [sp, #16]
+	vld1.8		{q6}, [r5]		@ load ctr
+	prepare_key	r2, r3
+	vmov		r6, s27			@ keep swabbed ctr in r6
+	rev		r6, r6
+	sub		r7, r4, #1		@ times ctr will be incremented
+	cmn		r6, r7			@ 32 bit overflow?
+	bcs		.Lctrloop
+.Lctrloop3x:
+	subs		r4, r4, #3
+	bmi		.Lctr1x
+	vmov		q0, q6
+	vmov		q1, q6
+	rev		ip, r6
+	add		r6, r6, #1
+	vmov		q2, q6
+	vmov		s7, ip
+	rev		ip, r6
+	add		r6, r6, #1
+	vmov		s11, ip
+	vld1.8		{q3-q4}, [r1]!
+	vld1.8		{q5}, [r1]!
+	bl		aes_encrypt_3x
+	veor		q0, q0, q3
+	veor		q1, q1, q4
+	veor		q2, q2, q5
+	rev		ip, r6
+	vst1.8		{q0-q1}, [r0]!
+	vst1.8		{q2}, [r0]!
+	vmov		s27, ip
+	b		.Lctrloop3x
+.Lctr1x:
+	adds		r4, r4, #3
+	beq		.Lctrout
+.Lctrloop:
+	vmov		q0, q6
+	bl		aes_encrypt
+	subs		r4, r4, #1
+	bmi		.Lctrhalfblock		@ blocks < 0 means 1/2 block
+	vld1.8		{q3}, [r1]!
+	veor		q3, q0, q3
+	vst1.8		{q3}, [r0]!
+	beq		.Lctrout
+
+	adds		r6, r6, #1		@ increment BE ctr
+	rev		ip, r6
+	vmov		s27, ip
+	bcs		.Lctrcarry
+	teq		r4, #0
+	bne		.Lctrloop
+.Lctrout:
+	vst1.8		{q6}, [r5]
+	pop		{r4-r6, pc}
+
+.Lctrhalfblock:
+	vld1.8		{d1}, [r1]
+	veor		d0, d0, d1
+	vst1.8		{d0}, [r0]
+	pop		{r4-r6, pc}
+
+.Lctrcarry:
+	.irp		sreg, s26, s25, s24
+	vmov		ip, \sreg		@ load next word of ctr
+	rev		ip, ip			@ ... to handle the carry
+	adds		ip, ip, #1
+	rev		ip, ip
+	vmov		\sreg, ip
+	bcc		0f
+	.endr
+0:	teq		r4, #0
+	beq		.Lctrout
+	b		.Lctrloop
+
+	/*
+	 * aes_xts_encrypt(u8 out[], u8 const in[], u8 const rk1[], int rounds,
+	 *		   int blocks, u8 iv[], u8 const rk2[])
+	 * aes_xts_decrypt(u8 out[], u8 const in[], u8 const rk1[], int rounds,
+	 *		   int blocks, u8 iv[], u8 const rk2[])
+	 */
+
+	.macro		next_tweak, out, in, const, tmp
+	vshr.s64	\tmp, \in, #63
+	vand		\tmp, \tmp, \const
+	vadd.u64	\out, \in, \in
+	vext.8		\tmp, \tmp, \tmp, #8
+	veor		\out, \out, \tmp
+	.endm
+
+	.align		3
+.Lxts_mul_x:
+	.quad		1, 0x87
+
+	.global		ce_aes_xts_init
+	.type		ce_aes_xts_init, %function
+ce_aes_xts_init:
+	vldr		d14, .Lxts_mul_x
+	vldr		d15, .Lxts_mul_x + 8
+
+	ldr		r4, [sp, #16]		@ load args
+	ldr		r5, [sp, #24]
+	vld1.8		{q0}, [r5]		@ load iv
+
+	@ Encrypt the IV in q0 with the second AES key. This should only
+	@ be done at the start of a block.
+	ldr		r6, [sp, #20]		@ load AES key 2
+	prepare_key	r6, r3
+	add		ip, r6, #32		@ 3rd round key of key 2
+	b		.Laes_encrypt_tweak	@ tail call
+
+	.global		ce_aes_xts_encrypt
+	.type		ce_aes_xts_encrypt, %function
+ce_aes_xts_encrypt:
+	push		{r4-r6, lr}
+
+	bl		ce_aes_xts_init		@ run shared prologue
+	prepare_key	r2, r3
+	vmov		q3, q0
+
+	teq		r6, #0			@ start of a block?
+	bne		.Lxtsenc3x
+
+.Lxtsencloop3x:
+	next_tweak	q3, q3, q7, q6
+.Lxtsenc3x:
+	subs		r4, r4, #3
+	bmi		.Lxtsenc1x
+	vld1.8		{q0-q1}, [r1]!		@ get 3 pt blocks
+	vld1.8		{q2}, [r1]!
+	next_tweak	q4, q3, q7, q6
+	veor		q0, q0, q3
+	next_tweak	q5, q4, q7, q6
+	veor		q1, q1, q4
+	veor		q2, q2, q5
+	bl		aes_encrypt_3x
+	veor		q0, q0, q3
+	veor		q1, q1, q4
+	veor		q2, q2, q5
+	vst1.8		{q0-q1}, [r0]!		@ write 3 ct blocks
+	vst1.8		{q2}, [r0]!
+	vmov		q3, q5
+	teq		r4, #0
+	beq		.Lxtsencout
+	b		.Lxtsencloop3x
+.Lxtsenc1x:
+	adds		r4, r4, #3
+	beq		.Lxtsencout
+.Lxtsencloop:
+	vld1.8		{q0}, [r1]!
+	veor		q0, q0, q3
+	bl		aes_encrypt
+	veor		q0, q0, q3
+	vst1.8		{q0}, [r0]!
+	subs		r4, r4, #1
+	beq		.Lxtsencout
+	next_tweak	q3, q3, q7, q6
+	b		.Lxtsencloop
+.Lxtsencout:
+	next_tweak	q3, q3, q7, q6
+	vst1.8		{q3}, [r5]
+	pop		{r4-r6, pc}
+
+	.global		ce_aes_xts_decrypt
+	.type		ce_aes_xts_decrypt, %function
+ce_aes_xts_decrypt:
+	push		{r4-r6, lr}
+
+	bl		ce_aes_xts_init		@ run shared prologue
+	prepare_key	r2, r3
+	vmov		q3, q0
+
+	teq		r6, #0			@ start of a block?
+	bne		.Lxtsdec3x
+
+.Lxtsdecloop3x:
+	next_tweak	q3, q3, q7, q6
+.Lxtsdec3x:
+	subs		r4, r4, #3
+	bmi		.Lxtsdec1x
+	vld1.8		{q0-q1}, [r1]!		@ get 3 ct blocks
+	vld1.8		{q2}, [r1]!
+	next_tweak	q4, q3, q7, q6
+	veor		q0, q0, q3
+	next_tweak	q5, q4, q7, q6
+	veor		q1, q1, q4
+	veor		q2, q2, q5
+	bl		aes_decrypt_3x
+	veor		q0, q0, q3
+	veor		q1, q1, q4
+	veor		q2, q2, q5
+	vst1.8		{q0-q1}, [r0]!		@ write 3 pt blocks
+	vst1.8		{q2}, [r0]!
+	vmov		q3, q5
+	teq		r4, #0
+	beq		.Lxtsdecout
+	b		.Lxtsdecloop3x
+.Lxtsdec1x:
+	adds		r4, r4, #3
+	beq		.Lxtsdecout
+.Lxtsdecloop:
+	vld1.8		{q0}, [r1]!
+	veor		q0, q0, q3
+	add		ip, r2, #32		@ 3rd round key
+	bl		aes_decrypt
+	veor		q0, q0, q3
+	vst1.8		{q0}, [r0]!
+	subs		r4, r4, #1
+	beq		.Lxtsdecout
+	next_tweak	q3, q3, q7, q6
+	b		.Lxtsdecloop
+.Lxtsdecout:
+	next_tweak	q3, q3, q7, q6
+	vst1.8		{q3}, [r5]
+	pop		{r4-r6, pc}
+
+	/*
+	 * u32 ce_aes_sub(u32 input) - use the aese instruction to perform the
+	 *                             AES sbox substitution on each byte in
+	 *                             'input'
+	 */
+	.global		ce_aes_sub
+	.type		ce_aes_sub, %function
+ce_aes_sub:
+	vdup.32		q1, r0
+	veor		q0, q0, q0
+	aese.8		q0, q1
+	vmov		r0, s0
+	bx		lr
+
+	/*
+	 * void ce_aes_invert(u8 *dst, u8 *src) - perform the Inverse MixColumns
+	 *                                        operation on round key *src
+	 */
+	.global		ce_aes_invert
+	.type		ce_aes_invert, %function
+ce_aes_invert:
+	vld1.8		{q0}, [r1]
+	aesimc.8	q0, q0
+	vst1.8		{q0}, [r0]
+	bx		lr
diff --git a/core/lib/libtomcrypt/src/ciphers/aes_modes_armv8a_ce_a64.S b/core/lib/libtomcrypt/src/ciphers/aes_modes_armv8a_ce_a64.S
new file mode 100644
index 0000000..bbc533e
--- /dev/null
+++ b/core/lib/libtomcrypt/src/ciphers/aes_modes_armv8a_ce_a64.S
@@ -0,0 +1,705 @@
+/*
+ * 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.
+ */
+
+/*
+ * - AES cipher for ARMv8 with Crypto Extensions
+ * - Chaining mode wrappers for AES
+ *
+ * Copyright (C) 2013 Linaro Ltd <ard.biesheuvel@linaro.org>
+ */
+
+
+#define ENTRY(func) \
+	.global func ; \
+	.type func , %function ; \
+	func :
+
+#define ENDPROC(func) \
+	.size func , .-func
+
+	.arch		armv8-a+crypto
+
+	/* preload all round keys */
+	.macro		load_round_keys, rounds, rk
+	cmp		\rounds, #12
+	blo		2222f		/* 128 bits */
+	beq		1111f		/* 192 bits */
+	ld1		{v17.16b-v18.16b}, [\rk], #32
+1111:	ld1		{v19.16b-v20.16b}, [\rk], #32
+2222:	ld1		{v21.16b-v24.16b}, [\rk], #64
+	ld1		{v25.16b-v28.16b}, [\rk], #64
+	ld1		{v29.16b-v31.16b}, [\rk]
+	.endm
+
+	/* prepare for encryption with key in rk[] */
+	.macro		enc_prepare, rounds, rk, ignore
+	load_round_keys	\rounds, \rk
+	.endm
+
+	/* prepare for encryption (again) but with new key in rk[] */
+	.macro		enc_switch_key, rounds, rk, ignore
+	load_round_keys	\rounds, \rk
+	.endm
+
+	/* prepare for decryption with key in rk[] */
+	.macro		dec_prepare, rounds, rk, ignore
+	load_round_keys	\rounds, \rk
+	.endm
+
+	.macro		do_enc_Nx, de, mc, k, i0, i1, i2, i3
+	aes\de		\i0\().16b, \k\().16b
+	aes\mc		\i0\().16b, \i0\().16b
+	.ifnb		\i1
+	aes\de		\i1\().16b, \k\().16b
+	aes\mc		\i1\().16b, \i1\().16b
+	.ifnb		\i3
+	aes\de		\i2\().16b, \k\().16b
+	aes\mc		\i2\().16b, \i2\().16b
+	aes\de		\i3\().16b, \k\().16b
+	aes\mc		\i3\().16b, \i3\().16b
+	.endif
+	.endif
+	.endm
+
+	/* up to 4 interleaved encryption rounds with the same round key */
+	.macro		round_Nx, enc, k, i0, i1, i2, i3
+	.ifc		\enc, e
+	do_enc_Nx	e, mc, \k, \i0, \i1, \i2, \i3
+	.else
+	do_enc_Nx	d, imc, \k, \i0, \i1, \i2, \i3
+	.endif
+	.endm
+
+	/* up to 4 interleaved final rounds */
+	.macro		fin_round_Nx, de, k, k2, i0, i1, i2, i3
+	aes\de		\i0\().16b, \k\().16b
+	.ifnb		\i1
+	aes\de		\i1\().16b, \k\().16b
+	.ifnb		\i3
+	aes\de		\i2\().16b, \k\().16b
+	aes\de		\i3\().16b, \k\().16b
+	.endif
+	.endif
+	eor		\i0\().16b, \i0\().16b, \k2\().16b
+	.ifnb		\i1
+	eor		\i1\().16b, \i1\().16b, \k2\().16b
+	.ifnb		\i3
+	eor		\i2\().16b, \i2\().16b, \k2\().16b
+	eor		\i3\().16b, \i3\().16b, \k2\().16b
+	.endif
+	.endif
+	.endm
+
+	/* up to 4 interleaved blocks */
+	.macro		do_block_Nx, enc, rounds, i0, i1, i2, i3
+	cmp		\rounds, #12
+	blo		2222f		/* 128 bits */
+	beq		1111f		/* 192 bits */
+	round_Nx	\enc, v17, \i0, \i1, \i2, \i3
+	round_Nx	\enc, v18, \i0, \i1, \i2, \i3
+1111:	round_Nx	\enc, v19, \i0, \i1, \i2, \i3
+	round_Nx	\enc, v20, \i0, \i1, \i2, \i3
+2222:	.irp		key, v21, v22, v23, v24, v25, v26, v27, v28, v29
+	round_Nx	\enc, \key, \i0, \i1, \i2, \i3
+	.endr
+	fin_round_Nx	\enc, v30, v31, \i0, \i1, \i2, \i3
+	.endm
+
+	.macro		encrypt_block, in, rounds, t0, t1, t2
+	do_block_Nx	e, \rounds, \in
+	.endm
+
+	.macro		encrypt_block2x, i0, i1, rounds, t0, t1, t2
+	do_block_Nx	e, \rounds, \i0, \i1
+	.endm
+
+	.macro		encrypt_block4x, i0, i1, i2, i3, rounds, t0, t1, t2
+	do_block_Nx	e, \rounds, \i0, \i1, \i2, \i3
+	.endm
+
+	.macro		decrypt_block, in, rounds, t0, t1, t2
+	do_block_Nx	d, \rounds, \in
+	.endm
+
+	.macro		decrypt_block2x, i0, i1, rounds, t0, t1, t2
+	do_block_Nx	d, \rounds, \i0, \i1
+	.endm
+
+	.macro		decrypt_block4x, i0, i1, i2, i3, rounds, t0, t1, t2
+	do_block_Nx	d, \rounds, \i0, \i1, \i2, \i3
+	.endm
+
+
+	.text
+	.align		4
+
+/*
+ * There are several ways to instantiate this code:
+ * - no interleave, all inline
+ * - 2-way interleave, 2x calls out of line (-DINTERLEAVE=2)
+ * - 2-way interleave, all inline (-DINTERLEAVE=2 -DINTERLEAVE_INLINE)
+ * - 4-way interleave, 4x calls out of line (-DINTERLEAVE=4)
+ * - 4-way interleave, all inline (-DINTERLEAVE=4 -DINTERLEAVE_INLINE)
+ *
+ * Macros imported by this code:
+ * - enc_prepare	- setup NEON registers for encryption
+ * - dec_prepare	- setup NEON registers for decryption
+ * - enc_switch_key	- change to new key after having prepared for encryption
+ * - encrypt_block	- encrypt a single block
+ * - decrypt block	- decrypt a single block
+ * - encrypt_block2x	- encrypt 2 blocks in parallel (if INTERLEAVE == 2)
+ * - decrypt_block2x	- decrypt 2 blocks in parallel (if INTERLEAVE == 2)
+ * - encrypt_block4x	- encrypt 4 blocks in parallel (if INTERLEAVE == 4)
+ * - decrypt_block4x	- decrypt 4 blocks in parallel (if INTERLEAVE == 4)
+ */
+
+#if defined(INTERLEAVE) && !defined(INTERLEAVE_INLINE)
+#define FRAME_PUSH	stp x29, x30, [sp,#-16]! ; mov x29, sp
+#define FRAME_POP	ldp x29, x30, [sp],#16
+
+#if INTERLEAVE == 2
+
+aes_encrypt_block2x:
+	encrypt_block2x	v0, v1, w3, x2, x6, w7
+	ret
+ENDPROC(aes_encrypt_block2x)
+
+aes_decrypt_block2x:
+	decrypt_block2x	v0, v1, w3, x2, x6, w7
+	ret
+ENDPROC(aes_decrypt_block2x)
+
+#elif INTERLEAVE == 4
+
+aes_encrypt_block4x:
+	encrypt_block4x	v0, v1, v2, v3, w3, x2, x6, w7
+	ret
+ENDPROC(aes_encrypt_block4x)
+
+aes_decrypt_block4x:
+	decrypt_block4x	v0, v1, v2, v3, w3, x2, x6, w7
+	ret
+ENDPROC(aes_decrypt_block4x)
+
+#else
+#error INTERLEAVE should equal 2 or 4
+#endif
+
+	.macro		do_encrypt_block2x
+	bl		aes_encrypt_block2x
+	.endm
+
+	.macro		do_decrypt_block2x
+	bl		aes_decrypt_block2x
+	.endm
+
+	.macro		do_encrypt_block4x
+	bl		aes_encrypt_block4x
+	.endm
+
+	.macro		do_decrypt_block4x
+	bl		aes_decrypt_block4x
+	.endm
+
+#else
+#define FRAME_PUSH
+#define FRAME_POP
+
+	.macro		do_encrypt_block2x
+	encrypt_block2x	v0, v1, w3, x2, x6, w7
+	.endm
+
+	.macro		do_decrypt_block2x
+	decrypt_block2x	v0, v1, w3, x2, x6, w7
+	.endm
+
+	.macro		do_encrypt_block4x
+	encrypt_block4x	v0, v1, v2, v3, w3, x2, x6, w7
+	.endm
+
+	.macro		do_decrypt_block4x
+	decrypt_block4x	v0, v1, v2, v3, w3, x2, x6, w7
+	.endm
+
+#endif
+
+
+	/*
+	 * uint32_t ce_aes_sub(uint32_t in) - use the aese instruction to
+	 * perform the AES sbox substitution on each byte in 'input'
+	 */
+ENTRY(ce_aes_sub)
+	dup		v1.4s, w0
+	movi		v0.16b, #0
+	aese		v0.16b, v1.16b
+	umov		w0, v0.4s[0]
+	ret
+ENDPROC(ce_aes_sub)
+
+	/*
+	 * void ce_aes_invert(void *dst, void *src)
+	 */
+ENTRY(ce_aes_invert)
+	ld1		{v0.16b}, [x1]
+	aesimc		v1.16b, v0.16b
+	st1		{v1.16b}, [x0]
+	ret
+ENDPROC(ce_aes_invert)
+
+	/*
+	 * ce_aes_ecb_encrypt(u8 out[], u8 const in[], u8 const rk[],
+	 *                    int rounds, int blocks, int first)
+	 * ce_aes_ecb_decrypt(u8 out[], u8 const in[], u8 const rk[],
+	 *                    int rounds, int blocks, int first)
+	 */
+
+ENTRY(ce_aes_ecb_encrypt)
+	FRAME_PUSH
+	cbz		w5, .LecbencloopNx
+
+	enc_prepare	w3, x2, x5
+
+.LecbencloopNx:
+#if INTERLEAVE >= 2
+	subs		w4, w4, #INTERLEAVE
+	bmi		.Lecbenc1x
+#if INTERLEAVE == 2
+	ld1		{v0.16b-v1.16b}, [x1], #32	/* get 2 pt blocks */
+	do_encrypt_block2x
+	st1		{v0.16b-v1.16b}, [x0], #32
+#else
+	ld1		{v0.16b-v3.16b}, [x1], #64	/* get 4 pt blocks */
+	do_encrypt_block4x
+	st1		{v0.16b-v3.16b}, [x0], #64
+#endif
+	b		.LecbencloopNx
+.Lecbenc1x:
+	adds		w4, w4, #INTERLEAVE
+	beq		.Lecbencout
+#endif
+.Lecbencloop:
+	ld1		{v0.16b}, [x1], #16		/* get next pt block */
+	encrypt_block	v0, w3, x2, x5, w6
+	st1		{v0.16b}, [x0], #16
+	subs		w4, w4, #1
+	bne		.Lecbencloop
+.Lecbencout:
+	FRAME_POP
+	ret
+ENDPROC(ce_aes_ecb_encrypt)
+
+
+ENTRY(ce_aes_ecb_decrypt)
+	FRAME_PUSH
+	cbz		w5, .LecbdecloopNx
+
+	dec_prepare	w3, x2, x5
+
+.LecbdecloopNx:
+#if INTERLEAVE >= 2
+	subs		w4, w4, #INTERLEAVE
+	bmi		.Lecbdec1x
+#if INTERLEAVE == 2
+	ld1		{v0.16b-v1.16b}, [x1], #32	/* get 2 ct blocks */
+	do_decrypt_block2x
+	st1		{v0.16b-v1.16b}, [x0], #32
+#else
+	ld1		{v0.16b-v3.16b}, [x1], #64	/* get 4 ct blocks */
+	do_decrypt_block4x
+	st1		{v0.16b-v3.16b}, [x0], #64
+#endif
+	b		.LecbdecloopNx
+.Lecbdec1x:
+	adds		w4, w4, #INTERLEAVE
+	beq		.Lecbdecout
+#endif
+.Lecbdecloop:
+	ld1		{v0.16b}, [x1], #16		/* get next ct block */
+	decrypt_block	v0, w3, x2, x5, w6
+	st1		{v0.16b}, [x0], #16
+	subs		w4, w4, #1
+	bne		.Lecbdecloop
+.Lecbdecout:
+	FRAME_POP
+	ret
+ENDPROC(ce_aes_ecb_decrypt)
+
+
+	/*
+	 * aes_cbc_encrypt(u8 out[], u8 const in[], u8 const rk[], int rounds,
+	 *		   int blocks, u8 iv[], int first)
+	 * aes_cbc_decrypt(u8 out[], u8 const in[], u8 const rk[], int rounds,
+	 *		   int blocks, u8 iv[], int first)
+	 */
+
+ENTRY(ce_aes_cbc_encrypt)
+	cbz		w6, .Lcbcencloop
+
+	ld1		{v0.16b}, [x5]			/* get iv */
+	enc_prepare	w3, x2, x5
+
+.Lcbcencloop:
+	ld1		{v1.16b}, [x1], #16		/* get next pt block */
+	eor		v0.16b, v0.16b, v1.16b		/* ..and xor with iv */
+	encrypt_block	v0, w3, x2, x5, w6
+	st1		{v0.16b}, [x0], #16
+	subs		w4, w4, #1
+	bne		.Lcbcencloop
+	st1		{v0.16b}, [x5]			/* save iv for later */
+	ret
+ENDPROC(ce_aes_cbc_encrypt)
+
+
+ENTRY(ce_aes_cbc_decrypt)
+	FRAME_PUSH
+	cbz		w6, .LcbcdecloopNx
+
+	ld1		{v7.16b}, [x5]			/* get iv */
+	dec_prepare	w3, x2, x5
+
+.LcbcdecloopNx:
+#if INTERLEAVE >= 2
+	subs		w4, w4, #INTERLEAVE
+	bmi		.Lcbcdec1x
+#if INTERLEAVE == 2
+	ld1		{v0.16b-v1.16b}, [x1], #32	/* get 2 ct blocks */
+	mov		v2.16b, v0.16b
+	mov		v3.16b, v1.16b
+	do_decrypt_block2x
+	eor		v0.16b, v0.16b, v7.16b
+	eor		v1.16b, v1.16b, v2.16b
+	mov		v7.16b, v3.16b
+	st1		{v0.16b-v1.16b}, [x0], #32
+#else
+	ld1		{v0.16b-v3.16b}, [x1], #64	/* get 4 ct blocks */
+	mov		v4.16b, v0.16b
+	mov		v5.16b, v1.16b
+	mov		v6.16b, v2.16b
+	do_decrypt_block4x
+	sub		x1, x1, #16
+	eor		v0.16b, v0.16b, v7.16b
+	eor		v1.16b, v1.16b, v4.16b
+	ld1		{v7.16b}, [x1], #16		/* reload 1 ct block */
+	eor		v2.16b, v2.16b, v5.16b
+	eor		v3.16b, v3.16b, v6.16b
+	st1		{v0.16b-v3.16b}, [x0], #64
+#endif
+	b		.LcbcdecloopNx
+.Lcbcdec1x:
+	adds		w4, w4, #INTERLEAVE
+	beq		.Lcbcdecout
+#endif
+.Lcbcdecloop:
+	ld1		{v1.16b}, [x1], #16		/* get next ct block */
+	mov		v0.16b, v1.16b			/* ...and copy to v0 */
+	decrypt_block	v0, w3, x2, x5, w6
+	eor		v0.16b, v0.16b, v7.16b		/* xor with iv => pt */
+	mov		v7.16b, v1.16b			/* ct is next iv */
+	st1		{v0.16b}, [x0], #16
+	subs		w4, w4, #1
+	bne		.Lcbcdecloop
+.Lcbcdecout:
+	st1		{v1.16b}, [x5]			/* save iv for later */
+	FRAME_POP
+	ret
+ENDPROC(ce_aes_cbc_decrypt)
+
+
+	/*
+	 * aes_ctr_encrypt(u8 out[], u8 const in[], u8 const rk[], int rounds,
+	 *		   int blocks, u8 ctr[], int first)
+	 */
+
+ENTRY(ce_aes_ctr_encrypt)
+	FRAME_PUSH
+	mov		x9, x5			/* save ctr pointer */
+	cbnz		w6, .Lctrfirst		/* 1st time around? */
+	umov		x5, v4.d[1]		/* keep swabbed ctr in reg */
+	rev		x5, x5
+#if INTERLEAVE >= 2
+	cmn		w5, w4			/* 32 bit overflow? */
+	bcs		.Lctrinc
+	add		x5, x5, #1		/* increment BE ctr */
+	b		.LctrincNx
+#else
+	b		.Lctrinc
+#endif
+.Lctrfirst:
+	enc_prepare	w3, x2, x6
+	ld1		{v4.16b}, [x5]
+	umov		x5, v4.d[1]		/* keep swabbed ctr in reg */
+	rev		x5, x5
+#if INTERLEAVE >= 2
+	cmn		w5, w4			/* 32 bit overflow? */
+	bcs		.Lctrloop
+.LctrloopNx:
+	subs		w4, w4, #INTERLEAVE
+	bmi		.Lctr1x
+#if INTERLEAVE == 2
+	mov		v0.8b, v4.8b
+	mov		v1.8b, v4.8b
+	rev		x7, x5
+	add		x5, x5, #1
+	ins		v0.d[1], x7
+	rev		x7, x5
+	add		x5, x5, #1
+	ins		v1.d[1], x7
+	ld1		{v2.16b-v3.16b}, [x1], #32	/* get 2 input blocks */
+	do_encrypt_block2x
+	eor		v0.16b, v0.16b, v2.16b
+	eor		v1.16b, v1.16b, v3.16b
+	st1		{v0.16b-v1.16b}, [x0], #32
+#else
+	ldr		q8, =0x30000000200000001	/* addends 1,2,3[,0] */
+	dup		v7.4s, w5
+	mov		v0.16b, v4.16b
+	add		v7.4s, v7.4s, v8.4s
+	mov		v1.16b, v4.16b
+	rev32		v8.16b, v7.16b
+	mov		v2.16b, v4.16b
+	mov		v3.16b, v4.16b
+	mov		v1.s[3], v8.s[0]
+	mov		v2.s[3], v8.s[1]
+	mov		v3.s[3], v8.s[2]
+	ld1		{v5.16b-v7.16b}, [x1], #48	/* get 3 input blocks */
+	do_encrypt_block4x
+	eor		v0.16b, v5.16b, v0.16b
+	ld1		{v5.16b}, [x1], #16		/* get 1 input block  */
+	eor		v1.16b, v6.16b, v1.16b
+	eor		v2.16b, v7.16b, v2.16b
+	eor		v3.16b, v5.16b, v3.16b
+	st1		{v0.16b-v3.16b}, [x0], #64
+	add		x5, x5, #INTERLEAVE
+#endif
+	cbz		w4, .LctroutNx
+.LctrincNx:
+	rev		x7, x5
+	ins		v4.d[1], x7
+	b		.LctrloopNx
+.LctroutNx:
+	sub		x5, x5, #1
+	rev		x7, x5
+	ins		v4.d[1], x7
+	b		.Lctrout
+.Lctr1x:
+	adds		w4, w4, #INTERLEAVE
+	beq		.Lctrout
+#endif
+.Lctrloop:
+	mov		v0.16b, v4.16b
+	encrypt_block	v0, w3, x2, x6, w7
+	subs		w4, w4, #1
+	bmi		.Lctrhalfblock		/* blocks < 0 means 1/2 block */
+	ld1		{v3.16b}, [x1], #16
+	eor		v3.16b, v0.16b, v3.16b
+	st1		{v3.16b}, [x0], #16
+	beq		.Lctrout
+.Lctrinc:
+	adds		x5, x5, #1		/* increment BE ctr */
+	rev		x7, x5
+	ins		v4.d[1], x7
+	bcc		.Lctrloop		/* no overflow? */
+	umov		x7, v4.d[0]		/* load upper word of ctr  */
+	rev		x7, x7			/* ... to handle the carry */
+	add		x7, x7, #1
+	rev		x7, x7
+	ins		v4.d[0], x7
+	b		.Lctrloop
+.Lctrhalfblock:
+	ld1		{v3.8b}, [x1]
+	eor		v3.8b, v0.8b, v3.8b
+	st1		{v3.8b}, [x0]
+.Lctrout:
+	st1		{v4.16b}, [x9]		/* save ctr for next call */
+	FRAME_POP
+	ret
+ENDPROC(ce_aes_ctr_encrypt)
+	.ltorg
+
+
+	/*
+	 * aes_xts_decrypt(u8 out[], u8 const in[], u8 const rk1[], int rounds,
+	 *		   int blocks, u8 const rk2[], u8 iv[])
+	 * aes_xts_decrypt(u8 out[], u8 const in[], u8 const rk1[], int rounds,
+	 *		   int blocks, u8 const rk2[], u8 iv[])
+	 */
+
+	.macro		next_tweak, out, in, const, tmp
+	sshr		\tmp\().2d,  \in\().2d,   #63
+	and		\tmp\().16b, \tmp\().16b, \const\().16b
+	add		\out\().2d,  \in\().2d,   \in\().2d
+	ext		\tmp\().16b, \tmp\().16b, \tmp\().16b, #8
+	eor		\out\().16b, \out\().16b, \tmp\().16b
+	.endm
+
+.Lxts_mul_x:
+	.word		1, 0, 0x87, 0
+
+ENTRY(ce_aes_xts_encrypt)
+	FRAME_PUSH
+
+	ld1		{v4.16b}, [x6]
+	enc_prepare	w3, x5, x6
+	encrypt_block	v4, w3, x5, x6, w7		/* first tweak */
+	enc_switch_key	w3, x2, x6
+	ldr		q7, .Lxts_mul_x
+	b		.LxtsencNx
+
+.LxtsencloopNx:
+	ldr		q7, .Lxts_mul_x
+	next_tweak	v4, v4, v7, v8
+.LxtsencNx:
+#if INTERLEAVE >= 2
+	subs		w4, w4, #INTERLEAVE
+	bmi		.Lxtsenc1x
+#if INTERLEAVE == 2
+	ld1		{v0.16b-v1.16b}, [x1], #32	/* get 2 pt blocks */
+	next_tweak	v5, v4, v7, v8
+	eor		v0.16b, v0.16b, v4.16b
+	eor		v1.16b, v1.16b, v5.16b
+	do_encrypt_block2x
+	eor		v0.16b, v0.16b, v4.16b
+	eor		v1.16b, v1.16b, v5.16b
+	st1		{v0.16b-v1.16b}, [x0], #32
+	cbz		w4, .LxtsencoutNx
+	next_tweak	v4, v5, v7, v8
+	b		.LxtsencNx
+.LxtsencoutNx:
+	mov		v4.16b, v5.16b
+	b		.Lxtsencout
+#else
+	ld1		{v0.16b-v3.16b}, [x1], #64	/* get 4 pt blocks */
+	next_tweak	v5, v4, v7, v8
+	eor		v0.16b, v0.16b, v4.16b
+	next_tweak	v6, v5, v7, v8
+	eor		v1.16b, v1.16b, v5.16b
+	eor		v2.16b, v2.16b, v6.16b
+	next_tweak	v7, v6, v7, v8
+	eor		v3.16b, v3.16b, v7.16b
+	do_encrypt_block4x
+	eor		v3.16b, v3.16b, v7.16b
+	eor		v0.16b, v0.16b, v4.16b
+	eor		v1.16b, v1.16b, v5.16b
+	eor		v2.16b, v2.16b, v6.16b
+	st1		{v0.16b-v3.16b}, [x0], #64
+	mov		v4.16b, v7.16b
+	cbz		w4, .Lxtsencout
+	b		.LxtsencloopNx
+#endif
+.Lxtsenc1x:
+	adds		w4, w4, #INTERLEAVE
+	beq		.Lxtsencout
+#endif
+.Lxtsencloop:
+	ld1		{v1.16b}, [x1], #16
+	eor		v0.16b, v1.16b, v4.16b
+	encrypt_block	v0, w3, x2, x6, w7
+	eor		v0.16b, v0.16b, v4.16b
+	st1		{v0.16b}, [x0], #16
+	subs		w4, w4, #1
+	beq		.Lxtsencout
+	next_tweak	v4, v4, v7, v8
+	b		.Lxtsencloop
+.Lxtsencout:
+	next_tweak	v4, v4, v7, v8
+	st1		{v4.16b}, [x6], #16
+	FRAME_POP
+	ret
+ENDPROC(ce_aes_xts_encrypt)
+
+
+ENTRY(ce_aes_xts_decrypt)
+	FRAME_PUSH
+
+	ld1		{v4.16b}, [x6]
+	enc_prepare	w3, x5, x6
+	encrypt_block	v4, w3, x5, x6, w7		/* first tweak */
+	dec_prepare	w3, x2, x6
+	ldr		q7, .Lxts_mul_x
+	b		.LxtsdecNx
+
+.LxtsdecloopNx:
+	ldr		q7, .Lxts_mul_x
+	next_tweak	v4, v4, v7, v8
+.LxtsdecNx:
+#if INTERLEAVE >= 2
+	subs		w4, w4, #INTERLEAVE
+	bmi		.Lxtsdec1x
+#if INTERLEAVE == 2
+	ld1		{v0.16b-v1.16b}, [x1], #32	/* get 2 ct blocks */
+	next_tweak	v5, v4, v7, v8
+	eor		v0.16b, v0.16b, v4.16b
+	eor		v1.16b, v1.16b, v5.16b
+	do_decrypt_block2x
+	eor		v0.16b, v0.16b, v4.16b
+	eor		v1.16b, v1.16b, v5.16b
+	st1		{v0.16b-v1.16b}, [x0], #32
+	cbz		w4, .LxtsdecoutNx
+	next_tweak	v4, v5, v7, v8
+	b		.LxtsdecNx
+.LxtsdecoutNx:
+	mov		v4.16b, v5.16b
+	b		.Lxtsdecout
+#else
+	ld1		{v0.16b-v3.16b}, [x1], #64	/* get 4 ct blocks */
+	next_tweak	v5, v4, v7, v8
+	eor		v0.16b, v0.16b, v4.16b
+	next_tweak	v6, v5, v7, v8
+	eor		v1.16b, v1.16b, v5.16b
+	eor		v2.16b, v2.16b, v6.16b
+	next_tweak	v7, v6, v7, v8
+	eor		v3.16b, v3.16b, v7.16b
+	do_decrypt_block4x
+	eor		v3.16b, v3.16b, v7.16b
+	eor		v0.16b, v0.16b, v4.16b
+	eor		v1.16b, v1.16b, v5.16b
+	eor		v2.16b, v2.16b, v6.16b
+	st1		{v0.16b-v3.16b}, [x0], #64
+	mov		v4.16b, v7.16b
+	cbz		w4, .Lxtsdecout
+	b		.LxtsdecloopNx
+#endif
+.Lxtsdec1x:
+	adds		w4, w4, #INTERLEAVE
+	beq		.Lxtsdecout
+#endif
+.Lxtsdecloop:
+	ld1		{v1.16b}, [x1], #16
+	eor		v0.16b, v1.16b, v4.16b
+	decrypt_block	v0, w3, x2, x6, w7
+	eor		v0.16b, v0.16b, v4.16b
+	st1		{v0.16b}, [x0], #16
+	subs		w4, w4, #1
+	beq		.Lxtsdecout
+	next_tweak	v4, v4, v7, v8
+	b		.Lxtsdecloop
+.Lxtsdecout:
+	FRAME_POP
+	next_tweak	v4, v4, v7, v8
+	st1		{v4.16b}, [x6], #16
+	ret
+ENDPROC(ce_aes_xts_decrypt)
diff --git a/core/lib/libtomcrypt/src/ciphers/aes_tab.c b/core/lib/libtomcrypt/src/ciphers/aes_tab.c
new file mode 100644
index 0000000..51a90a6
--- /dev/null
+++ b/core/lib/libtomcrypt/src/ciphers/aes_tab.c
@@ -0,0 +1,1057 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+/* The precomputed tables for AES */
+/*
+Te0[x] = S [x].[02, 01, 01, 03];
+Te1[x] = S [x].[03, 02, 01, 01];
+Te2[x] = S [x].[01, 03, 02, 01];
+Te3[x] = S [x].[01, 01, 03, 02];
+Te4[x] = S [x].[01, 01, 01, 01];
+
+Td0[x] = Si[x].[0e, 09, 0d, 0b];
+Td1[x] = Si[x].[0b, 0e, 09, 0d];
+Td2[x] = Si[x].[0d, 0b, 0e, 09];
+Td3[x] = Si[x].[09, 0d, 0b, 0e];
+Td4[x] = Si[x].[01, 01, 01, 01];
+*/
+
+/**
+  @file aes_tab.c
+  AES tables
+*/  
+#include "tomcrypt_macros.h"
+
+static const ulong32 TE0[256] = {
+    0xc66363a5UL, 0xf87c7c84UL, 0xee777799UL, 0xf67b7b8dUL,
+    0xfff2f20dUL, 0xd66b6bbdUL, 0xde6f6fb1UL, 0x91c5c554UL,
+    0x60303050UL, 0x02010103UL, 0xce6767a9UL, 0x562b2b7dUL,
+    0xe7fefe19UL, 0xb5d7d762UL, 0x4dababe6UL, 0xec76769aUL,
+    0x8fcaca45UL, 0x1f82829dUL, 0x89c9c940UL, 0xfa7d7d87UL,
+    0xeffafa15UL, 0xb25959ebUL, 0x8e4747c9UL, 0xfbf0f00bUL,
+    0x41adadecUL, 0xb3d4d467UL, 0x5fa2a2fdUL, 0x45afafeaUL,
+    0x239c9cbfUL, 0x53a4a4f7UL, 0xe4727296UL, 0x9bc0c05bUL,
+    0x75b7b7c2UL, 0xe1fdfd1cUL, 0x3d9393aeUL, 0x4c26266aUL,
+    0x6c36365aUL, 0x7e3f3f41UL, 0xf5f7f702UL, 0x83cccc4fUL,
+    0x6834345cUL, 0x51a5a5f4UL, 0xd1e5e534UL, 0xf9f1f108UL,
+    0xe2717193UL, 0xabd8d873UL, 0x62313153UL, 0x2a15153fUL,
+    0x0804040cUL, 0x95c7c752UL, 0x46232365UL, 0x9dc3c35eUL,
+    0x30181828UL, 0x379696a1UL, 0x0a05050fUL, 0x2f9a9ab5UL,
+    0x0e070709UL, 0x24121236UL, 0x1b80809bUL, 0xdfe2e23dUL,
+    0xcdebeb26UL, 0x4e272769UL, 0x7fb2b2cdUL, 0xea75759fUL,
+    0x1209091bUL, 0x1d83839eUL, 0x582c2c74UL, 0x341a1a2eUL,
+    0x361b1b2dUL, 0xdc6e6eb2UL, 0xb45a5aeeUL, 0x5ba0a0fbUL,
+    0xa45252f6UL, 0x763b3b4dUL, 0xb7d6d661UL, 0x7db3b3ceUL,
+    0x5229297bUL, 0xdde3e33eUL, 0x5e2f2f71UL, 0x13848497UL,
+    0xa65353f5UL, 0xb9d1d168UL, 0x00000000UL, 0xc1eded2cUL,
+    0x40202060UL, 0xe3fcfc1fUL, 0x79b1b1c8UL, 0xb65b5bedUL,
+    0xd46a6abeUL, 0x8dcbcb46UL, 0x67bebed9UL, 0x7239394bUL,
+    0x944a4adeUL, 0x984c4cd4UL, 0xb05858e8UL, 0x85cfcf4aUL,
+    0xbbd0d06bUL, 0xc5efef2aUL, 0x4faaaae5UL, 0xedfbfb16UL,
+    0x864343c5UL, 0x9a4d4dd7UL, 0x66333355UL, 0x11858594UL,
+    0x8a4545cfUL, 0xe9f9f910UL, 0x04020206UL, 0xfe7f7f81UL,
+    0xa05050f0UL, 0x783c3c44UL, 0x259f9fbaUL, 0x4ba8a8e3UL,
+    0xa25151f3UL, 0x5da3a3feUL, 0x804040c0UL, 0x058f8f8aUL,
+    0x3f9292adUL, 0x219d9dbcUL, 0x70383848UL, 0xf1f5f504UL,
+    0x63bcbcdfUL, 0x77b6b6c1UL, 0xafdada75UL, 0x42212163UL,
+    0x20101030UL, 0xe5ffff1aUL, 0xfdf3f30eUL, 0xbfd2d26dUL,
+    0x81cdcd4cUL, 0x180c0c14UL, 0x26131335UL, 0xc3ecec2fUL,
+    0xbe5f5fe1UL, 0x359797a2UL, 0x884444ccUL, 0x2e171739UL,
+    0x93c4c457UL, 0x55a7a7f2UL, 0xfc7e7e82UL, 0x7a3d3d47UL,
+    0xc86464acUL, 0xba5d5de7UL, 0x3219192bUL, 0xe6737395UL,
+    0xc06060a0UL, 0x19818198UL, 0x9e4f4fd1UL, 0xa3dcdc7fUL,
+    0x44222266UL, 0x542a2a7eUL, 0x3b9090abUL, 0x0b888883UL,
+    0x8c4646caUL, 0xc7eeee29UL, 0x6bb8b8d3UL, 0x2814143cUL,
+    0xa7dede79UL, 0xbc5e5ee2UL, 0x160b0b1dUL, 0xaddbdb76UL,
+    0xdbe0e03bUL, 0x64323256UL, 0x743a3a4eUL, 0x140a0a1eUL,
+    0x924949dbUL, 0x0c06060aUL, 0x4824246cUL, 0xb85c5ce4UL,
+    0x9fc2c25dUL, 0xbdd3d36eUL, 0x43acacefUL, 0xc46262a6UL,
+    0x399191a8UL, 0x319595a4UL, 0xd3e4e437UL, 0xf279798bUL,
+    0xd5e7e732UL, 0x8bc8c843UL, 0x6e373759UL, 0xda6d6db7UL,
+    0x018d8d8cUL, 0xb1d5d564UL, 0x9c4e4ed2UL, 0x49a9a9e0UL,
+    0xd86c6cb4UL, 0xac5656faUL, 0xf3f4f407UL, 0xcfeaea25UL,
+    0xca6565afUL, 0xf47a7a8eUL, 0x47aeaee9UL, 0x10080818UL,
+    0x6fbabad5UL, 0xf0787888UL, 0x4a25256fUL, 0x5c2e2e72UL,
+    0x381c1c24UL, 0x57a6a6f1UL, 0x73b4b4c7UL, 0x97c6c651UL,
+    0xcbe8e823UL, 0xa1dddd7cUL, 0xe874749cUL, 0x3e1f1f21UL,
+    0x964b4bddUL, 0x61bdbddcUL, 0x0d8b8b86UL, 0x0f8a8a85UL,
+    0xe0707090UL, 0x7c3e3e42UL, 0x71b5b5c4UL, 0xcc6666aaUL,
+    0x904848d8UL, 0x06030305UL, 0xf7f6f601UL, 0x1c0e0e12UL,
+    0xc26161a3UL, 0x6a35355fUL, 0xae5757f9UL, 0x69b9b9d0UL,
+    0x17868691UL, 0x99c1c158UL, 0x3a1d1d27UL, 0x279e9eb9UL,
+    0xd9e1e138UL, 0xebf8f813UL, 0x2b9898b3UL, 0x22111133UL,
+    0xd26969bbUL, 0xa9d9d970UL, 0x078e8e89UL, 0x339494a7UL,
+    0x2d9b9bb6UL, 0x3c1e1e22UL, 0x15878792UL, 0xc9e9e920UL,
+    0x87cece49UL, 0xaa5555ffUL, 0x50282878UL, 0xa5dfdf7aUL,
+    0x038c8c8fUL, 0x59a1a1f8UL, 0x09898980UL, 0x1a0d0d17UL,
+    0x65bfbfdaUL, 0xd7e6e631UL, 0x844242c6UL, 0xd06868b8UL,
+    0x824141c3UL, 0x299999b0UL, 0x5a2d2d77UL, 0x1e0f0f11UL,
+    0x7bb0b0cbUL, 0xa85454fcUL, 0x6dbbbbd6UL, 0x2c16163aUL,
+};
+
+#ifndef PELI_TAB
+static const ulong32 Te4[256] = {
+    0x63636363UL, 0x7c7c7c7cUL, 0x77777777UL, 0x7b7b7b7bUL,
+    0xf2f2f2f2UL, 0x6b6b6b6bUL, 0x6f6f6f6fUL, 0xc5c5c5c5UL,
+    0x30303030UL, 0x01010101UL, 0x67676767UL, 0x2b2b2b2bUL,
+    0xfefefefeUL, 0xd7d7d7d7UL, 0xababababUL, 0x76767676UL,
+    0xcacacacaUL, 0x82828282UL, 0xc9c9c9c9UL, 0x7d7d7d7dUL,
+    0xfafafafaUL, 0x59595959UL, 0x47474747UL, 0xf0f0f0f0UL,
+    0xadadadadUL, 0xd4d4d4d4UL, 0xa2a2a2a2UL, 0xafafafafUL,
+    0x9c9c9c9cUL, 0xa4a4a4a4UL, 0x72727272UL, 0xc0c0c0c0UL,
+    0xb7b7b7b7UL, 0xfdfdfdfdUL, 0x93939393UL, 0x26262626UL,
+    0x36363636UL, 0x3f3f3f3fUL, 0xf7f7f7f7UL, 0xccccccccUL,
+    0x34343434UL, 0xa5a5a5a5UL, 0xe5e5e5e5UL, 0xf1f1f1f1UL,
+    0x71717171UL, 0xd8d8d8d8UL, 0x31313131UL, 0x15151515UL,
+    0x04040404UL, 0xc7c7c7c7UL, 0x23232323UL, 0xc3c3c3c3UL,
+    0x18181818UL, 0x96969696UL, 0x05050505UL, 0x9a9a9a9aUL,
+    0x07070707UL, 0x12121212UL, 0x80808080UL, 0xe2e2e2e2UL,
+    0xebebebebUL, 0x27272727UL, 0xb2b2b2b2UL, 0x75757575UL,
+    0x09090909UL, 0x83838383UL, 0x2c2c2c2cUL, 0x1a1a1a1aUL,
+    0x1b1b1b1bUL, 0x6e6e6e6eUL, 0x5a5a5a5aUL, 0xa0a0a0a0UL,
+    0x52525252UL, 0x3b3b3b3bUL, 0xd6d6d6d6UL, 0xb3b3b3b3UL,
+    0x29292929UL, 0xe3e3e3e3UL, 0x2f2f2f2fUL, 0x84848484UL,
+    0x53535353UL, 0xd1d1d1d1UL, 0x00000000UL, 0xededededUL,
+    0x20202020UL, 0xfcfcfcfcUL, 0xb1b1b1b1UL, 0x5b5b5b5bUL,
+    0x6a6a6a6aUL, 0xcbcbcbcbUL, 0xbebebebeUL, 0x39393939UL,
+    0x4a4a4a4aUL, 0x4c4c4c4cUL, 0x58585858UL, 0xcfcfcfcfUL,
+    0xd0d0d0d0UL, 0xefefefefUL, 0xaaaaaaaaUL, 0xfbfbfbfbUL,
+    0x43434343UL, 0x4d4d4d4dUL, 0x33333333UL, 0x85858585UL,
+    0x45454545UL, 0xf9f9f9f9UL, 0x02020202UL, 0x7f7f7f7fUL,
+    0x50505050UL, 0x3c3c3c3cUL, 0x9f9f9f9fUL, 0xa8a8a8a8UL,
+    0x51515151UL, 0xa3a3a3a3UL, 0x40404040UL, 0x8f8f8f8fUL,
+    0x92929292UL, 0x9d9d9d9dUL, 0x38383838UL, 0xf5f5f5f5UL,
+    0xbcbcbcbcUL, 0xb6b6b6b6UL, 0xdadadadaUL, 0x21212121UL,
+    0x10101010UL, 0xffffffffUL, 0xf3f3f3f3UL, 0xd2d2d2d2UL,
+    0xcdcdcdcdUL, 0x0c0c0c0cUL, 0x13131313UL, 0xececececUL,
+    0x5f5f5f5fUL, 0x97979797UL, 0x44444444UL, 0x17171717UL,
+    0xc4c4c4c4UL, 0xa7a7a7a7UL, 0x7e7e7e7eUL, 0x3d3d3d3dUL,
+    0x64646464UL, 0x5d5d5d5dUL, 0x19191919UL, 0x73737373UL,
+    0x60606060UL, 0x81818181UL, 0x4f4f4f4fUL, 0xdcdcdcdcUL,
+    0x22222222UL, 0x2a2a2a2aUL, 0x90909090UL, 0x88888888UL,
+    0x46464646UL, 0xeeeeeeeeUL, 0xb8b8b8b8UL, 0x14141414UL,
+    0xdedededeUL, 0x5e5e5e5eUL, 0x0b0b0b0bUL, 0xdbdbdbdbUL,
+    0xe0e0e0e0UL, 0x32323232UL, 0x3a3a3a3aUL, 0x0a0a0a0aUL,
+    0x49494949UL, 0x06060606UL, 0x24242424UL, 0x5c5c5c5cUL,
+    0xc2c2c2c2UL, 0xd3d3d3d3UL, 0xacacacacUL, 0x62626262UL,
+    0x91919191UL, 0x95959595UL, 0xe4e4e4e4UL, 0x79797979UL,
+    0xe7e7e7e7UL, 0xc8c8c8c8UL, 0x37373737UL, 0x6d6d6d6dUL,
+    0x8d8d8d8dUL, 0xd5d5d5d5UL, 0x4e4e4e4eUL, 0xa9a9a9a9UL,
+    0x6c6c6c6cUL, 0x56565656UL, 0xf4f4f4f4UL, 0xeaeaeaeaUL,
+    0x65656565UL, 0x7a7a7a7aUL, 0xaeaeaeaeUL, 0x08080808UL,
+    0xbabababaUL, 0x78787878UL, 0x25252525UL, 0x2e2e2e2eUL,
+    0x1c1c1c1cUL, 0xa6a6a6a6UL, 0xb4b4b4b4UL, 0xc6c6c6c6UL,
+    0xe8e8e8e8UL, 0xddddddddUL, 0x74747474UL, 0x1f1f1f1fUL,
+    0x4b4b4b4bUL, 0xbdbdbdbdUL, 0x8b8b8b8bUL, 0x8a8a8a8aUL,
+    0x70707070UL, 0x3e3e3e3eUL, 0xb5b5b5b5UL, 0x66666666UL,
+    0x48484848UL, 0x03030303UL, 0xf6f6f6f6UL, 0x0e0e0e0eUL,
+    0x61616161UL, 0x35353535UL, 0x57575757UL, 0xb9b9b9b9UL,
+    0x86868686UL, 0xc1c1c1c1UL, 0x1d1d1d1dUL, 0x9e9e9e9eUL,
+    0xe1e1e1e1UL, 0xf8f8f8f8UL, 0x98989898UL, 0x11111111UL,
+    0x69696969UL, 0xd9d9d9d9UL, 0x8e8e8e8eUL, 0x94949494UL,
+    0x9b9b9b9bUL, 0x1e1e1e1eUL, 0x87878787UL, 0xe9e9e9e9UL,
+    0xcecececeUL, 0x55555555UL, 0x28282828UL, 0xdfdfdfdfUL,
+    0x8c8c8c8cUL, 0xa1a1a1a1UL, 0x89898989UL, 0x0d0d0d0dUL,
+    0xbfbfbfbfUL, 0xe6e6e6e6UL, 0x42424242UL, 0x68686868UL,
+    0x41414141UL, 0x99999999UL, 0x2d2d2d2dUL, 0x0f0f0f0fUL,
+    0xb0b0b0b0UL, 0x54545454UL, 0xbbbbbbbbUL, 0x16161616UL,
+};
+#endif
+
+#ifndef ENCRYPT_ONLY
+
+static const ulong32 TD0[256] = {
+    0x51f4a750UL, 0x7e416553UL, 0x1a17a4c3UL, 0x3a275e96UL,
+    0x3bab6bcbUL, 0x1f9d45f1UL, 0xacfa58abUL, 0x4be30393UL,
+    0x2030fa55UL, 0xad766df6UL, 0x88cc7691UL, 0xf5024c25UL,
+    0x4fe5d7fcUL, 0xc52acbd7UL, 0x26354480UL, 0xb562a38fUL,
+    0xdeb15a49UL, 0x25ba1b67UL, 0x45ea0e98UL, 0x5dfec0e1UL,
+    0xc32f7502UL, 0x814cf012UL, 0x8d4697a3UL, 0x6bd3f9c6UL,
+    0x038f5fe7UL, 0x15929c95UL, 0xbf6d7aebUL, 0x955259daUL,
+    0xd4be832dUL, 0x587421d3UL, 0x49e06929UL, 0x8ec9c844UL,
+    0x75c2896aUL, 0xf48e7978UL, 0x99583e6bUL, 0x27b971ddUL,
+    0xbee14fb6UL, 0xf088ad17UL, 0xc920ac66UL, 0x7dce3ab4UL,
+    0x63df4a18UL, 0xe51a3182UL, 0x97513360UL, 0x62537f45UL,
+    0xb16477e0UL, 0xbb6bae84UL, 0xfe81a01cUL, 0xf9082b94UL,
+    0x70486858UL, 0x8f45fd19UL, 0x94de6c87UL, 0x527bf8b7UL,
+    0xab73d323UL, 0x724b02e2UL, 0xe31f8f57UL, 0x6655ab2aUL,
+    0xb2eb2807UL, 0x2fb5c203UL, 0x86c57b9aUL, 0xd33708a5UL,
+    0x302887f2UL, 0x23bfa5b2UL, 0x02036abaUL, 0xed16825cUL,
+    0x8acf1c2bUL, 0xa779b492UL, 0xf307f2f0UL, 0x4e69e2a1UL,
+    0x65daf4cdUL, 0x0605bed5UL, 0xd134621fUL, 0xc4a6fe8aUL,
+    0x342e539dUL, 0xa2f355a0UL, 0x058ae132UL, 0xa4f6eb75UL,
+    0x0b83ec39UL, 0x4060efaaUL, 0x5e719f06UL, 0xbd6e1051UL,
+    0x3e218af9UL, 0x96dd063dUL, 0xdd3e05aeUL, 0x4de6bd46UL,
+    0x91548db5UL, 0x71c45d05UL, 0x0406d46fUL, 0x605015ffUL,
+    0x1998fb24UL, 0xd6bde997UL, 0x894043ccUL, 0x67d99e77UL,
+    0xb0e842bdUL, 0x07898b88UL, 0xe7195b38UL, 0x79c8eedbUL,
+    0xa17c0a47UL, 0x7c420fe9UL, 0xf8841ec9UL, 0x00000000UL,
+    0x09808683UL, 0x322bed48UL, 0x1e1170acUL, 0x6c5a724eUL,
+    0xfd0efffbUL, 0x0f853856UL, 0x3daed51eUL, 0x362d3927UL,
+    0x0a0fd964UL, 0x685ca621UL, 0x9b5b54d1UL, 0x24362e3aUL,
+    0x0c0a67b1UL, 0x9357e70fUL, 0xb4ee96d2UL, 0x1b9b919eUL,
+    0x80c0c54fUL, 0x61dc20a2UL, 0x5a774b69UL, 0x1c121a16UL,
+    0xe293ba0aUL, 0xc0a02ae5UL, 0x3c22e043UL, 0x121b171dUL,
+    0x0e090d0bUL, 0xf28bc7adUL, 0x2db6a8b9UL, 0x141ea9c8UL,
+    0x57f11985UL, 0xaf75074cUL, 0xee99ddbbUL, 0xa37f60fdUL,
+    0xf701269fUL, 0x5c72f5bcUL, 0x44663bc5UL, 0x5bfb7e34UL,
+    0x8b432976UL, 0xcb23c6dcUL, 0xb6edfc68UL, 0xb8e4f163UL,
+    0xd731dccaUL, 0x42638510UL, 0x13972240UL, 0x84c61120UL,
+    0x854a247dUL, 0xd2bb3df8UL, 0xaef93211UL, 0xc729a16dUL,
+    0x1d9e2f4bUL, 0xdcb230f3UL, 0x0d8652ecUL, 0x77c1e3d0UL,
+    0x2bb3166cUL, 0xa970b999UL, 0x119448faUL, 0x47e96422UL,
+    0xa8fc8cc4UL, 0xa0f03f1aUL, 0x567d2cd8UL, 0x223390efUL,
+    0x87494ec7UL, 0xd938d1c1UL, 0x8ccaa2feUL, 0x98d40b36UL,
+    0xa6f581cfUL, 0xa57ade28UL, 0xdab78e26UL, 0x3fadbfa4UL,
+    0x2c3a9de4UL, 0x5078920dUL, 0x6a5fcc9bUL, 0x547e4662UL,
+    0xf68d13c2UL, 0x90d8b8e8UL, 0x2e39f75eUL, 0x82c3aff5UL,
+    0x9f5d80beUL, 0x69d0937cUL, 0x6fd52da9UL, 0xcf2512b3UL,
+    0xc8ac993bUL, 0x10187da7UL, 0xe89c636eUL, 0xdb3bbb7bUL,
+    0xcd267809UL, 0x6e5918f4UL, 0xec9ab701UL, 0x834f9aa8UL,
+    0xe6956e65UL, 0xaaffe67eUL, 0x21bccf08UL, 0xef15e8e6UL,
+    0xbae79bd9UL, 0x4a6f36ceUL, 0xea9f09d4UL, 0x29b07cd6UL,
+    0x31a4b2afUL, 0x2a3f2331UL, 0xc6a59430UL, 0x35a266c0UL,
+    0x744ebc37UL, 0xfc82caa6UL, 0xe090d0b0UL, 0x33a7d815UL,
+    0xf104984aUL, 0x41ecdaf7UL, 0x7fcd500eUL, 0x1791f62fUL,
+    0x764dd68dUL, 0x43efb04dUL, 0xccaa4d54UL, 0xe49604dfUL,
+    0x9ed1b5e3UL, 0x4c6a881bUL, 0xc12c1fb8UL, 0x4665517fUL,
+    0x9d5eea04UL, 0x018c355dUL, 0xfa877473UL, 0xfb0b412eUL,
+    0xb3671d5aUL, 0x92dbd252UL, 0xe9105633UL, 0x6dd64713UL,
+    0x9ad7618cUL, 0x37a10c7aUL, 0x59f8148eUL, 0xeb133c89UL,
+    0xcea927eeUL, 0xb761c935UL, 0xe11ce5edUL, 0x7a47b13cUL,
+    0x9cd2df59UL, 0x55f2733fUL, 0x1814ce79UL, 0x73c737bfUL,
+    0x53f7cdeaUL, 0x5ffdaa5bUL, 0xdf3d6f14UL, 0x7844db86UL,
+    0xcaaff381UL, 0xb968c43eUL, 0x3824342cUL, 0xc2a3405fUL,
+    0x161dc372UL, 0xbce2250cUL, 0x283c498bUL, 0xff0d9541UL,
+    0x39a80171UL, 0x080cb3deUL, 0xd8b4e49cUL, 0x6456c190UL,
+    0x7bcb8461UL, 0xd532b670UL, 0x486c5c74UL, 0xd0b85742UL,
+};
+
+static const ulong32 Td4[256] = {
+    0x52525252UL, 0x09090909UL, 0x6a6a6a6aUL, 0xd5d5d5d5UL,
+    0x30303030UL, 0x36363636UL, 0xa5a5a5a5UL, 0x38383838UL,
+    0xbfbfbfbfUL, 0x40404040UL, 0xa3a3a3a3UL, 0x9e9e9e9eUL,
+    0x81818181UL, 0xf3f3f3f3UL, 0xd7d7d7d7UL, 0xfbfbfbfbUL,
+    0x7c7c7c7cUL, 0xe3e3e3e3UL, 0x39393939UL, 0x82828282UL,
+    0x9b9b9b9bUL, 0x2f2f2f2fUL, 0xffffffffUL, 0x87878787UL,
+    0x34343434UL, 0x8e8e8e8eUL, 0x43434343UL, 0x44444444UL,
+    0xc4c4c4c4UL, 0xdedededeUL, 0xe9e9e9e9UL, 0xcbcbcbcbUL,
+    0x54545454UL, 0x7b7b7b7bUL, 0x94949494UL, 0x32323232UL,
+    0xa6a6a6a6UL, 0xc2c2c2c2UL, 0x23232323UL, 0x3d3d3d3dUL,
+    0xeeeeeeeeUL, 0x4c4c4c4cUL, 0x95959595UL, 0x0b0b0b0bUL,
+    0x42424242UL, 0xfafafafaUL, 0xc3c3c3c3UL, 0x4e4e4e4eUL,
+    0x08080808UL, 0x2e2e2e2eUL, 0xa1a1a1a1UL, 0x66666666UL,
+    0x28282828UL, 0xd9d9d9d9UL, 0x24242424UL, 0xb2b2b2b2UL,
+    0x76767676UL, 0x5b5b5b5bUL, 0xa2a2a2a2UL, 0x49494949UL,
+    0x6d6d6d6dUL, 0x8b8b8b8bUL, 0xd1d1d1d1UL, 0x25252525UL,
+    0x72727272UL, 0xf8f8f8f8UL, 0xf6f6f6f6UL, 0x64646464UL,
+    0x86868686UL, 0x68686868UL, 0x98989898UL, 0x16161616UL,
+    0xd4d4d4d4UL, 0xa4a4a4a4UL, 0x5c5c5c5cUL, 0xccccccccUL,
+    0x5d5d5d5dUL, 0x65656565UL, 0xb6b6b6b6UL, 0x92929292UL,
+    0x6c6c6c6cUL, 0x70707070UL, 0x48484848UL, 0x50505050UL,
+    0xfdfdfdfdUL, 0xededededUL, 0xb9b9b9b9UL, 0xdadadadaUL,
+    0x5e5e5e5eUL, 0x15151515UL, 0x46464646UL, 0x57575757UL,
+    0xa7a7a7a7UL, 0x8d8d8d8dUL, 0x9d9d9d9dUL, 0x84848484UL,
+    0x90909090UL, 0xd8d8d8d8UL, 0xababababUL, 0x00000000UL,
+    0x8c8c8c8cUL, 0xbcbcbcbcUL, 0xd3d3d3d3UL, 0x0a0a0a0aUL,
+    0xf7f7f7f7UL, 0xe4e4e4e4UL, 0x58585858UL, 0x05050505UL,
+    0xb8b8b8b8UL, 0xb3b3b3b3UL, 0x45454545UL, 0x06060606UL,
+    0xd0d0d0d0UL, 0x2c2c2c2cUL, 0x1e1e1e1eUL, 0x8f8f8f8fUL,
+    0xcacacacaUL, 0x3f3f3f3fUL, 0x0f0f0f0fUL, 0x02020202UL,
+    0xc1c1c1c1UL, 0xafafafafUL, 0xbdbdbdbdUL, 0x03030303UL,
+    0x01010101UL, 0x13131313UL, 0x8a8a8a8aUL, 0x6b6b6b6bUL,
+    0x3a3a3a3aUL, 0x91919191UL, 0x11111111UL, 0x41414141UL,
+    0x4f4f4f4fUL, 0x67676767UL, 0xdcdcdcdcUL, 0xeaeaeaeaUL,
+    0x97979797UL, 0xf2f2f2f2UL, 0xcfcfcfcfUL, 0xcecececeUL,
+    0xf0f0f0f0UL, 0xb4b4b4b4UL, 0xe6e6e6e6UL, 0x73737373UL,
+    0x96969696UL, 0xacacacacUL, 0x74747474UL, 0x22222222UL,
+    0xe7e7e7e7UL, 0xadadadadUL, 0x35353535UL, 0x85858585UL,
+    0xe2e2e2e2UL, 0xf9f9f9f9UL, 0x37373737UL, 0xe8e8e8e8UL,
+    0x1c1c1c1cUL, 0x75757575UL, 0xdfdfdfdfUL, 0x6e6e6e6eUL,
+    0x47474747UL, 0xf1f1f1f1UL, 0x1a1a1a1aUL, 0x71717171UL,
+    0x1d1d1d1dUL, 0x29292929UL, 0xc5c5c5c5UL, 0x89898989UL,
+    0x6f6f6f6fUL, 0xb7b7b7b7UL, 0x62626262UL, 0x0e0e0e0eUL,
+    0xaaaaaaaaUL, 0x18181818UL, 0xbebebebeUL, 0x1b1b1b1bUL,
+    0xfcfcfcfcUL, 0x56565656UL, 0x3e3e3e3eUL, 0x4b4b4b4bUL,
+    0xc6c6c6c6UL, 0xd2d2d2d2UL, 0x79797979UL, 0x20202020UL,
+    0x9a9a9a9aUL, 0xdbdbdbdbUL, 0xc0c0c0c0UL, 0xfefefefeUL,
+    0x78787878UL, 0xcdcdcdcdUL, 0x5a5a5a5aUL, 0xf4f4f4f4UL,
+    0x1f1f1f1fUL, 0xddddddddUL, 0xa8a8a8a8UL, 0x33333333UL,
+    0x88888888UL, 0x07070707UL, 0xc7c7c7c7UL, 0x31313131UL,
+    0xb1b1b1b1UL, 0x12121212UL, 0x10101010UL, 0x59595959UL,
+    0x27272727UL, 0x80808080UL, 0xececececUL, 0x5f5f5f5fUL,
+    0x60606060UL, 0x51515151UL, 0x7f7f7f7fUL, 0xa9a9a9a9UL,
+    0x19191919UL, 0xb5b5b5b5UL, 0x4a4a4a4aUL, 0x0d0d0d0dUL,
+    0x2d2d2d2dUL, 0xe5e5e5e5UL, 0x7a7a7a7aUL, 0x9f9f9f9fUL,
+    0x93939393UL, 0xc9c9c9c9UL, 0x9c9c9c9cUL, 0xefefefefUL,
+    0xa0a0a0a0UL, 0xe0e0e0e0UL, 0x3b3b3b3bUL, 0x4d4d4d4dUL,
+    0xaeaeaeaeUL, 0x2a2a2a2aUL, 0xf5f5f5f5UL, 0xb0b0b0b0UL,
+    0xc8c8c8c8UL, 0xebebebebUL, 0xbbbbbbbbUL, 0x3c3c3c3cUL,
+    0x83838383UL, 0x53535353UL, 0x99999999UL, 0x61616161UL,
+    0x17171717UL, 0x2b2b2b2bUL, 0x04040404UL, 0x7e7e7e7eUL,
+    0xbabababaUL, 0x77777777UL, 0xd6d6d6d6UL, 0x26262626UL,
+    0xe1e1e1e1UL, 0x69696969UL, 0x14141414UL, 0x63636363UL,
+    0x55555555UL, 0x21212121UL, 0x0c0c0c0cUL, 0x7d7d7d7dUL,
+};
+
+#endif /* ENCRYPT_ONLY */
+
+#ifdef LTC_SMALL_CODE
+
+#define Te0(x) TE0[x]
+#define Te1(x) RORc(TE0[x], 8)
+#define Te2(x) RORc(TE0[x], 16)
+#define Te3(x) RORc(TE0[x], 24)
+
+#define Td0(x) TD0[x]
+#define Td1(x) RORc(TD0[x], 8)
+#define Td2(x) RORc(TD0[x], 16)
+#define Td3(x) RORc(TD0[x], 24)
+
+#define Te4_0 0x000000FF & Te4
+#define Te4_1 0x0000FF00 & Te4
+#define Te4_2 0x00FF0000 & Te4
+#define Te4_3 0xFF000000 & Te4
+
+#else
+
+#define Te0(x) TE0[x]
+#define Te1(x) TE1[x]
+#define Te2(x) TE2[x]
+#define Te3(x) TE3[x]
+
+#define Td0(x) TD0[x]
+#define Td1(x) TD1[x]
+#define Td2(x) TD2[x]
+#define Td3(x) TD3[x]
+
+static const ulong32 TE1[256] = {
+    0xa5c66363UL, 0x84f87c7cUL, 0x99ee7777UL, 0x8df67b7bUL,
+    0x0dfff2f2UL, 0xbdd66b6bUL, 0xb1de6f6fUL, 0x5491c5c5UL,
+    0x50603030UL, 0x03020101UL, 0xa9ce6767UL, 0x7d562b2bUL,
+    0x19e7fefeUL, 0x62b5d7d7UL, 0xe64dababUL, 0x9aec7676UL,
+    0x458fcacaUL, 0x9d1f8282UL, 0x4089c9c9UL, 0x87fa7d7dUL,
+    0x15effafaUL, 0xebb25959UL, 0xc98e4747UL, 0x0bfbf0f0UL,
+    0xec41adadUL, 0x67b3d4d4UL, 0xfd5fa2a2UL, 0xea45afafUL,
+    0xbf239c9cUL, 0xf753a4a4UL, 0x96e47272UL, 0x5b9bc0c0UL,
+    0xc275b7b7UL, 0x1ce1fdfdUL, 0xae3d9393UL, 0x6a4c2626UL,
+    0x5a6c3636UL, 0x417e3f3fUL, 0x02f5f7f7UL, 0x4f83ccccUL,
+    0x5c683434UL, 0xf451a5a5UL, 0x34d1e5e5UL, 0x08f9f1f1UL,
+    0x93e27171UL, 0x73abd8d8UL, 0x53623131UL, 0x3f2a1515UL,
+    0x0c080404UL, 0x5295c7c7UL, 0x65462323UL, 0x5e9dc3c3UL,
+    0x28301818UL, 0xa1379696UL, 0x0f0a0505UL, 0xb52f9a9aUL,
+    0x090e0707UL, 0x36241212UL, 0x9b1b8080UL, 0x3ddfe2e2UL,
+    0x26cdebebUL, 0x694e2727UL, 0xcd7fb2b2UL, 0x9fea7575UL,
+    0x1b120909UL, 0x9e1d8383UL, 0x74582c2cUL, 0x2e341a1aUL,
+    0x2d361b1bUL, 0xb2dc6e6eUL, 0xeeb45a5aUL, 0xfb5ba0a0UL,
+    0xf6a45252UL, 0x4d763b3bUL, 0x61b7d6d6UL, 0xce7db3b3UL,
+    0x7b522929UL, 0x3edde3e3UL, 0x715e2f2fUL, 0x97138484UL,
+    0xf5a65353UL, 0x68b9d1d1UL, 0x00000000UL, 0x2cc1ededUL,
+    0x60402020UL, 0x1fe3fcfcUL, 0xc879b1b1UL, 0xedb65b5bUL,
+    0xbed46a6aUL, 0x468dcbcbUL, 0xd967bebeUL, 0x4b723939UL,
+    0xde944a4aUL, 0xd4984c4cUL, 0xe8b05858UL, 0x4a85cfcfUL,
+    0x6bbbd0d0UL, 0x2ac5efefUL, 0xe54faaaaUL, 0x16edfbfbUL,
+    0xc5864343UL, 0xd79a4d4dUL, 0x55663333UL, 0x94118585UL,
+    0xcf8a4545UL, 0x10e9f9f9UL, 0x06040202UL, 0x81fe7f7fUL,
+    0xf0a05050UL, 0x44783c3cUL, 0xba259f9fUL, 0xe34ba8a8UL,
+    0xf3a25151UL, 0xfe5da3a3UL, 0xc0804040UL, 0x8a058f8fUL,
+    0xad3f9292UL, 0xbc219d9dUL, 0x48703838UL, 0x04f1f5f5UL,
+    0xdf63bcbcUL, 0xc177b6b6UL, 0x75afdadaUL, 0x63422121UL,
+    0x30201010UL, 0x1ae5ffffUL, 0x0efdf3f3UL, 0x6dbfd2d2UL,
+    0x4c81cdcdUL, 0x14180c0cUL, 0x35261313UL, 0x2fc3ececUL,
+    0xe1be5f5fUL, 0xa2359797UL, 0xcc884444UL, 0x392e1717UL,
+    0x5793c4c4UL, 0xf255a7a7UL, 0x82fc7e7eUL, 0x477a3d3dUL,
+    0xacc86464UL, 0xe7ba5d5dUL, 0x2b321919UL, 0x95e67373UL,
+    0xa0c06060UL, 0x98198181UL, 0xd19e4f4fUL, 0x7fa3dcdcUL,
+    0x66442222UL, 0x7e542a2aUL, 0xab3b9090UL, 0x830b8888UL,
+    0xca8c4646UL, 0x29c7eeeeUL, 0xd36bb8b8UL, 0x3c281414UL,
+    0x79a7dedeUL, 0xe2bc5e5eUL, 0x1d160b0bUL, 0x76addbdbUL,
+    0x3bdbe0e0UL, 0x56643232UL, 0x4e743a3aUL, 0x1e140a0aUL,
+    0xdb924949UL, 0x0a0c0606UL, 0x6c482424UL, 0xe4b85c5cUL,
+    0x5d9fc2c2UL, 0x6ebdd3d3UL, 0xef43acacUL, 0xa6c46262UL,
+    0xa8399191UL, 0xa4319595UL, 0x37d3e4e4UL, 0x8bf27979UL,
+    0x32d5e7e7UL, 0x438bc8c8UL, 0x596e3737UL, 0xb7da6d6dUL,
+    0x8c018d8dUL, 0x64b1d5d5UL, 0xd29c4e4eUL, 0xe049a9a9UL,
+    0xb4d86c6cUL, 0xfaac5656UL, 0x07f3f4f4UL, 0x25cfeaeaUL,
+    0xafca6565UL, 0x8ef47a7aUL, 0xe947aeaeUL, 0x18100808UL,
+    0xd56fbabaUL, 0x88f07878UL, 0x6f4a2525UL, 0x725c2e2eUL,
+    0x24381c1cUL, 0xf157a6a6UL, 0xc773b4b4UL, 0x5197c6c6UL,
+    0x23cbe8e8UL, 0x7ca1ddddUL, 0x9ce87474UL, 0x213e1f1fUL,
+    0xdd964b4bUL, 0xdc61bdbdUL, 0x860d8b8bUL, 0x850f8a8aUL,
+    0x90e07070UL, 0x427c3e3eUL, 0xc471b5b5UL, 0xaacc6666UL,
+    0xd8904848UL, 0x05060303UL, 0x01f7f6f6UL, 0x121c0e0eUL,
+    0xa3c26161UL, 0x5f6a3535UL, 0xf9ae5757UL, 0xd069b9b9UL,
+    0x91178686UL, 0x5899c1c1UL, 0x273a1d1dUL, 0xb9279e9eUL,
+    0x38d9e1e1UL, 0x13ebf8f8UL, 0xb32b9898UL, 0x33221111UL,
+    0xbbd26969UL, 0x70a9d9d9UL, 0x89078e8eUL, 0xa7339494UL,
+    0xb62d9b9bUL, 0x223c1e1eUL, 0x92158787UL, 0x20c9e9e9UL,
+    0x4987ceceUL, 0xffaa5555UL, 0x78502828UL, 0x7aa5dfdfUL,
+    0x8f038c8cUL, 0xf859a1a1UL, 0x80098989UL, 0x171a0d0dUL,
+    0xda65bfbfUL, 0x31d7e6e6UL, 0xc6844242UL, 0xb8d06868UL,
+    0xc3824141UL, 0xb0299999UL, 0x775a2d2dUL, 0x111e0f0fUL,
+    0xcb7bb0b0UL, 0xfca85454UL, 0xd66dbbbbUL, 0x3a2c1616UL,
+};
+static const ulong32 TE2[256] = {
+    0x63a5c663UL, 0x7c84f87cUL, 0x7799ee77UL, 0x7b8df67bUL,
+    0xf20dfff2UL, 0x6bbdd66bUL, 0x6fb1de6fUL, 0xc55491c5UL,
+    0x30506030UL, 0x01030201UL, 0x67a9ce67UL, 0x2b7d562bUL,
+    0xfe19e7feUL, 0xd762b5d7UL, 0xabe64dabUL, 0x769aec76UL,
+    0xca458fcaUL, 0x829d1f82UL, 0xc94089c9UL, 0x7d87fa7dUL,
+    0xfa15effaUL, 0x59ebb259UL, 0x47c98e47UL, 0xf00bfbf0UL,
+    0xadec41adUL, 0xd467b3d4UL, 0xa2fd5fa2UL, 0xafea45afUL,
+    0x9cbf239cUL, 0xa4f753a4UL, 0x7296e472UL, 0xc05b9bc0UL,
+    0xb7c275b7UL, 0xfd1ce1fdUL, 0x93ae3d93UL, 0x266a4c26UL,
+    0x365a6c36UL, 0x3f417e3fUL, 0xf702f5f7UL, 0xcc4f83ccUL,
+    0x345c6834UL, 0xa5f451a5UL, 0xe534d1e5UL, 0xf108f9f1UL,
+    0x7193e271UL, 0xd873abd8UL, 0x31536231UL, 0x153f2a15UL,
+    0x040c0804UL, 0xc75295c7UL, 0x23654623UL, 0xc35e9dc3UL,
+    0x18283018UL, 0x96a13796UL, 0x050f0a05UL, 0x9ab52f9aUL,
+    0x07090e07UL, 0x12362412UL, 0x809b1b80UL, 0xe23ddfe2UL,
+    0xeb26cdebUL, 0x27694e27UL, 0xb2cd7fb2UL, 0x759fea75UL,
+    0x091b1209UL, 0x839e1d83UL, 0x2c74582cUL, 0x1a2e341aUL,
+    0x1b2d361bUL, 0x6eb2dc6eUL, 0x5aeeb45aUL, 0xa0fb5ba0UL,
+    0x52f6a452UL, 0x3b4d763bUL, 0xd661b7d6UL, 0xb3ce7db3UL,
+    0x297b5229UL, 0xe33edde3UL, 0x2f715e2fUL, 0x84971384UL,
+    0x53f5a653UL, 0xd168b9d1UL, 0x00000000UL, 0xed2cc1edUL,
+    0x20604020UL, 0xfc1fe3fcUL, 0xb1c879b1UL, 0x5bedb65bUL,
+    0x6abed46aUL, 0xcb468dcbUL, 0xbed967beUL, 0x394b7239UL,
+    0x4ade944aUL, 0x4cd4984cUL, 0x58e8b058UL, 0xcf4a85cfUL,
+    0xd06bbbd0UL, 0xef2ac5efUL, 0xaae54faaUL, 0xfb16edfbUL,
+    0x43c58643UL, 0x4dd79a4dUL, 0x33556633UL, 0x85941185UL,
+    0x45cf8a45UL, 0xf910e9f9UL, 0x02060402UL, 0x7f81fe7fUL,
+    0x50f0a050UL, 0x3c44783cUL, 0x9fba259fUL, 0xa8e34ba8UL,
+    0x51f3a251UL, 0xa3fe5da3UL, 0x40c08040UL, 0x8f8a058fUL,
+    0x92ad3f92UL, 0x9dbc219dUL, 0x38487038UL, 0xf504f1f5UL,
+    0xbcdf63bcUL, 0xb6c177b6UL, 0xda75afdaUL, 0x21634221UL,
+    0x10302010UL, 0xff1ae5ffUL, 0xf30efdf3UL, 0xd26dbfd2UL,
+    0xcd4c81cdUL, 0x0c14180cUL, 0x13352613UL, 0xec2fc3ecUL,
+    0x5fe1be5fUL, 0x97a23597UL, 0x44cc8844UL, 0x17392e17UL,
+    0xc45793c4UL, 0xa7f255a7UL, 0x7e82fc7eUL, 0x3d477a3dUL,
+    0x64acc864UL, 0x5de7ba5dUL, 0x192b3219UL, 0x7395e673UL,
+    0x60a0c060UL, 0x81981981UL, 0x4fd19e4fUL, 0xdc7fa3dcUL,
+    0x22664422UL, 0x2a7e542aUL, 0x90ab3b90UL, 0x88830b88UL,
+    0x46ca8c46UL, 0xee29c7eeUL, 0xb8d36bb8UL, 0x143c2814UL,
+    0xde79a7deUL, 0x5ee2bc5eUL, 0x0b1d160bUL, 0xdb76addbUL,
+    0xe03bdbe0UL, 0x32566432UL, 0x3a4e743aUL, 0x0a1e140aUL,
+    0x49db9249UL, 0x060a0c06UL, 0x246c4824UL, 0x5ce4b85cUL,
+    0xc25d9fc2UL, 0xd36ebdd3UL, 0xacef43acUL, 0x62a6c462UL,
+    0x91a83991UL, 0x95a43195UL, 0xe437d3e4UL, 0x798bf279UL,
+    0xe732d5e7UL, 0xc8438bc8UL, 0x37596e37UL, 0x6db7da6dUL,
+    0x8d8c018dUL, 0xd564b1d5UL, 0x4ed29c4eUL, 0xa9e049a9UL,
+    0x6cb4d86cUL, 0x56faac56UL, 0xf407f3f4UL, 0xea25cfeaUL,
+    0x65afca65UL, 0x7a8ef47aUL, 0xaee947aeUL, 0x08181008UL,
+    0xbad56fbaUL, 0x7888f078UL, 0x256f4a25UL, 0x2e725c2eUL,
+    0x1c24381cUL, 0xa6f157a6UL, 0xb4c773b4UL, 0xc65197c6UL,
+    0xe823cbe8UL, 0xdd7ca1ddUL, 0x749ce874UL, 0x1f213e1fUL,
+    0x4bdd964bUL, 0xbddc61bdUL, 0x8b860d8bUL, 0x8a850f8aUL,
+    0x7090e070UL, 0x3e427c3eUL, 0xb5c471b5UL, 0x66aacc66UL,
+    0x48d89048UL, 0x03050603UL, 0xf601f7f6UL, 0x0e121c0eUL,
+    0x61a3c261UL, 0x355f6a35UL, 0x57f9ae57UL, 0xb9d069b9UL,
+    0x86911786UL, 0xc15899c1UL, 0x1d273a1dUL, 0x9eb9279eUL,
+    0xe138d9e1UL, 0xf813ebf8UL, 0x98b32b98UL, 0x11332211UL,
+    0x69bbd269UL, 0xd970a9d9UL, 0x8e89078eUL, 0x94a73394UL,
+    0x9bb62d9bUL, 0x1e223c1eUL, 0x87921587UL, 0xe920c9e9UL,
+    0xce4987ceUL, 0x55ffaa55UL, 0x28785028UL, 0xdf7aa5dfUL,
+    0x8c8f038cUL, 0xa1f859a1UL, 0x89800989UL, 0x0d171a0dUL,
+    0xbfda65bfUL, 0xe631d7e6UL, 0x42c68442UL, 0x68b8d068UL,
+    0x41c38241UL, 0x99b02999UL, 0x2d775a2dUL, 0x0f111e0fUL,
+    0xb0cb7bb0UL, 0x54fca854UL, 0xbbd66dbbUL, 0x163a2c16UL,
+};
+static const ulong32 TE3[256] = {
+
+    0x6363a5c6UL, 0x7c7c84f8UL, 0x777799eeUL, 0x7b7b8df6UL,
+    0xf2f20dffUL, 0x6b6bbdd6UL, 0x6f6fb1deUL, 0xc5c55491UL,
+    0x30305060UL, 0x01010302UL, 0x6767a9ceUL, 0x2b2b7d56UL,
+    0xfefe19e7UL, 0xd7d762b5UL, 0xababe64dUL, 0x76769aecUL,
+    0xcaca458fUL, 0x82829d1fUL, 0xc9c94089UL, 0x7d7d87faUL,
+    0xfafa15efUL, 0x5959ebb2UL, 0x4747c98eUL, 0xf0f00bfbUL,
+    0xadadec41UL, 0xd4d467b3UL, 0xa2a2fd5fUL, 0xafafea45UL,
+    0x9c9cbf23UL, 0xa4a4f753UL, 0x727296e4UL, 0xc0c05b9bUL,
+    0xb7b7c275UL, 0xfdfd1ce1UL, 0x9393ae3dUL, 0x26266a4cUL,
+    0x36365a6cUL, 0x3f3f417eUL, 0xf7f702f5UL, 0xcccc4f83UL,
+    0x34345c68UL, 0xa5a5f451UL, 0xe5e534d1UL, 0xf1f108f9UL,
+    0x717193e2UL, 0xd8d873abUL, 0x31315362UL, 0x15153f2aUL,
+    0x04040c08UL, 0xc7c75295UL, 0x23236546UL, 0xc3c35e9dUL,
+    0x18182830UL, 0x9696a137UL, 0x05050f0aUL, 0x9a9ab52fUL,
+    0x0707090eUL, 0x12123624UL, 0x80809b1bUL, 0xe2e23ddfUL,
+    0xebeb26cdUL, 0x2727694eUL, 0xb2b2cd7fUL, 0x75759feaUL,
+    0x09091b12UL, 0x83839e1dUL, 0x2c2c7458UL, 0x1a1a2e34UL,
+    0x1b1b2d36UL, 0x6e6eb2dcUL, 0x5a5aeeb4UL, 0xa0a0fb5bUL,
+    0x5252f6a4UL, 0x3b3b4d76UL, 0xd6d661b7UL, 0xb3b3ce7dUL,
+    0x29297b52UL, 0xe3e33eddUL, 0x2f2f715eUL, 0x84849713UL,
+    0x5353f5a6UL, 0xd1d168b9UL, 0x00000000UL, 0xeded2cc1UL,
+    0x20206040UL, 0xfcfc1fe3UL, 0xb1b1c879UL, 0x5b5bedb6UL,
+    0x6a6abed4UL, 0xcbcb468dUL, 0xbebed967UL, 0x39394b72UL,
+    0x4a4ade94UL, 0x4c4cd498UL, 0x5858e8b0UL, 0xcfcf4a85UL,
+    0xd0d06bbbUL, 0xefef2ac5UL, 0xaaaae54fUL, 0xfbfb16edUL,
+    0x4343c586UL, 0x4d4dd79aUL, 0x33335566UL, 0x85859411UL,
+    0x4545cf8aUL, 0xf9f910e9UL, 0x02020604UL, 0x7f7f81feUL,
+    0x5050f0a0UL, 0x3c3c4478UL, 0x9f9fba25UL, 0xa8a8e34bUL,
+    0x5151f3a2UL, 0xa3a3fe5dUL, 0x4040c080UL, 0x8f8f8a05UL,
+    0x9292ad3fUL, 0x9d9dbc21UL, 0x38384870UL, 0xf5f504f1UL,
+    0xbcbcdf63UL, 0xb6b6c177UL, 0xdada75afUL, 0x21216342UL,
+    0x10103020UL, 0xffff1ae5UL, 0xf3f30efdUL, 0xd2d26dbfUL,
+    0xcdcd4c81UL, 0x0c0c1418UL, 0x13133526UL, 0xecec2fc3UL,
+    0x5f5fe1beUL, 0x9797a235UL, 0x4444cc88UL, 0x1717392eUL,
+    0xc4c45793UL, 0xa7a7f255UL, 0x7e7e82fcUL, 0x3d3d477aUL,
+    0x6464acc8UL, 0x5d5de7baUL, 0x19192b32UL, 0x737395e6UL,
+    0x6060a0c0UL, 0x81819819UL, 0x4f4fd19eUL, 0xdcdc7fa3UL,
+    0x22226644UL, 0x2a2a7e54UL, 0x9090ab3bUL, 0x8888830bUL,
+    0x4646ca8cUL, 0xeeee29c7UL, 0xb8b8d36bUL, 0x14143c28UL,
+    0xdede79a7UL, 0x5e5ee2bcUL, 0x0b0b1d16UL, 0xdbdb76adUL,
+    0xe0e03bdbUL, 0x32325664UL, 0x3a3a4e74UL, 0x0a0a1e14UL,
+    0x4949db92UL, 0x06060a0cUL, 0x24246c48UL, 0x5c5ce4b8UL,
+    0xc2c25d9fUL, 0xd3d36ebdUL, 0xacacef43UL, 0x6262a6c4UL,
+    0x9191a839UL, 0x9595a431UL, 0xe4e437d3UL, 0x79798bf2UL,
+    0xe7e732d5UL, 0xc8c8438bUL, 0x3737596eUL, 0x6d6db7daUL,
+    0x8d8d8c01UL, 0xd5d564b1UL, 0x4e4ed29cUL, 0xa9a9e049UL,
+    0x6c6cb4d8UL, 0x5656faacUL, 0xf4f407f3UL, 0xeaea25cfUL,
+    0x6565afcaUL, 0x7a7a8ef4UL, 0xaeaee947UL, 0x08081810UL,
+    0xbabad56fUL, 0x787888f0UL, 0x25256f4aUL, 0x2e2e725cUL,
+    0x1c1c2438UL, 0xa6a6f157UL, 0xb4b4c773UL, 0xc6c65197UL,
+    0xe8e823cbUL, 0xdddd7ca1UL, 0x74749ce8UL, 0x1f1f213eUL,
+    0x4b4bdd96UL, 0xbdbddc61UL, 0x8b8b860dUL, 0x8a8a850fUL,
+    0x707090e0UL, 0x3e3e427cUL, 0xb5b5c471UL, 0x6666aaccUL,
+    0x4848d890UL, 0x03030506UL, 0xf6f601f7UL, 0x0e0e121cUL,
+    0x6161a3c2UL, 0x35355f6aUL, 0x5757f9aeUL, 0xb9b9d069UL,
+    0x86869117UL, 0xc1c15899UL, 0x1d1d273aUL, 0x9e9eb927UL,
+    0xe1e138d9UL, 0xf8f813ebUL, 0x9898b32bUL, 0x11113322UL,
+    0x6969bbd2UL, 0xd9d970a9UL, 0x8e8e8907UL, 0x9494a733UL,
+    0x9b9bb62dUL, 0x1e1e223cUL, 0x87879215UL, 0xe9e920c9UL,
+    0xcece4987UL, 0x5555ffaaUL, 0x28287850UL, 0xdfdf7aa5UL,
+    0x8c8c8f03UL, 0xa1a1f859UL, 0x89898009UL, 0x0d0d171aUL,
+    0xbfbfda65UL, 0xe6e631d7UL, 0x4242c684UL, 0x6868b8d0UL,
+    0x4141c382UL, 0x9999b029UL, 0x2d2d775aUL, 0x0f0f111eUL,
+    0xb0b0cb7bUL, 0x5454fca8UL, 0xbbbbd66dUL, 0x16163a2cUL,
+};
+
+#ifndef PELI_TAB
+static const ulong32 Te4_0[] = {
+0x00000063UL, 0x0000007cUL, 0x00000077UL, 0x0000007bUL, 0x000000f2UL, 0x0000006bUL, 0x0000006fUL, 0x000000c5UL, 
+0x00000030UL, 0x00000001UL, 0x00000067UL, 0x0000002bUL, 0x000000feUL, 0x000000d7UL, 0x000000abUL, 0x00000076UL, 
+0x000000caUL, 0x00000082UL, 0x000000c9UL, 0x0000007dUL, 0x000000faUL, 0x00000059UL, 0x00000047UL, 0x000000f0UL, 
+0x000000adUL, 0x000000d4UL, 0x000000a2UL, 0x000000afUL, 0x0000009cUL, 0x000000a4UL, 0x00000072UL, 0x000000c0UL, 
+0x000000b7UL, 0x000000fdUL, 0x00000093UL, 0x00000026UL, 0x00000036UL, 0x0000003fUL, 0x000000f7UL, 0x000000ccUL, 
+0x00000034UL, 0x000000a5UL, 0x000000e5UL, 0x000000f1UL, 0x00000071UL, 0x000000d8UL, 0x00000031UL, 0x00000015UL, 
+0x00000004UL, 0x000000c7UL, 0x00000023UL, 0x000000c3UL, 0x00000018UL, 0x00000096UL, 0x00000005UL, 0x0000009aUL, 
+0x00000007UL, 0x00000012UL, 0x00000080UL, 0x000000e2UL, 0x000000ebUL, 0x00000027UL, 0x000000b2UL, 0x00000075UL, 
+0x00000009UL, 0x00000083UL, 0x0000002cUL, 0x0000001aUL, 0x0000001bUL, 0x0000006eUL, 0x0000005aUL, 0x000000a0UL, 
+0x00000052UL, 0x0000003bUL, 0x000000d6UL, 0x000000b3UL, 0x00000029UL, 0x000000e3UL, 0x0000002fUL, 0x00000084UL, 
+0x00000053UL, 0x000000d1UL, 0x00000000UL, 0x000000edUL, 0x00000020UL, 0x000000fcUL, 0x000000b1UL, 0x0000005bUL, 
+0x0000006aUL, 0x000000cbUL, 0x000000beUL, 0x00000039UL, 0x0000004aUL, 0x0000004cUL, 0x00000058UL, 0x000000cfUL, 
+0x000000d0UL, 0x000000efUL, 0x000000aaUL, 0x000000fbUL, 0x00000043UL, 0x0000004dUL, 0x00000033UL, 0x00000085UL, 
+0x00000045UL, 0x000000f9UL, 0x00000002UL, 0x0000007fUL, 0x00000050UL, 0x0000003cUL, 0x0000009fUL, 0x000000a8UL, 
+0x00000051UL, 0x000000a3UL, 0x00000040UL, 0x0000008fUL, 0x00000092UL, 0x0000009dUL, 0x00000038UL, 0x000000f5UL, 
+0x000000bcUL, 0x000000b6UL, 0x000000daUL, 0x00000021UL, 0x00000010UL, 0x000000ffUL, 0x000000f3UL, 0x000000d2UL, 
+0x000000cdUL, 0x0000000cUL, 0x00000013UL, 0x000000ecUL, 0x0000005fUL, 0x00000097UL, 0x00000044UL, 0x00000017UL, 
+0x000000c4UL, 0x000000a7UL, 0x0000007eUL, 0x0000003dUL, 0x00000064UL, 0x0000005dUL, 0x00000019UL, 0x00000073UL, 
+0x00000060UL, 0x00000081UL, 0x0000004fUL, 0x000000dcUL, 0x00000022UL, 0x0000002aUL, 0x00000090UL, 0x00000088UL, 
+0x00000046UL, 0x000000eeUL, 0x000000b8UL, 0x00000014UL, 0x000000deUL, 0x0000005eUL, 0x0000000bUL, 0x000000dbUL, 
+0x000000e0UL, 0x00000032UL, 0x0000003aUL, 0x0000000aUL, 0x00000049UL, 0x00000006UL, 0x00000024UL, 0x0000005cUL, 
+0x000000c2UL, 0x000000d3UL, 0x000000acUL, 0x00000062UL, 0x00000091UL, 0x00000095UL, 0x000000e4UL, 0x00000079UL, 
+0x000000e7UL, 0x000000c8UL, 0x00000037UL, 0x0000006dUL, 0x0000008dUL, 0x000000d5UL, 0x0000004eUL, 0x000000a9UL, 
+0x0000006cUL, 0x00000056UL, 0x000000f4UL, 0x000000eaUL, 0x00000065UL, 0x0000007aUL, 0x000000aeUL, 0x00000008UL, 
+0x000000baUL, 0x00000078UL, 0x00000025UL, 0x0000002eUL, 0x0000001cUL, 0x000000a6UL, 0x000000b4UL, 0x000000c6UL, 
+0x000000e8UL, 0x000000ddUL, 0x00000074UL, 0x0000001fUL, 0x0000004bUL, 0x000000bdUL, 0x0000008bUL, 0x0000008aUL, 
+0x00000070UL, 0x0000003eUL, 0x000000b5UL, 0x00000066UL, 0x00000048UL, 0x00000003UL, 0x000000f6UL, 0x0000000eUL, 
+0x00000061UL, 0x00000035UL, 0x00000057UL, 0x000000b9UL, 0x00000086UL, 0x000000c1UL, 0x0000001dUL, 0x0000009eUL, 
+0x000000e1UL, 0x000000f8UL, 0x00000098UL, 0x00000011UL, 0x00000069UL, 0x000000d9UL, 0x0000008eUL, 0x00000094UL, 
+0x0000009bUL, 0x0000001eUL, 0x00000087UL, 0x000000e9UL, 0x000000ceUL, 0x00000055UL, 0x00000028UL, 0x000000dfUL, 
+0x0000008cUL, 0x000000a1UL, 0x00000089UL, 0x0000000dUL, 0x000000bfUL, 0x000000e6UL, 0x00000042UL, 0x00000068UL, 
+0x00000041UL, 0x00000099UL, 0x0000002dUL, 0x0000000fUL, 0x000000b0UL, 0x00000054UL, 0x000000bbUL, 0x00000016UL
+};
+
+static const ulong32 Te4_1[] = {
+0x00006300UL, 0x00007c00UL, 0x00007700UL, 0x00007b00UL, 0x0000f200UL, 0x00006b00UL, 0x00006f00UL, 0x0000c500UL, 
+0x00003000UL, 0x00000100UL, 0x00006700UL, 0x00002b00UL, 0x0000fe00UL, 0x0000d700UL, 0x0000ab00UL, 0x00007600UL, 
+0x0000ca00UL, 0x00008200UL, 0x0000c900UL, 0x00007d00UL, 0x0000fa00UL, 0x00005900UL, 0x00004700UL, 0x0000f000UL, 
+0x0000ad00UL, 0x0000d400UL, 0x0000a200UL, 0x0000af00UL, 0x00009c00UL, 0x0000a400UL, 0x00007200UL, 0x0000c000UL, 
+0x0000b700UL, 0x0000fd00UL, 0x00009300UL, 0x00002600UL, 0x00003600UL, 0x00003f00UL, 0x0000f700UL, 0x0000cc00UL, 
+0x00003400UL, 0x0000a500UL, 0x0000e500UL, 0x0000f100UL, 0x00007100UL, 0x0000d800UL, 0x00003100UL, 0x00001500UL, 
+0x00000400UL, 0x0000c700UL, 0x00002300UL, 0x0000c300UL, 0x00001800UL, 0x00009600UL, 0x00000500UL, 0x00009a00UL, 
+0x00000700UL, 0x00001200UL, 0x00008000UL, 0x0000e200UL, 0x0000eb00UL, 0x00002700UL, 0x0000b200UL, 0x00007500UL, 
+0x00000900UL, 0x00008300UL, 0x00002c00UL, 0x00001a00UL, 0x00001b00UL, 0x00006e00UL, 0x00005a00UL, 0x0000a000UL, 
+0x00005200UL, 0x00003b00UL, 0x0000d600UL, 0x0000b300UL, 0x00002900UL, 0x0000e300UL, 0x00002f00UL, 0x00008400UL, 
+0x00005300UL, 0x0000d100UL, 0x00000000UL, 0x0000ed00UL, 0x00002000UL, 0x0000fc00UL, 0x0000b100UL, 0x00005b00UL, 
+0x00006a00UL, 0x0000cb00UL, 0x0000be00UL, 0x00003900UL, 0x00004a00UL, 0x00004c00UL, 0x00005800UL, 0x0000cf00UL, 
+0x0000d000UL, 0x0000ef00UL, 0x0000aa00UL, 0x0000fb00UL, 0x00004300UL, 0x00004d00UL, 0x00003300UL, 0x00008500UL, 
+0x00004500UL, 0x0000f900UL, 0x00000200UL, 0x00007f00UL, 0x00005000UL, 0x00003c00UL, 0x00009f00UL, 0x0000a800UL, 
+0x00005100UL, 0x0000a300UL, 0x00004000UL, 0x00008f00UL, 0x00009200UL, 0x00009d00UL, 0x00003800UL, 0x0000f500UL, 
+0x0000bc00UL, 0x0000b600UL, 0x0000da00UL, 0x00002100UL, 0x00001000UL, 0x0000ff00UL, 0x0000f300UL, 0x0000d200UL, 
+0x0000cd00UL, 0x00000c00UL, 0x00001300UL, 0x0000ec00UL, 0x00005f00UL, 0x00009700UL, 0x00004400UL, 0x00001700UL, 
+0x0000c400UL, 0x0000a700UL, 0x00007e00UL, 0x00003d00UL, 0x00006400UL, 0x00005d00UL, 0x00001900UL, 0x00007300UL, 
+0x00006000UL, 0x00008100UL, 0x00004f00UL, 0x0000dc00UL, 0x00002200UL, 0x00002a00UL, 0x00009000UL, 0x00008800UL, 
+0x00004600UL, 0x0000ee00UL, 0x0000b800UL, 0x00001400UL, 0x0000de00UL, 0x00005e00UL, 0x00000b00UL, 0x0000db00UL, 
+0x0000e000UL, 0x00003200UL, 0x00003a00UL, 0x00000a00UL, 0x00004900UL, 0x00000600UL, 0x00002400UL, 0x00005c00UL, 
+0x0000c200UL, 0x0000d300UL, 0x0000ac00UL, 0x00006200UL, 0x00009100UL, 0x00009500UL, 0x0000e400UL, 0x00007900UL, 
+0x0000e700UL, 0x0000c800UL, 0x00003700UL, 0x00006d00UL, 0x00008d00UL, 0x0000d500UL, 0x00004e00UL, 0x0000a900UL, 
+0x00006c00UL, 0x00005600UL, 0x0000f400UL, 0x0000ea00UL, 0x00006500UL, 0x00007a00UL, 0x0000ae00UL, 0x00000800UL, 
+0x0000ba00UL, 0x00007800UL, 0x00002500UL, 0x00002e00UL, 0x00001c00UL, 0x0000a600UL, 0x0000b400UL, 0x0000c600UL, 
+0x0000e800UL, 0x0000dd00UL, 0x00007400UL, 0x00001f00UL, 0x00004b00UL, 0x0000bd00UL, 0x00008b00UL, 0x00008a00UL, 
+0x00007000UL, 0x00003e00UL, 0x0000b500UL, 0x00006600UL, 0x00004800UL, 0x00000300UL, 0x0000f600UL, 0x00000e00UL, 
+0x00006100UL, 0x00003500UL, 0x00005700UL, 0x0000b900UL, 0x00008600UL, 0x0000c100UL, 0x00001d00UL, 0x00009e00UL, 
+0x0000e100UL, 0x0000f800UL, 0x00009800UL, 0x00001100UL, 0x00006900UL, 0x0000d900UL, 0x00008e00UL, 0x00009400UL, 
+0x00009b00UL, 0x00001e00UL, 0x00008700UL, 0x0000e900UL, 0x0000ce00UL, 0x00005500UL, 0x00002800UL, 0x0000df00UL, 
+0x00008c00UL, 0x0000a100UL, 0x00008900UL, 0x00000d00UL, 0x0000bf00UL, 0x0000e600UL, 0x00004200UL, 0x00006800UL, 
+0x00004100UL, 0x00009900UL, 0x00002d00UL, 0x00000f00UL, 0x0000b000UL, 0x00005400UL, 0x0000bb00UL, 0x00001600UL
+};
+
+static const ulong32 Te4_2[] = {
+0x00630000UL, 0x007c0000UL, 0x00770000UL, 0x007b0000UL, 0x00f20000UL, 0x006b0000UL, 0x006f0000UL, 0x00c50000UL, 
+0x00300000UL, 0x00010000UL, 0x00670000UL, 0x002b0000UL, 0x00fe0000UL, 0x00d70000UL, 0x00ab0000UL, 0x00760000UL, 
+0x00ca0000UL, 0x00820000UL, 0x00c90000UL, 0x007d0000UL, 0x00fa0000UL, 0x00590000UL, 0x00470000UL, 0x00f00000UL, 
+0x00ad0000UL, 0x00d40000UL, 0x00a20000UL, 0x00af0000UL, 0x009c0000UL, 0x00a40000UL, 0x00720000UL, 0x00c00000UL, 
+0x00b70000UL, 0x00fd0000UL, 0x00930000UL, 0x00260000UL, 0x00360000UL, 0x003f0000UL, 0x00f70000UL, 0x00cc0000UL, 
+0x00340000UL, 0x00a50000UL, 0x00e50000UL, 0x00f10000UL, 0x00710000UL, 0x00d80000UL, 0x00310000UL, 0x00150000UL, 
+0x00040000UL, 0x00c70000UL, 0x00230000UL, 0x00c30000UL, 0x00180000UL, 0x00960000UL, 0x00050000UL, 0x009a0000UL, 
+0x00070000UL, 0x00120000UL, 0x00800000UL, 0x00e20000UL, 0x00eb0000UL, 0x00270000UL, 0x00b20000UL, 0x00750000UL, 
+0x00090000UL, 0x00830000UL, 0x002c0000UL, 0x001a0000UL, 0x001b0000UL, 0x006e0000UL, 0x005a0000UL, 0x00a00000UL, 
+0x00520000UL, 0x003b0000UL, 0x00d60000UL, 0x00b30000UL, 0x00290000UL, 0x00e30000UL, 0x002f0000UL, 0x00840000UL, 
+0x00530000UL, 0x00d10000UL, 0x00000000UL, 0x00ed0000UL, 0x00200000UL, 0x00fc0000UL, 0x00b10000UL, 0x005b0000UL, 
+0x006a0000UL, 0x00cb0000UL, 0x00be0000UL, 0x00390000UL, 0x004a0000UL, 0x004c0000UL, 0x00580000UL, 0x00cf0000UL, 
+0x00d00000UL, 0x00ef0000UL, 0x00aa0000UL, 0x00fb0000UL, 0x00430000UL, 0x004d0000UL, 0x00330000UL, 0x00850000UL, 
+0x00450000UL, 0x00f90000UL, 0x00020000UL, 0x007f0000UL, 0x00500000UL, 0x003c0000UL, 0x009f0000UL, 0x00a80000UL, 
+0x00510000UL, 0x00a30000UL, 0x00400000UL, 0x008f0000UL, 0x00920000UL, 0x009d0000UL, 0x00380000UL, 0x00f50000UL, 
+0x00bc0000UL, 0x00b60000UL, 0x00da0000UL, 0x00210000UL, 0x00100000UL, 0x00ff0000UL, 0x00f30000UL, 0x00d20000UL, 
+0x00cd0000UL, 0x000c0000UL, 0x00130000UL, 0x00ec0000UL, 0x005f0000UL, 0x00970000UL, 0x00440000UL, 0x00170000UL, 
+0x00c40000UL, 0x00a70000UL, 0x007e0000UL, 0x003d0000UL, 0x00640000UL, 0x005d0000UL, 0x00190000UL, 0x00730000UL, 
+0x00600000UL, 0x00810000UL, 0x004f0000UL, 0x00dc0000UL, 0x00220000UL, 0x002a0000UL, 0x00900000UL, 0x00880000UL, 
+0x00460000UL, 0x00ee0000UL, 0x00b80000UL, 0x00140000UL, 0x00de0000UL, 0x005e0000UL, 0x000b0000UL, 0x00db0000UL, 
+0x00e00000UL, 0x00320000UL, 0x003a0000UL, 0x000a0000UL, 0x00490000UL, 0x00060000UL, 0x00240000UL, 0x005c0000UL, 
+0x00c20000UL, 0x00d30000UL, 0x00ac0000UL, 0x00620000UL, 0x00910000UL, 0x00950000UL, 0x00e40000UL, 0x00790000UL, 
+0x00e70000UL, 0x00c80000UL, 0x00370000UL, 0x006d0000UL, 0x008d0000UL, 0x00d50000UL, 0x004e0000UL, 0x00a90000UL, 
+0x006c0000UL, 0x00560000UL, 0x00f40000UL, 0x00ea0000UL, 0x00650000UL, 0x007a0000UL, 0x00ae0000UL, 0x00080000UL, 
+0x00ba0000UL, 0x00780000UL, 0x00250000UL, 0x002e0000UL, 0x001c0000UL, 0x00a60000UL, 0x00b40000UL, 0x00c60000UL, 
+0x00e80000UL, 0x00dd0000UL, 0x00740000UL, 0x001f0000UL, 0x004b0000UL, 0x00bd0000UL, 0x008b0000UL, 0x008a0000UL, 
+0x00700000UL, 0x003e0000UL, 0x00b50000UL, 0x00660000UL, 0x00480000UL, 0x00030000UL, 0x00f60000UL, 0x000e0000UL, 
+0x00610000UL, 0x00350000UL, 0x00570000UL, 0x00b90000UL, 0x00860000UL, 0x00c10000UL, 0x001d0000UL, 0x009e0000UL, 
+0x00e10000UL, 0x00f80000UL, 0x00980000UL, 0x00110000UL, 0x00690000UL, 0x00d90000UL, 0x008e0000UL, 0x00940000UL, 
+0x009b0000UL, 0x001e0000UL, 0x00870000UL, 0x00e90000UL, 0x00ce0000UL, 0x00550000UL, 0x00280000UL, 0x00df0000UL, 
+0x008c0000UL, 0x00a10000UL, 0x00890000UL, 0x000d0000UL, 0x00bf0000UL, 0x00e60000UL, 0x00420000UL, 0x00680000UL, 
+0x00410000UL, 0x00990000UL, 0x002d0000UL, 0x000f0000UL, 0x00b00000UL, 0x00540000UL, 0x00bb0000UL, 0x00160000UL
+};
+
+static const ulong32 Te4_3[] = {
+0x63000000UL, 0x7c000000UL, 0x77000000UL, 0x7b000000UL, 0xf2000000UL, 0x6b000000UL, 0x6f000000UL, 0xc5000000UL, 
+0x30000000UL, 0x01000000UL, 0x67000000UL, 0x2b000000UL, 0xfe000000UL, 0xd7000000UL, 0xab000000UL, 0x76000000UL, 
+0xca000000UL, 0x82000000UL, 0xc9000000UL, 0x7d000000UL, 0xfa000000UL, 0x59000000UL, 0x47000000UL, 0xf0000000UL, 
+0xad000000UL, 0xd4000000UL, 0xa2000000UL, 0xaf000000UL, 0x9c000000UL, 0xa4000000UL, 0x72000000UL, 0xc0000000UL, 
+0xb7000000UL, 0xfd000000UL, 0x93000000UL, 0x26000000UL, 0x36000000UL, 0x3f000000UL, 0xf7000000UL, 0xcc000000UL, 
+0x34000000UL, 0xa5000000UL, 0xe5000000UL, 0xf1000000UL, 0x71000000UL, 0xd8000000UL, 0x31000000UL, 0x15000000UL, 
+0x04000000UL, 0xc7000000UL, 0x23000000UL, 0xc3000000UL, 0x18000000UL, 0x96000000UL, 0x05000000UL, 0x9a000000UL, 
+0x07000000UL, 0x12000000UL, 0x80000000UL, 0xe2000000UL, 0xeb000000UL, 0x27000000UL, 0xb2000000UL, 0x75000000UL, 
+0x09000000UL, 0x83000000UL, 0x2c000000UL, 0x1a000000UL, 0x1b000000UL, 0x6e000000UL, 0x5a000000UL, 0xa0000000UL, 
+0x52000000UL, 0x3b000000UL, 0xd6000000UL, 0xb3000000UL, 0x29000000UL, 0xe3000000UL, 0x2f000000UL, 0x84000000UL, 
+0x53000000UL, 0xd1000000UL, 0x00000000UL, 0xed000000UL, 0x20000000UL, 0xfc000000UL, 0xb1000000UL, 0x5b000000UL, 
+0x6a000000UL, 0xcb000000UL, 0xbe000000UL, 0x39000000UL, 0x4a000000UL, 0x4c000000UL, 0x58000000UL, 0xcf000000UL, 
+0xd0000000UL, 0xef000000UL, 0xaa000000UL, 0xfb000000UL, 0x43000000UL, 0x4d000000UL, 0x33000000UL, 0x85000000UL, 
+0x45000000UL, 0xf9000000UL, 0x02000000UL, 0x7f000000UL, 0x50000000UL, 0x3c000000UL, 0x9f000000UL, 0xa8000000UL, 
+0x51000000UL, 0xa3000000UL, 0x40000000UL, 0x8f000000UL, 0x92000000UL, 0x9d000000UL, 0x38000000UL, 0xf5000000UL, 
+0xbc000000UL, 0xb6000000UL, 0xda000000UL, 0x21000000UL, 0x10000000UL, 0xff000000UL, 0xf3000000UL, 0xd2000000UL, 
+0xcd000000UL, 0x0c000000UL, 0x13000000UL, 0xec000000UL, 0x5f000000UL, 0x97000000UL, 0x44000000UL, 0x17000000UL, 
+0xc4000000UL, 0xa7000000UL, 0x7e000000UL, 0x3d000000UL, 0x64000000UL, 0x5d000000UL, 0x19000000UL, 0x73000000UL, 
+0x60000000UL, 0x81000000UL, 0x4f000000UL, 0xdc000000UL, 0x22000000UL, 0x2a000000UL, 0x90000000UL, 0x88000000UL, 
+0x46000000UL, 0xee000000UL, 0xb8000000UL, 0x14000000UL, 0xde000000UL, 0x5e000000UL, 0x0b000000UL, 0xdb000000UL, 
+0xe0000000UL, 0x32000000UL, 0x3a000000UL, 0x0a000000UL, 0x49000000UL, 0x06000000UL, 0x24000000UL, 0x5c000000UL, 
+0xc2000000UL, 0xd3000000UL, 0xac000000UL, 0x62000000UL, 0x91000000UL, 0x95000000UL, 0xe4000000UL, 0x79000000UL, 
+0xe7000000UL, 0xc8000000UL, 0x37000000UL, 0x6d000000UL, 0x8d000000UL, 0xd5000000UL, 0x4e000000UL, 0xa9000000UL, 
+0x6c000000UL, 0x56000000UL, 0xf4000000UL, 0xea000000UL, 0x65000000UL, 0x7a000000UL, 0xae000000UL, 0x08000000UL, 
+0xba000000UL, 0x78000000UL, 0x25000000UL, 0x2e000000UL, 0x1c000000UL, 0xa6000000UL, 0xb4000000UL, 0xc6000000UL, 
+0xe8000000UL, 0xdd000000UL, 0x74000000UL, 0x1f000000UL, 0x4b000000UL, 0xbd000000UL, 0x8b000000UL, 0x8a000000UL, 
+0x70000000UL, 0x3e000000UL, 0xb5000000UL, 0x66000000UL, 0x48000000UL, 0x03000000UL, 0xf6000000UL, 0x0e000000UL, 
+0x61000000UL, 0x35000000UL, 0x57000000UL, 0xb9000000UL, 0x86000000UL, 0xc1000000UL, 0x1d000000UL, 0x9e000000UL, 
+0xe1000000UL, 0xf8000000UL, 0x98000000UL, 0x11000000UL, 0x69000000UL, 0xd9000000UL, 0x8e000000UL, 0x94000000UL, 
+0x9b000000UL, 0x1e000000UL, 0x87000000UL, 0xe9000000UL, 0xce000000UL, 0x55000000UL, 0x28000000UL, 0xdf000000UL, 
+0x8c000000UL, 0xa1000000UL, 0x89000000UL, 0x0d000000UL, 0xbf000000UL, 0xe6000000UL, 0x42000000UL, 0x68000000UL, 
+0x41000000UL, 0x99000000UL, 0x2d000000UL, 0x0f000000UL, 0xb0000000UL, 0x54000000UL, 0xbb000000UL, 0x16000000UL
+};
+#endif /* pelimac */
+
+#ifndef ENCRYPT_ONLY
+
+static const ulong32 TD1[256] = {
+    0x5051f4a7UL, 0x537e4165UL, 0xc31a17a4UL, 0x963a275eUL,
+    0xcb3bab6bUL, 0xf11f9d45UL, 0xabacfa58UL, 0x934be303UL,
+    0x552030faUL, 0xf6ad766dUL, 0x9188cc76UL, 0x25f5024cUL,
+    0xfc4fe5d7UL, 0xd7c52acbUL, 0x80263544UL, 0x8fb562a3UL,
+    0x49deb15aUL, 0x6725ba1bUL, 0x9845ea0eUL, 0xe15dfec0UL,
+    0x02c32f75UL, 0x12814cf0UL, 0xa38d4697UL, 0xc66bd3f9UL,
+    0xe7038f5fUL, 0x9515929cUL, 0xebbf6d7aUL, 0xda955259UL,
+    0x2dd4be83UL, 0xd3587421UL, 0x2949e069UL, 0x448ec9c8UL,
+    0x6a75c289UL, 0x78f48e79UL, 0x6b99583eUL, 0xdd27b971UL,
+    0xb6bee14fUL, 0x17f088adUL, 0x66c920acUL, 0xb47dce3aUL,
+    0x1863df4aUL, 0x82e51a31UL, 0x60975133UL, 0x4562537fUL,
+    0xe0b16477UL, 0x84bb6baeUL, 0x1cfe81a0UL, 0x94f9082bUL,
+    0x58704868UL, 0x198f45fdUL, 0x8794de6cUL, 0xb7527bf8UL,
+    0x23ab73d3UL, 0xe2724b02UL, 0x57e31f8fUL, 0x2a6655abUL,
+    0x07b2eb28UL, 0x032fb5c2UL, 0x9a86c57bUL, 0xa5d33708UL,
+    0xf2302887UL, 0xb223bfa5UL, 0xba02036aUL, 0x5ced1682UL,
+    0x2b8acf1cUL, 0x92a779b4UL, 0xf0f307f2UL, 0xa14e69e2UL,
+    0xcd65daf4UL, 0xd50605beUL, 0x1fd13462UL, 0x8ac4a6feUL,
+    0x9d342e53UL, 0xa0a2f355UL, 0x32058ae1UL, 0x75a4f6ebUL,
+    0x390b83ecUL, 0xaa4060efUL, 0x065e719fUL, 0x51bd6e10UL,
+    0xf93e218aUL, 0x3d96dd06UL, 0xaedd3e05UL, 0x464de6bdUL,
+    0xb591548dUL, 0x0571c45dUL, 0x6f0406d4UL, 0xff605015UL,
+    0x241998fbUL, 0x97d6bde9UL, 0xcc894043UL, 0x7767d99eUL,
+    0xbdb0e842UL, 0x8807898bUL, 0x38e7195bUL, 0xdb79c8eeUL,
+    0x47a17c0aUL, 0xe97c420fUL, 0xc9f8841eUL, 0x00000000UL,
+    0x83098086UL, 0x48322bedUL, 0xac1e1170UL, 0x4e6c5a72UL,
+    0xfbfd0effUL, 0x560f8538UL, 0x1e3daed5UL, 0x27362d39UL,
+    0x640a0fd9UL, 0x21685ca6UL, 0xd19b5b54UL, 0x3a24362eUL,
+    0xb10c0a67UL, 0x0f9357e7UL, 0xd2b4ee96UL, 0x9e1b9b91UL,
+    0x4f80c0c5UL, 0xa261dc20UL, 0x695a774bUL, 0x161c121aUL,
+    0x0ae293baUL, 0xe5c0a02aUL, 0x433c22e0UL, 0x1d121b17UL,
+    0x0b0e090dUL, 0xadf28bc7UL, 0xb92db6a8UL, 0xc8141ea9UL,
+    0x8557f119UL, 0x4caf7507UL, 0xbbee99ddUL, 0xfda37f60UL,
+    0x9ff70126UL, 0xbc5c72f5UL, 0xc544663bUL, 0x345bfb7eUL,
+    0x768b4329UL, 0xdccb23c6UL, 0x68b6edfcUL, 0x63b8e4f1UL,
+    0xcad731dcUL, 0x10426385UL, 0x40139722UL, 0x2084c611UL,
+    0x7d854a24UL, 0xf8d2bb3dUL, 0x11aef932UL, 0x6dc729a1UL,
+    0x4b1d9e2fUL, 0xf3dcb230UL, 0xec0d8652UL, 0xd077c1e3UL,
+    0x6c2bb316UL, 0x99a970b9UL, 0xfa119448UL, 0x2247e964UL,
+    0xc4a8fc8cUL, 0x1aa0f03fUL, 0xd8567d2cUL, 0xef223390UL,
+    0xc787494eUL, 0xc1d938d1UL, 0xfe8ccaa2UL, 0x3698d40bUL,
+    0xcfa6f581UL, 0x28a57adeUL, 0x26dab78eUL, 0xa43fadbfUL,
+    0xe42c3a9dUL, 0x0d507892UL, 0x9b6a5fccUL, 0x62547e46UL,
+    0xc2f68d13UL, 0xe890d8b8UL, 0x5e2e39f7UL, 0xf582c3afUL,
+    0xbe9f5d80UL, 0x7c69d093UL, 0xa96fd52dUL, 0xb3cf2512UL,
+    0x3bc8ac99UL, 0xa710187dUL, 0x6ee89c63UL, 0x7bdb3bbbUL,
+    0x09cd2678UL, 0xf46e5918UL, 0x01ec9ab7UL, 0xa8834f9aUL,
+    0x65e6956eUL, 0x7eaaffe6UL, 0x0821bccfUL, 0xe6ef15e8UL,
+    0xd9bae79bUL, 0xce4a6f36UL, 0xd4ea9f09UL, 0xd629b07cUL,
+    0xaf31a4b2UL, 0x312a3f23UL, 0x30c6a594UL, 0xc035a266UL,
+    0x37744ebcUL, 0xa6fc82caUL, 0xb0e090d0UL, 0x1533a7d8UL,
+    0x4af10498UL, 0xf741ecdaUL, 0x0e7fcd50UL, 0x2f1791f6UL,
+    0x8d764dd6UL, 0x4d43efb0UL, 0x54ccaa4dUL, 0xdfe49604UL,
+    0xe39ed1b5UL, 0x1b4c6a88UL, 0xb8c12c1fUL, 0x7f466551UL,
+    0x049d5eeaUL, 0x5d018c35UL, 0x73fa8774UL, 0x2efb0b41UL,
+    0x5ab3671dUL, 0x5292dbd2UL, 0x33e91056UL, 0x136dd647UL,
+    0x8c9ad761UL, 0x7a37a10cUL, 0x8e59f814UL, 0x89eb133cUL,
+    0xeecea927UL, 0x35b761c9UL, 0xede11ce5UL, 0x3c7a47b1UL,
+    0x599cd2dfUL, 0x3f55f273UL, 0x791814ceUL, 0xbf73c737UL,
+    0xea53f7cdUL, 0x5b5ffdaaUL, 0x14df3d6fUL, 0x867844dbUL,
+    0x81caaff3UL, 0x3eb968c4UL, 0x2c382434UL, 0x5fc2a340UL,
+    0x72161dc3UL, 0x0cbce225UL, 0x8b283c49UL, 0x41ff0d95UL,
+    0x7139a801UL, 0xde080cb3UL, 0x9cd8b4e4UL, 0x906456c1UL,
+    0x617bcb84UL, 0x70d532b6UL, 0x74486c5cUL, 0x42d0b857UL,
+};
+static const ulong32 TD2[256] = {
+    0xa75051f4UL, 0x65537e41UL, 0xa4c31a17UL, 0x5e963a27UL,
+    0x6bcb3babUL, 0x45f11f9dUL, 0x58abacfaUL, 0x03934be3UL,
+    0xfa552030UL, 0x6df6ad76UL, 0x769188ccUL, 0x4c25f502UL,
+    0xd7fc4fe5UL, 0xcbd7c52aUL, 0x44802635UL, 0xa38fb562UL,
+    0x5a49deb1UL, 0x1b6725baUL, 0x0e9845eaUL, 0xc0e15dfeUL,
+    0x7502c32fUL, 0xf012814cUL, 0x97a38d46UL, 0xf9c66bd3UL,
+    0x5fe7038fUL, 0x9c951592UL, 0x7aebbf6dUL, 0x59da9552UL,
+    0x832dd4beUL, 0x21d35874UL, 0x692949e0UL, 0xc8448ec9UL,
+    0x896a75c2UL, 0x7978f48eUL, 0x3e6b9958UL, 0x71dd27b9UL,
+    0x4fb6bee1UL, 0xad17f088UL, 0xac66c920UL, 0x3ab47dceUL,
+    0x4a1863dfUL, 0x3182e51aUL, 0x33609751UL, 0x7f456253UL,
+    0x77e0b164UL, 0xae84bb6bUL, 0xa01cfe81UL, 0x2b94f908UL,
+    0x68587048UL, 0xfd198f45UL, 0x6c8794deUL, 0xf8b7527bUL,
+    0xd323ab73UL, 0x02e2724bUL, 0x8f57e31fUL, 0xab2a6655UL,
+    0x2807b2ebUL, 0xc2032fb5UL, 0x7b9a86c5UL, 0x08a5d337UL,
+    0x87f23028UL, 0xa5b223bfUL, 0x6aba0203UL, 0x825ced16UL,
+    0x1c2b8acfUL, 0xb492a779UL, 0xf2f0f307UL, 0xe2a14e69UL,
+    0xf4cd65daUL, 0xbed50605UL, 0x621fd134UL, 0xfe8ac4a6UL,
+    0x539d342eUL, 0x55a0a2f3UL, 0xe132058aUL, 0xeb75a4f6UL,
+    0xec390b83UL, 0xefaa4060UL, 0x9f065e71UL, 0x1051bd6eUL,
+    0x8af93e21UL, 0x063d96ddUL, 0x05aedd3eUL, 0xbd464de6UL,
+    0x8db59154UL, 0x5d0571c4UL, 0xd46f0406UL, 0x15ff6050UL,
+    0xfb241998UL, 0xe997d6bdUL, 0x43cc8940UL, 0x9e7767d9UL,
+    0x42bdb0e8UL, 0x8b880789UL, 0x5b38e719UL, 0xeedb79c8UL,
+    0x0a47a17cUL, 0x0fe97c42UL, 0x1ec9f884UL, 0x00000000UL,
+    0x86830980UL, 0xed48322bUL, 0x70ac1e11UL, 0x724e6c5aUL,
+    0xfffbfd0eUL, 0x38560f85UL, 0xd51e3daeUL, 0x3927362dUL,
+    0xd9640a0fUL, 0xa621685cUL, 0x54d19b5bUL, 0x2e3a2436UL,
+    0x67b10c0aUL, 0xe70f9357UL, 0x96d2b4eeUL, 0x919e1b9bUL,
+    0xc54f80c0UL, 0x20a261dcUL, 0x4b695a77UL, 0x1a161c12UL,
+    0xba0ae293UL, 0x2ae5c0a0UL, 0xe0433c22UL, 0x171d121bUL,
+    0x0d0b0e09UL, 0xc7adf28bUL, 0xa8b92db6UL, 0xa9c8141eUL,
+    0x198557f1UL, 0x074caf75UL, 0xddbbee99UL, 0x60fda37fUL,
+    0x269ff701UL, 0xf5bc5c72UL, 0x3bc54466UL, 0x7e345bfbUL,
+    0x29768b43UL, 0xc6dccb23UL, 0xfc68b6edUL, 0xf163b8e4UL,
+    0xdccad731UL, 0x85104263UL, 0x22401397UL, 0x112084c6UL,
+    0x247d854aUL, 0x3df8d2bbUL, 0x3211aef9UL, 0xa16dc729UL,
+    0x2f4b1d9eUL, 0x30f3dcb2UL, 0x52ec0d86UL, 0xe3d077c1UL,
+    0x166c2bb3UL, 0xb999a970UL, 0x48fa1194UL, 0x642247e9UL,
+    0x8cc4a8fcUL, 0x3f1aa0f0UL, 0x2cd8567dUL, 0x90ef2233UL,
+    0x4ec78749UL, 0xd1c1d938UL, 0xa2fe8ccaUL, 0x0b3698d4UL,
+    0x81cfa6f5UL, 0xde28a57aUL, 0x8e26dab7UL, 0xbfa43fadUL,
+    0x9de42c3aUL, 0x920d5078UL, 0xcc9b6a5fUL, 0x4662547eUL,
+    0x13c2f68dUL, 0xb8e890d8UL, 0xf75e2e39UL, 0xaff582c3UL,
+    0x80be9f5dUL, 0x937c69d0UL, 0x2da96fd5UL, 0x12b3cf25UL,
+    0x993bc8acUL, 0x7da71018UL, 0x636ee89cUL, 0xbb7bdb3bUL,
+    0x7809cd26UL, 0x18f46e59UL, 0xb701ec9aUL, 0x9aa8834fUL,
+    0x6e65e695UL, 0xe67eaaffUL, 0xcf0821bcUL, 0xe8e6ef15UL,
+    0x9bd9bae7UL, 0x36ce4a6fUL, 0x09d4ea9fUL, 0x7cd629b0UL,
+    0xb2af31a4UL, 0x23312a3fUL, 0x9430c6a5UL, 0x66c035a2UL,
+    0xbc37744eUL, 0xcaa6fc82UL, 0xd0b0e090UL, 0xd81533a7UL,
+    0x984af104UL, 0xdaf741ecUL, 0x500e7fcdUL, 0xf62f1791UL,
+    0xd68d764dUL, 0xb04d43efUL, 0x4d54ccaaUL, 0x04dfe496UL,
+    0xb5e39ed1UL, 0x881b4c6aUL, 0x1fb8c12cUL, 0x517f4665UL,
+    0xea049d5eUL, 0x355d018cUL, 0x7473fa87UL, 0x412efb0bUL,
+    0x1d5ab367UL, 0xd25292dbUL, 0x5633e910UL, 0x47136dd6UL,
+    0x618c9ad7UL, 0x0c7a37a1UL, 0x148e59f8UL, 0x3c89eb13UL,
+    0x27eecea9UL, 0xc935b761UL, 0xe5ede11cUL, 0xb13c7a47UL,
+    0xdf599cd2UL, 0x733f55f2UL, 0xce791814UL, 0x37bf73c7UL,
+    0xcdea53f7UL, 0xaa5b5ffdUL, 0x6f14df3dUL, 0xdb867844UL,
+    0xf381caafUL, 0xc43eb968UL, 0x342c3824UL, 0x405fc2a3UL,
+    0xc372161dUL, 0x250cbce2UL, 0x498b283cUL, 0x9541ff0dUL,
+    0x017139a8UL, 0xb3de080cUL, 0xe49cd8b4UL, 0xc1906456UL,
+    0x84617bcbUL, 0xb670d532UL, 0x5c74486cUL, 0x5742d0b8UL,
+};
+static const ulong32 TD3[256] = {
+    0xf4a75051UL, 0x4165537eUL, 0x17a4c31aUL, 0x275e963aUL,
+    0xab6bcb3bUL, 0x9d45f11fUL, 0xfa58abacUL, 0xe303934bUL,
+    0x30fa5520UL, 0x766df6adUL, 0xcc769188UL, 0x024c25f5UL,
+    0xe5d7fc4fUL, 0x2acbd7c5UL, 0x35448026UL, 0x62a38fb5UL,
+    0xb15a49deUL, 0xba1b6725UL, 0xea0e9845UL, 0xfec0e15dUL,
+    0x2f7502c3UL, 0x4cf01281UL, 0x4697a38dUL, 0xd3f9c66bUL,
+    0x8f5fe703UL, 0x929c9515UL, 0x6d7aebbfUL, 0x5259da95UL,
+    0xbe832dd4UL, 0x7421d358UL, 0xe0692949UL, 0xc9c8448eUL,
+    0xc2896a75UL, 0x8e7978f4UL, 0x583e6b99UL, 0xb971dd27UL,
+    0xe14fb6beUL, 0x88ad17f0UL, 0x20ac66c9UL, 0xce3ab47dUL,
+    0xdf4a1863UL, 0x1a3182e5UL, 0x51336097UL, 0x537f4562UL,
+    0x6477e0b1UL, 0x6bae84bbUL, 0x81a01cfeUL, 0x082b94f9UL,
+    0x48685870UL, 0x45fd198fUL, 0xde6c8794UL, 0x7bf8b752UL,
+    0x73d323abUL, 0x4b02e272UL, 0x1f8f57e3UL, 0x55ab2a66UL,
+    0xeb2807b2UL, 0xb5c2032fUL, 0xc57b9a86UL, 0x3708a5d3UL,
+    0x2887f230UL, 0xbfa5b223UL, 0x036aba02UL, 0x16825cedUL,
+    0xcf1c2b8aUL, 0x79b492a7UL, 0x07f2f0f3UL, 0x69e2a14eUL,
+    0xdaf4cd65UL, 0x05bed506UL, 0x34621fd1UL, 0xa6fe8ac4UL,
+    0x2e539d34UL, 0xf355a0a2UL, 0x8ae13205UL, 0xf6eb75a4UL,
+    0x83ec390bUL, 0x60efaa40UL, 0x719f065eUL, 0x6e1051bdUL,
+    0x218af93eUL, 0xdd063d96UL, 0x3e05aeddUL, 0xe6bd464dUL,
+    0x548db591UL, 0xc45d0571UL, 0x06d46f04UL, 0x5015ff60UL,
+    0x98fb2419UL, 0xbde997d6UL, 0x4043cc89UL, 0xd99e7767UL,
+    0xe842bdb0UL, 0x898b8807UL, 0x195b38e7UL, 0xc8eedb79UL,
+    0x7c0a47a1UL, 0x420fe97cUL, 0x841ec9f8UL, 0x00000000UL,
+    0x80868309UL, 0x2bed4832UL, 0x1170ac1eUL, 0x5a724e6cUL,
+    0x0efffbfdUL, 0x8538560fUL, 0xaed51e3dUL, 0x2d392736UL,
+    0x0fd9640aUL, 0x5ca62168UL, 0x5b54d19bUL, 0x362e3a24UL,
+    0x0a67b10cUL, 0x57e70f93UL, 0xee96d2b4UL, 0x9b919e1bUL,
+    0xc0c54f80UL, 0xdc20a261UL, 0x774b695aUL, 0x121a161cUL,
+    0x93ba0ae2UL, 0xa02ae5c0UL, 0x22e0433cUL, 0x1b171d12UL,
+    0x090d0b0eUL, 0x8bc7adf2UL, 0xb6a8b92dUL, 0x1ea9c814UL,
+    0xf1198557UL, 0x75074cafUL, 0x99ddbbeeUL, 0x7f60fda3UL,
+    0x01269ff7UL, 0x72f5bc5cUL, 0x663bc544UL, 0xfb7e345bUL,
+    0x4329768bUL, 0x23c6dccbUL, 0xedfc68b6UL, 0xe4f163b8UL,
+    0x31dccad7UL, 0x63851042UL, 0x97224013UL, 0xc6112084UL,
+    0x4a247d85UL, 0xbb3df8d2UL, 0xf93211aeUL, 0x29a16dc7UL,
+    0x9e2f4b1dUL, 0xb230f3dcUL, 0x8652ec0dUL, 0xc1e3d077UL,
+    0xb3166c2bUL, 0x70b999a9UL, 0x9448fa11UL, 0xe9642247UL,
+    0xfc8cc4a8UL, 0xf03f1aa0UL, 0x7d2cd856UL, 0x3390ef22UL,
+    0x494ec787UL, 0x38d1c1d9UL, 0xcaa2fe8cUL, 0xd40b3698UL,
+    0xf581cfa6UL, 0x7ade28a5UL, 0xb78e26daUL, 0xadbfa43fUL,
+    0x3a9de42cUL, 0x78920d50UL, 0x5fcc9b6aUL, 0x7e466254UL,
+    0x8d13c2f6UL, 0xd8b8e890UL, 0x39f75e2eUL, 0xc3aff582UL,
+    0x5d80be9fUL, 0xd0937c69UL, 0xd52da96fUL, 0x2512b3cfUL,
+    0xac993bc8UL, 0x187da710UL, 0x9c636ee8UL, 0x3bbb7bdbUL,
+    0x267809cdUL, 0x5918f46eUL, 0x9ab701ecUL, 0x4f9aa883UL,
+    0x956e65e6UL, 0xffe67eaaUL, 0xbccf0821UL, 0x15e8e6efUL,
+    0xe79bd9baUL, 0x6f36ce4aUL, 0x9f09d4eaUL, 0xb07cd629UL,
+    0xa4b2af31UL, 0x3f23312aUL, 0xa59430c6UL, 0xa266c035UL,
+    0x4ebc3774UL, 0x82caa6fcUL, 0x90d0b0e0UL, 0xa7d81533UL,
+    0x04984af1UL, 0xecdaf741UL, 0xcd500e7fUL, 0x91f62f17UL,
+    0x4dd68d76UL, 0xefb04d43UL, 0xaa4d54ccUL, 0x9604dfe4UL,
+    0xd1b5e39eUL, 0x6a881b4cUL, 0x2c1fb8c1UL, 0x65517f46UL,
+    0x5eea049dUL, 0x8c355d01UL, 0x877473faUL, 0x0b412efbUL,
+    0x671d5ab3UL, 0xdbd25292UL, 0x105633e9UL, 0xd647136dUL,
+    0xd7618c9aUL, 0xa10c7a37UL, 0xf8148e59UL, 0x133c89ebUL,
+    0xa927eeceUL, 0x61c935b7UL, 0x1ce5ede1UL, 0x47b13c7aUL,
+    0xd2df599cUL, 0xf2733f55UL, 0x14ce7918UL, 0xc737bf73UL,
+    0xf7cdea53UL, 0xfdaa5b5fUL, 0x3d6f14dfUL, 0x44db8678UL,
+    0xaff381caUL, 0x68c43eb9UL, 0x24342c38UL, 0xa3405fc2UL,
+    0x1dc37216UL, 0xe2250cbcUL, 0x3c498b28UL, 0x0d9541ffUL,
+    0xa8017139UL, 0x0cb3de08UL, 0xb4e49cd8UL, 0x56c19064UL,
+    0xcb84617bUL, 0x32b670d5UL, 0x6c5c7448UL, 0xb85742d0UL,
+};
+
+static const ulong32 Tks0[] = {
+0x00000000UL, 0x0e090d0bUL, 0x1c121a16UL, 0x121b171dUL, 0x3824342cUL, 0x362d3927UL, 0x24362e3aUL, 0x2a3f2331UL, 
+0x70486858UL, 0x7e416553UL, 0x6c5a724eUL, 0x62537f45UL, 0x486c5c74UL, 0x4665517fUL, 0x547e4662UL, 0x5a774b69UL, 
+0xe090d0b0UL, 0xee99ddbbUL, 0xfc82caa6UL, 0xf28bc7adUL, 0xd8b4e49cUL, 0xd6bde997UL, 0xc4a6fe8aUL, 0xcaaff381UL, 
+0x90d8b8e8UL, 0x9ed1b5e3UL, 0x8ccaa2feUL, 0x82c3aff5UL, 0xa8fc8cc4UL, 0xa6f581cfUL, 0xb4ee96d2UL, 0xbae79bd9UL, 
+0xdb3bbb7bUL, 0xd532b670UL, 0xc729a16dUL, 0xc920ac66UL, 0xe31f8f57UL, 0xed16825cUL, 0xff0d9541UL, 0xf104984aUL, 
+0xab73d323UL, 0xa57ade28UL, 0xb761c935UL, 0xb968c43eUL, 0x9357e70fUL, 0x9d5eea04UL, 0x8f45fd19UL, 0x814cf012UL, 
+0x3bab6bcbUL, 0x35a266c0UL, 0x27b971ddUL, 0x29b07cd6UL, 0x038f5fe7UL, 0x0d8652ecUL, 0x1f9d45f1UL, 0x119448faUL, 
+0x4be30393UL, 0x45ea0e98UL, 0x57f11985UL, 0x59f8148eUL, 0x73c737bfUL, 0x7dce3ab4UL, 0x6fd52da9UL, 0x61dc20a2UL, 
+0xad766df6UL, 0xa37f60fdUL, 0xb16477e0UL, 0xbf6d7aebUL, 0x955259daUL, 0x9b5b54d1UL, 0x894043ccUL, 0x87494ec7UL, 
+0xdd3e05aeUL, 0xd33708a5UL, 0xc12c1fb8UL, 0xcf2512b3UL, 0xe51a3182UL, 0xeb133c89UL, 0xf9082b94UL, 0xf701269fUL, 
+0x4de6bd46UL, 0x43efb04dUL, 0x51f4a750UL, 0x5ffdaa5bUL, 0x75c2896aUL, 0x7bcb8461UL, 0x69d0937cUL, 0x67d99e77UL, 
+0x3daed51eUL, 0x33a7d815UL, 0x21bccf08UL, 0x2fb5c203UL, 0x058ae132UL, 0x0b83ec39UL, 0x1998fb24UL, 0x1791f62fUL, 
+0x764dd68dUL, 0x7844db86UL, 0x6a5fcc9bUL, 0x6456c190UL, 0x4e69e2a1UL, 0x4060efaaUL, 0x527bf8b7UL, 0x5c72f5bcUL, 
+0x0605bed5UL, 0x080cb3deUL, 0x1a17a4c3UL, 0x141ea9c8UL, 0x3e218af9UL, 0x302887f2UL, 0x223390efUL, 0x2c3a9de4UL, 
+0x96dd063dUL, 0x98d40b36UL, 0x8acf1c2bUL, 0x84c61120UL, 0xaef93211UL, 0xa0f03f1aUL, 0xb2eb2807UL, 0xbce2250cUL, 
+0xe6956e65UL, 0xe89c636eUL, 0xfa877473UL, 0xf48e7978UL, 0xdeb15a49UL, 0xd0b85742UL, 0xc2a3405fUL, 0xccaa4d54UL, 
+0x41ecdaf7UL, 0x4fe5d7fcUL, 0x5dfec0e1UL, 0x53f7cdeaUL, 0x79c8eedbUL, 0x77c1e3d0UL, 0x65daf4cdUL, 0x6bd3f9c6UL, 
+0x31a4b2afUL, 0x3fadbfa4UL, 0x2db6a8b9UL, 0x23bfa5b2UL, 0x09808683UL, 0x07898b88UL, 0x15929c95UL, 0x1b9b919eUL, 
+0xa17c0a47UL, 0xaf75074cUL, 0xbd6e1051UL, 0xb3671d5aUL, 0x99583e6bUL, 0x97513360UL, 0x854a247dUL, 0x8b432976UL, 
+0xd134621fUL, 0xdf3d6f14UL, 0xcd267809UL, 0xc32f7502UL, 0xe9105633UL, 0xe7195b38UL, 0xf5024c25UL, 0xfb0b412eUL, 
+0x9ad7618cUL, 0x94de6c87UL, 0x86c57b9aUL, 0x88cc7691UL, 0xa2f355a0UL, 0xacfa58abUL, 0xbee14fb6UL, 0xb0e842bdUL, 
+0xea9f09d4UL, 0xe49604dfUL, 0xf68d13c2UL, 0xf8841ec9UL, 0xd2bb3df8UL, 0xdcb230f3UL, 0xcea927eeUL, 0xc0a02ae5UL, 
+0x7a47b13cUL, 0x744ebc37UL, 0x6655ab2aUL, 0x685ca621UL, 0x42638510UL, 0x4c6a881bUL, 0x5e719f06UL, 0x5078920dUL, 
+0x0a0fd964UL, 0x0406d46fUL, 0x161dc372UL, 0x1814ce79UL, 0x322bed48UL, 0x3c22e043UL, 0x2e39f75eUL, 0x2030fa55UL, 
+0xec9ab701UL, 0xe293ba0aUL, 0xf088ad17UL, 0xfe81a01cUL, 0xd4be832dUL, 0xdab78e26UL, 0xc8ac993bUL, 0xc6a59430UL, 
+0x9cd2df59UL, 0x92dbd252UL, 0x80c0c54fUL, 0x8ec9c844UL, 0xa4f6eb75UL, 0xaaffe67eUL, 0xb8e4f163UL, 0xb6edfc68UL, 
+0x0c0a67b1UL, 0x02036abaUL, 0x10187da7UL, 0x1e1170acUL, 0x342e539dUL, 0x3a275e96UL, 0x283c498bUL, 0x26354480UL, 
+0x7c420fe9UL, 0x724b02e2UL, 0x605015ffUL, 0x6e5918f4UL, 0x44663bc5UL, 0x4a6f36ceUL, 0x587421d3UL, 0x567d2cd8UL, 
+0x37a10c7aUL, 0x39a80171UL, 0x2bb3166cUL, 0x25ba1b67UL, 0x0f853856UL, 0x018c355dUL, 0x13972240UL, 0x1d9e2f4bUL, 
+0x47e96422UL, 0x49e06929UL, 0x5bfb7e34UL, 0x55f2733fUL, 0x7fcd500eUL, 0x71c45d05UL, 0x63df4a18UL, 0x6dd64713UL, 
+0xd731dccaUL, 0xd938d1c1UL, 0xcb23c6dcUL, 0xc52acbd7UL, 0xef15e8e6UL, 0xe11ce5edUL, 0xf307f2f0UL, 0xfd0efffbUL, 
+0xa779b492UL, 0xa970b999UL, 0xbb6bae84UL, 0xb562a38fUL, 0x9f5d80beUL, 0x91548db5UL, 0x834f9aa8UL, 0x8d4697a3UL
+};
+
+static const ulong32 Tks1[] = {
+0x00000000UL, 0x0b0e090dUL, 0x161c121aUL, 0x1d121b17UL, 0x2c382434UL, 0x27362d39UL, 0x3a24362eUL, 0x312a3f23UL, 
+0x58704868UL, 0x537e4165UL, 0x4e6c5a72UL, 0x4562537fUL, 0x74486c5cUL, 0x7f466551UL, 0x62547e46UL, 0x695a774bUL, 
+0xb0e090d0UL, 0xbbee99ddUL, 0xa6fc82caUL, 0xadf28bc7UL, 0x9cd8b4e4UL, 0x97d6bde9UL, 0x8ac4a6feUL, 0x81caaff3UL, 
+0xe890d8b8UL, 0xe39ed1b5UL, 0xfe8ccaa2UL, 0xf582c3afUL, 0xc4a8fc8cUL, 0xcfa6f581UL, 0xd2b4ee96UL, 0xd9bae79bUL, 
+0x7bdb3bbbUL, 0x70d532b6UL, 0x6dc729a1UL, 0x66c920acUL, 0x57e31f8fUL, 0x5ced1682UL, 0x41ff0d95UL, 0x4af10498UL, 
+0x23ab73d3UL, 0x28a57adeUL, 0x35b761c9UL, 0x3eb968c4UL, 0x0f9357e7UL, 0x049d5eeaUL, 0x198f45fdUL, 0x12814cf0UL, 
+0xcb3bab6bUL, 0xc035a266UL, 0xdd27b971UL, 0xd629b07cUL, 0xe7038f5fUL, 0xec0d8652UL, 0xf11f9d45UL, 0xfa119448UL, 
+0x934be303UL, 0x9845ea0eUL, 0x8557f119UL, 0x8e59f814UL, 0xbf73c737UL, 0xb47dce3aUL, 0xa96fd52dUL, 0xa261dc20UL, 
+0xf6ad766dUL, 0xfda37f60UL, 0xe0b16477UL, 0xebbf6d7aUL, 0xda955259UL, 0xd19b5b54UL, 0xcc894043UL, 0xc787494eUL, 
+0xaedd3e05UL, 0xa5d33708UL, 0xb8c12c1fUL, 0xb3cf2512UL, 0x82e51a31UL, 0x89eb133cUL, 0x94f9082bUL, 0x9ff70126UL, 
+0x464de6bdUL, 0x4d43efb0UL, 0x5051f4a7UL, 0x5b5ffdaaUL, 0x6a75c289UL, 0x617bcb84UL, 0x7c69d093UL, 0x7767d99eUL, 
+0x1e3daed5UL, 0x1533a7d8UL, 0x0821bccfUL, 0x032fb5c2UL, 0x32058ae1UL, 0x390b83ecUL, 0x241998fbUL, 0x2f1791f6UL, 
+0x8d764dd6UL, 0x867844dbUL, 0x9b6a5fccUL, 0x906456c1UL, 0xa14e69e2UL, 0xaa4060efUL, 0xb7527bf8UL, 0xbc5c72f5UL, 
+0xd50605beUL, 0xde080cb3UL, 0xc31a17a4UL, 0xc8141ea9UL, 0xf93e218aUL, 0xf2302887UL, 0xef223390UL, 0xe42c3a9dUL, 
+0x3d96dd06UL, 0x3698d40bUL, 0x2b8acf1cUL, 0x2084c611UL, 0x11aef932UL, 0x1aa0f03fUL, 0x07b2eb28UL, 0x0cbce225UL, 
+0x65e6956eUL, 0x6ee89c63UL, 0x73fa8774UL, 0x78f48e79UL, 0x49deb15aUL, 0x42d0b857UL, 0x5fc2a340UL, 0x54ccaa4dUL, 
+0xf741ecdaUL, 0xfc4fe5d7UL, 0xe15dfec0UL, 0xea53f7cdUL, 0xdb79c8eeUL, 0xd077c1e3UL, 0xcd65daf4UL, 0xc66bd3f9UL, 
+0xaf31a4b2UL, 0xa43fadbfUL, 0xb92db6a8UL, 0xb223bfa5UL, 0x83098086UL, 0x8807898bUL, 0x9515929cUL, 0x9e1b9b91UL, 
+0x47a17c0aUL, 0x4caf7507UL, 0x51bd6e10UL, 0x5ab3671dUL, 0x6b99583eUL, 0x60975133UL, 0x7d854a24UL, 0x768b4329UL, 
+0x1fd13462UL, 0x14df3d6fUL, 0x09cd2678UL, 0x02c32f75UL, 0x33e91056UL, 0x38e7195bUL, 0x25f5024cUL, 0x2efb0b41UL, 
+0x8c9ad761UL, 0x8794de6cUL, 0x9a86c57bUL, 0x9188cc76UL, 0xa0a2f355UL, 0xabacfa58UL, 0xb6bee14fUL, 0xbdb0e842UL, 
+0xd4ea9f09UL, 0xdfe49604UL, 0xc2f68d13UL, 0xc9f8841eUL, 0xf8d2bb3dUL, 0xf3dcb230UL, 0xeecea927UL, 0xe5c0a02aUL, 
+0x3c7a47b1UL, 0x37744ebcUL, 0x2a6655abUL, 0x21685ca6UL, 0x10426385UL, 0x1b4c6a88UL, 0x065e719fUL, 0x0d507892UL, 
+0x640a0fd9UL, 0x6f0406d4UL, 0x72161dc3UL, 0x791814ceUL, 0x48322bedUL, 0x433c22e0UL, 0x5e2e39f7UL, 0x552030faUL, 
+0x01ec9ab7UL, 0x0ae293baUL, 0x17f088adUL, 0x1cfe81a0UL, 0x2dd4be83UL, 0x26dab78eUL, 0x3bc8ac99UL, 0x30c6a594UL, 
+0x599cd2dfUL, 0x5292dbd2UL, 0x4f80c0c5UL, 0x448ec9c8UL, 0x75a4f6ebUL, 0x7eaaffe6UL, 0x63b8e4f1UL, 0x68b6edfcUL, 
+0xb10c0a67UL, 0xba02036aUL, 0xa710187dUL, 0xac1e1170UL, 0x9d342e53UL, 0x963a275eUL, 0x8b283c49UL, 0x80263544UL, 
+0xe97c420fUL, 0xe2724b02UL, 0xff605015UL, 0xf46e5918UL, 0xc544663bUL, 0xce4a6f36UL, 0xd3587421UL, 0xd8567d2cUL, 
+0x7a37a10cUL, 0x7139a801UL, 0x6c2bb316UL, 0x6725ba1bUL, 0x560f8538UL, 0x5d018c35UL, 0x40139722UL, 0x4b1d9e2fUL, 
+0x2247e964UL, 0x2949e069UL, 0x345bfb7eUL, 0x3f55f273UL, 0x0e7fcd50UL, 0x0571c45dUL, 0x1863df4aUL, 0x136dd647UL, 
+0xcad731dcUL, 0xc1d938d1UL, 0xdccb23c6UL, 0xd7c52acbUL, 0xe6ef15e8UL, 0xede11ce5UL, 0xf0f307f2UL, 0xfbfd0effUL, 
+0x92a779b4UL, 0x99a970b9UL, 0x84bb6baeUL, 0x8fb562a3UL, 0xbe9f5d80UL, 0xb591548dUL, 0xa8834f9aUL, 0xa38d4697UL
+};
+
+static const ulong32 Tks2[] = {
+0x00000000UL, 0x0d0b0e09UL, 0x1a161c12UL, 0x171d121bUL, 0x342c3824UL, 0x3927362dUL, 0x2e3a2436UL, 0x23312a3fUL, 
+0x68587048UL, 0x65537e41UL, 0x724e6c5aUL, 0x7f456253UL, 0x5c74486cUL, 0x517f4665UL, 0x4662547eUL, 0x4b695a77UL, 
+0xd0b0e090UL, 0xddbbee99UL, 0xcaa6fc82UL, 0xc7adf28bUL, 0xe49cd8b4UL, 0xe997d6bdUL, 0xfe8ac4a6UL, 0xf381caafUL, 
+0xb8e890d8UL, 0xb5e39ed1UL, 0xa2fe8ccaUL, 0xaff582c3UL, 0x8cc4a8fcUL, 0x81cfa6f5UL, 0x96d2b4eeUL, 0x9bd9bae7UL, 
+0xbb7bdb3bUL, 0xb670d532UL, 0xa16dc729UL, 0xac66c920UL, 0x8f57e31fUL, 0x825ced16UL, 0x9541ff0dUL, 0x984af104UL, 
+0xd323ab73UL, 0xde28a57aUL, 0xc935b761UL, 0xc43eb968UL, 0xe70f9357UL, 0xea049d5eUL, 0xfd198f45UL, 0xf012814cUL, 
+0x6bcb3babUL, 0x66c035a2UL, 0x71dd27b9UL, 0x7cd629b0UL, 0x5fe7038fUL, 0x52ec0d86UL, 0x45f11f9dUL, 0x48fa1194UL, 
+0x03934be3UL, 0x0e9845eaUL, 0x198557f1UL, 0x148e59f8UL, 0x37bf73c7UL, 0x3ab47dceUL, 0x2da96fd5UL, 0x20a261dcUL, 
+0x6df6ad76UL, 0x60fda37fUL, 0x77e0b164UL, 0x7aebbf6dUL, 0x59da9552UL, 0x54d19b5bUL, 0x43cc8940UL, 0x4ec78749UL, 
+0x05aedd3eUL, 0x08a5d337UL, 0x1fb8c12cUL, 0x12b3cf25UL, 0x3182e51aUL, 0x3c89eb13UL, 0x2b94f908UL, 0x269ff701UL, 
+0xbd464de6UL, 0xb04d43efUL, 0xa75051f4UL, 0xaa5b5ffdUL, 0x896a75c2UL, 0x84617bcbUL, 0x937c69d0UL, 0x9e7767d9UL, 
+0xd51e3daeUL, 0xd81533a7UL, 0xcf0821bcUL, 0xc2032fb5UL, 0xe132058aUL, 0xec390b83UL, 0xfb241998UL, 0xf62f1791UL, 
+0xd68d764dUL, 0xdb867844UL, 0xcc9b6a5fUL, 0xc1906456UL, 0xe2a14e69UL, 0xefaa4060UL, 0xf8b7527bUL, 0xf5bc5c72UL, 
+0xbed50605UL, 0xb3de080cUL, 0xa4c31a17UL, 0xa9c8141eUL, 0x8af93e21UL, 0x87f23028UL, 0x90ef2233UL, 0x9de42c3aUL, 
+0x063d96ddUL, 0x0b3698d4UL, 0x1c2b8acfUL, 0x112084c6UL, 0x3211aef9UL, 0x3f1aa0f0UL, 0x2807b2ebUL, 0x250cbce2UL, 
+0x6e65e695UL, 0x636ee89cUL, 0x7473fa87UL, 0x7978f48eUL, 0x5a49deb1UL, 0x5742d0b8UL, 0x405fc2a3UL, 0x4d54ccaaUL, 
+0xdaf741ecUL, 0xd7fc4fe5UL, 0xc0e15dfeUL, 0xcdea53f7UL, 0xeedb79c8UL, 0xe3d077c1UL, 0xf4cd65daUL, 0xf9c66bd3UL, 
+0xb2af31a4UL, 0xbfa43fadUL, 0xa8b92db6UL, 0xa5b223bfUL, 0x86830980UL, 0x8b880789UL, 0x9c951592UL, 0x919e1b9bUL, 
+0x0a47a17cUL, 0x074caf75UL, 0x1051bd6eUL, 0x1d5ab367UL, 0x3e6b9958UL, 0x33609751UL, 0x247d854aUL, 0x29768b43UL, 
+0x621fd134UL, 0x6f14df3dUL, 0x7809cd26UL, 0x7502c32fUL, 0x5633e910UL, 0x5b38e719UL, 0x4c25f502UL, 0x412efb0bUL, 
+0x618c9ad7UL, 0x6c8794deUL, 0x7b9a86c5UL, 0x769188ccUL, 0x55a0a2f3UL, 0x58abacfaUL, 0x4fb6bee1UL, 0x42bdb0e8UL, 
+0x09d4ea9fUL, 0x04dfe496UL, 0x13c2f68dUL, 0x1ec9f884UL, 0x3df8d2bbUL, 0x30f3dcb2UL, 0x27eecea9UL, 0x2ae5c0a0UL, 
+0xb13c7a47UL, 0xbc37744eUL, 0xab2a6655UL, 0xa621685cUL, 0x85104263UL, 0x881b4c6aUL, 0x9f065e71UL, 0x920d5078UL, 
+0xd9640a0fUL, 0xd46f0406UL, 0xc372161dUL, 0xce791814UL, 0xed48322bUL, 0xe0433c22UL, 0xf75e2e39UL, 0xfa552030UL, 
+0xb701ec9aUL, 0xba0ae293UL, 0xad17f088UL, 0xa01cfe81UL, 0x832dd4beUL, 0x8e26dab7UL, 0x993bc8acUL, 0x9430c6a5UL, 
+0xdf599cd2UL, 0xd25292dbUL, 0xc54f80c0UL, 0xc8448ec9UL, 0xeb75a4f6UL, 0xe67eaaffUL, 0xf163b8e4UL, 0xfc68b6edUL, 
+0x67b10c0aUL, 0x6aba0203UL, 0x7da71018UL, 0x70ac1e11UL, 0x539d342eUL, 0x5e963a27UL, 0x498b283cUL, 0x44802635UL, 
+0x0fe97c42UL, 0x02e2724bUL, 0x15ff6050UL, 0x18f46e59UL, 0x3bc54466UL, 0x36ce4a6fUL, 0x21d35874UL, 0x2cd8567dUL, 
+0x0c7a37a1UL, 0x017139a8UL, 0x166c2bb3UL, 0x1b6725baUL, 0x38560f85UL, 0x355d018cUL, 0x22401397UL, 0x2f4b1d9eUL, 
+0x642247e9UL, 0x692949e0UL, 0x7e345bfbUL, 0x733f55f2UL, 0x500e7fcdUL, 0x5d0571c4UL, 0x4a1863dfUL, 0x47136dd6UL, 
+0xdccad731UL, 0xd1c1d938UL, 0xc6dccb23UL, 0xcbd7c52aUL, 0xe8e6ef15UL, 0xe5ede11cUL, 0xf2f0f307UL, 0xfffbfd0eUL, 
+0xb492a779UL, 0xb999a970UL, 0xae84bb6bUL, 0xa38fb562UL, 0x80be9f5dUL, 0x8db59154UL, 0x9aa8834fUL, 0x97a38d46UL
+};
+
+static const ulong32 Tks3[] = {
+0x00000000UL, 0x090d0b0eUL, 0x121a161cUL, 0x1b171d12UL, 0x24342c38UL, 0x2d392736UL, 0x362e3a24UL, 0x3f23312aUL, 
+0x48685870UL, 0x4165537eUL, 0x5a724e6cUL, 0x537f4562UL, 0x6c5c7448UL, 0x65517f46UL, 0x7e466254UL, 0x774b695aUL, 
+0x90d0b0e0UL, 0x99ddbbeeUL, 0x82caa6fcUL, 0x8bc7adf2UL, 0xb4e49cd8UL, 0xbde997d6UL, 0xa6fe8ac4UL, 0xaff381caUL, 
+0xd8b8e890UL, 0xd1b5e39eUL, 0xcaa2fe8cUL, 0xc3aff582UL, 0xfc8cc4a8UL, 0xf581cfa6UL, 0xee96d2b4UL, 0xe79bd9baUL, 
+0x3bbb7bdbUL, 0x32b670d5UL, 0x29a16dc7UL, 0x20ac66c9UL, 0x1f8f57e3UL, 0x16825cedUL, 0x0d9541ffUL, 0x04984af1UL, 
+0x73d323abUL, 0x7ade28a5UL, 0x61c935b7UL, 0x68c43eb9UL, 0x57e70f93UL, 0x5eea049dUL, 0x45fd198fUL, 0x4cf01281UL, 
+0xab6bcb3bUL, 0xa266c035UL, 0xb971dd27UL, 0xb07cd629UL, 0x8f5fe703UL, 0x8652ec0dUL, 0x9d45f11fUL, 0x9448fa11UL, 
+0xe303934bUL, 0xea0e9845UL, 0xf1198557UL, 0xf8148e59UL, 0xc737bf73UL, 0xce3ab47dUL, 0xd52da96fUL, 0xdc20a261UL, 
+0x766df6adUL, 0x7f60fda3UL, 0x6477e0b1UL, 0x6d7aebbfUL, 0x5259da95UL, 0x5b54d19bUL, 0x4043cc89UL, 0x494ec787UL, 
+0x3e05aeddUL, 0x3708a5d3UL, 0x2c1fb8c1UL, 0x2512b3cfUL, 0x1a3182e5UL, 0x133c89ebUL, 0x082b94f9UL, 0x01269ff7UL, 
+0xe6bd464dUL, 0xefb04d43UL, 0xf4a75051UL, 0xfdaa5b5fUL, 0xc2896a75UL, 0xcb84617bUL, 0xd0937c69UL, 0xd99e7767UL, 
+0xaed51e3dUL, 0xa7d81533UL, 0xbccf0821UL, 0xb5c2032fUL, 0x8ae13205UL, 0x83ec390bUL, 0x98fb2419UL, 0x91f62f17UL, 
+0x4dd68d76UL, 0x44db8678UL, 0x5fcc9b6aUL, 0x56c19064UL, 0x69e2a14eUL, 0x60efaa40UL, 0x7bf8b752UL, 0x72f5bc5cUL, 
+0x05bed506UL, 0x0cb3de08UL, 0x17a4c31aUL, 0x1ea9c814UL, 0x218af93eUL, 0x2887f230UL, 0x3390ef22UL, 0x3a9de42cUL, 
+0xdd063d96UL, 0xd40b3698UL, 0xcf1c2b8aUL, 0xc6112084UL, 0xf93211aeUL, 0xf03f1aa0UL, 0xeb2807b2UL, 0xe2250cbcUL, 
+0x956e65e6UL, 0x9c636ee8UL, 0x877473faUL, 0x8e7978f4UL, 0xb15a49deUL, 0xb85742d0UL, 0xa3405fc2UL, 0xaa4d54ccUL, 
+0xecdaf741UL, 0xe5d7fc4fUL, 0xfec0e15dUL, 0xf7cdea53UL, 0xc8eedb79UL, 0xc1e3d077UL, 0xdaf4cd65UL, 0xd3f9c66bUL, 
+0xa4b2af31UL, 0xadbfa43fUL, 0xb6a8b92dUL, 0xbfa5b223UL, 0x80868309UL, 0x898b8807UL, 0x929c9515UL, 0x9b919e1bUL, 
+0x7c0a47a1UL, 0x75074cafUL, 0x6e1051bdUL, 0x671d5ab3UL, 0x583e6b99UL, 0x51336097UL, 0x4a247d85UL, 0x4329768bUL, 
+0x34621fd1UL, 0x3d6f14dfUL, 0x267809cdUL, 0x2f7502c3UL, 0x105633e9UL, 0x195b38e7UL, 0x024c25f5UL, 0x0b412efbUL, 
+0xd7618c9aUL, 0xde6c8794UL, 0xc57b9a86UL, 0xcc769188UL, 0xf355a0a2UL, 0xfa58abacUL, 0xe14fb6beUL, 0xe842bdb0UL, 
+0x9f09d4eaUL, 0x9604dfe4UL, 0x8d13c2f6UL, 0x841ec9f8UL, 0xbb3df8d2UL, 0xb230f3dcUL, 0xa927eeceUL, 0xa02ae5c0UL, 
+0x47b13c7aUL, 0x4ebc3774UL, 0x55ab2a66UL, 0x5ca62168UL, 0x63851042UL, 0x6a881b4cUL, 0x719f065eUL, 0x78920d50UL, 
+0x0fd9640aUL, 0x06d46f04UL, 0x1dc37216UL, 0x14ce7918UL, 0x2bed4832UL, 0x22e0433cUL, 0x39f75e2eUL, 0x30fa5520UL, 
+0x9ab701ecUL, 0x93ba0ae2UL, 0x88ad17f0UL, 0x81a01cfeUL, 0xbe832dd4UL, 0xb78e26daUL, 0xac993bc8UL, 0xa59430c6UL, 
+0xd2df599cUL, 0xdbd25292UL, 0xc0c54f80UL, 0xc9c8448eUL, 0xf6eb75a4UL, 0xffe67eaaUL, 0xe4f163b8UL, 0xedfc68b6UL, 
+0x0a67b10cUL, 0x036aba02UL, 0x187da710UL, 0x1170ac1eUL, 0x2e539d34UL, 0x275e963aUL, 0x3c498b28UL, 0x35448026UL, 
+0x420fe97cUL, 0x4b02e272UL, 0x5015ff60UL, 0x5918f46eUL, 0x663bc544UL, 0x6f36ce4aUL, 0x7421d358UL, 0x7d2cd856UL, 
+0xa10c7a37UL, 0xa8017139UL, 0xb3166c2bUL, 0xba1b6725UL, 0x8538560fUL, 0x8c355d01UL, 0x97224013UL, 0x9e2f4b1dUL, 
+0xe9642247UL, 0xe0692949UL, 0xfb7e345bUL, 0xf2733f55UL, 0xcd500e7fUL, 0xc45d0571UL, 0xdf4a1863UL, 0xd647136dUL, 
+0x31dccad7UL, 0x38d1c1d9UL, 0x23c6dccbUL, 0x2acbd7c5UL, 0x15e8e6efUL, 0x1ce5ede1UL, 0x07f2f0f3UL, 0x0efffbfdUL, 
+0x79b492a7UL, 0x70b999a9UL, 0x6bae84bbUL, 0x62a38fb5UL, 0x5d80be9fUL, 0x548db591UL, 0x4f9aa883UL, 0x4697a38dUL
+};
+
+#endif /* ENCRYPT_ONLY */
+
+#endif /* SMALL CODE */
+
+static const ulong32 rcon[] = {
+    0x01000000UL, 0x02000000UL, 0x04000000UL, 0x08000000UL,
+    0x10000000UL, 0x20000000UL, 0x40000000UL, 0x80000000UL,
+    0x1B000000UL, 0x36000000UL, /* for 128-bit blocks, Rijndael never uses more than 10 rcon values */
+};
+
+/* $Source: /cvs/libtom/libtomcrypt/src/ciphers/aes/aes_tab.c,v $ */
+/* $Revision: 1.6 $ */
+/* $Date: 2006/12/28 01:27:23 $ */
diff --git a/core/lib/libtomcrypt/src/ciphers/des.c b/core/lib/libtomcrypt/src/ciphers/des.c
new file mode 100644
index 0000000..a83b7df
--- /dev/null
+++ b/core/lib/libtomcrypt/src/ciphers/des.c
@@ -0,0 +1,2112 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/** 
+  @file des.c
+  LTC_DES code submitted by Dobes Vandermeer 
+*/
+
+#ifdef LTC_DES
+
+#define EN0 0 
+#define DE1 1
+
+const struct ltc_cipher_descriptor des_desc =
+{
+    "des",
+    13,
+    8, 8, 8, 16,
+    &des_setup,
+    &des_ecb_encrypt,
+    &des_ecb_decrypt,
+    &des_test,
+    &des_done,
+    &des_keysize,
+    NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL
+};
+
+const struct ltc_cipher_descriptor des3_desc =
+{
+    "3des",
+    14,
+    24, 24, 8, 16,
+    &des3_setup,
+    &des3_ecb_encrypt,
+    &des3_ecb_decrypt,
+    &des3_test,
+    &des3_done,
+    &des3_keysize,
+    NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL
+};
+
+static const ulong32 bytebit[8] =
+{
+    0200, 0100, 040, 020, 010, 04, 02, 01 
+};
+
+static const ulong32 bigbyte[24] =
+{
+    0x800000UL,  0x400000UL,  0x200000UL,  0x100000UL,
+    0x80000UL,   0x40000UL,   0x20000UL,   0x10000UL,
+    0x8000UL,    0x4000UL,    0x2000UL,    0x1000UL,
+    0x800UL,     0x400UL,     0x200UL,     0x100UL,
+    0x80UL,      0x40UL,      0x20UL,      0x10UL,
+    0x8UL,       0x4UL,       0x2UL,       0x1L 
+};
+
+/* Use the key schedule specific in the standard (ANSI X3.92-1981) */
+
+static const unsigned char pc1[56] = {
+    56, 48, 40, 32, 24, 16,  8,  0, 57, 49, 41, 33, 25, 17,  
+     9,  1, 58, 50, 42, 34, 26, 18, 10,  2, 59, 51, 43, 35, 
+    62, 54, 46, 38, 30, 22, 14,  6, 61, 53, 45, 37, 29, 21,
+    13,  5, 60, 52, 44, 36, 28, 20, 12,  4, 27, 19, 11,  3 
+};
+
+static const unsigned char totrot[16] = {
+    1,   2,  4,  6,
+    8,  10, 12, 14, 
+    15, 17, 19, 21, 
+    23, 25, 27, 28
+};
+
+static const unsigned char pc2[48] = {
+    13, 16, 10, 23,  0,  4,      2, 27, 14,  5, 20,  9,
+    22, 18, 11,  3, 25,  7,     15,  6, 26, 19, 12,  1,
+    40, 51, 30, 36, 46, 54,     29, 39, 50, 44, 32, 47,
+    43, 48, 38, 55, 33, 52,     45, 41, 49, 35, 28, 31
+};
+
+
+static const ulong32 SP1[64] =
+{
+    0x01010400UL, 0x00000000UL, 0x00010000UL, 0x01010404UL,
+    0x01010004UL, 0x00010404UL, 0x00000004UL, 0x00010000UL,
+    0x00000400UL, 0x01010400UL, 0x01010404UL, 0x00000400UL,
+    0x01000404UL, 0x01010004UL, 0x01000000UL, 0x00000004UL,
+    0x00000404UL, 0x01000400UL, 0x01000400UL, 0x00010400UL,
+    0x00010400UL, 0x01010000UL, 0x01010000UL, 0x01000404UL,
+    0x00010004UL, 0x01000004UL, 0x01000004UL, 0x00010004UL,
+    0x00000000UL, 0x00000404UL, 0x00010404UL, 0x01000000UL,
+    0x00010000UL, 0x01010404UL, 0x00000004UL, 0x01010000UL,
+    0x01010400UL, 0x01000000UL, 0x01000000UL, 0x00000400UL,
+    0x01010004UL, 0x00010000UL, 0x00010400UL, 0x01000004UL,
+    0x00000400UL, 0x00000004UL, 0x01000404UL, 0x00010404UL,
+    0x01010404UL, 0x00010004UL, 0x01010000UL, 0x01000404UL,
+    0x01000004UL, 0x00000404UL, 0x00010404UL, 0x01010400UL,
+    0x00000404UL, 0x01000400UL, 0x01000400UL, 0x00000000UL,
+    0x00010004UL, 0x00010400UL, 0x00000000UL, 0x01010004UL
+};
+
+static const ulong32 SP2[64] =
+{
+    0x80108020UL, 0x80008000UL, 0x00008000UL, 0x00108020UL,
+    0x00100000UL, 0x00000020UL, 0x80100020UL, 0x80008020UL,
+    0x80000020UL, 0x80108020UL, 0x80108000UL, 0x80000000UL,
+    0x80008000UL, 0x00100000UL, 0x00000020UL, 0x80100020UL,
+    0x00108000UL, 0x00100020UL, 0x80008020UL, 0x00000000UL,
+    0x80000000UL, 0x00008000UL, 0x00108020UL, 0x80100000UL,
+    0x00100020UL, 0x80000020UL, 0x00000000UL, 0x00108000UL,
+    0x00008020UL, 0x80108000UL, 0x80100000UL, 0x00008020UL,
+    0x00000000UL, 0x00108020UL, 0x80100020UL, 0x00100000UL,
+    0x80008020UL, 0x80100000UL, 0x80108000UL, 0x00008000UL,
+    0x80100000UL, 0x80008000UL, 0x00000020UL, 0x80108020UL,
+    0x00108020UL, 0x00000020UL, 0x00008000UL, 0x80000000UL,
+    0x00008020UL, 0x80108000UL, 0x00100000UL, 0x80000020UL,
+    0x00100020UL, 0x80008020UL, 0x80000020UL, 0x00100020UL,
+    0x00108000UL, 0x00000000UL, 0x80008000UL, 0x00008020UL,
+    0x80000000UL, 0x80100020UL, 0x80108020UL, 0x00108000UL
+};
+
+static const ulong32 SP3[64] =
+{
+    0x00000208UL, 0x08020200UL, 0x00000000UL, 0x08020008UL,
+    0x08000200UL, 0x00000000UL, 0x00020208UL, 0x08000200UL,
+    0x00020008UL, 0x08000008UL, 0x08000008UL, 0x00020000UL,
+    0x08020208UL, 0x00020008UL, 0x08020000UL, 0x00000208UL,
+    0x08000000UL, 0x00000008UL, 0x08020200UL, 0x00000200UL,
+    0x00020200UL, 0x08020000UL, 0x08020008UL, 0x00020208UL,
+    0x08000208UL, 0x00020200UL, 0x00020000UL, 0x08000208UL,
+    0x00000008UL, 0x08020208UL, 0x00000200UL, 0x08000000UL,
+    0x08020200UL, 0x08000000UL, 0x00020008UL, 0x00000208UL,
+    0x00020000UL, 0x08020200UL, 0x08000200UL, 0x00000000UL,
+    0x00000200UL, 0x00020008UL, 0x08020208UL, 0x08000200UL,
+    0x08000008UL, 0x00000200UL, 0x00000000UL, 0x08020008UL,
+    0x08000208UL, 0x00020000UL, 0x08000000UL, 0x08020208UL,
+    0x00000008UL, 0x00020208UL, 0x00020200UL, 0x08000008UL,
+    0x08020000UL, 0x08000208UL, 0x00000208UL, 0x08020000UL,
+    0x00020208UL, 0x00000008UL, 0x08020008UL, 0x00020200UL
+};
+
+static const ulong32 SP4[64] =
+{
+    0x00802001UL, 0x00002081UL, 0x00002081UL, 0x00000080UL,
+    0x00802080UL, 0x00800081UL, 0x00800001UL, 0x00002001UL,
+    0x00000000UL, 0x00802000UL, 0x00802000UL, 0x00802081UL,
+    0x00000081UL, 0x00000000UL, 0x00800080UL, 0x00800001UL,
+    0x00000001UL, 0x00002000UL, 0x00800000UL, 0x00802001UL,
+    0x00000080UL, 0x00800000UL, 0x00002001UL, 0x00002080UL,
+    0x00800081UL, 0x00000001UL, 0x00002080UL, 0x00800080UL,
+    0x00002000UL, 0x00802080UL, 0x00802081UL, 0x00000081UL,
+    0x00800080UL, 0x00800001UL, 0x00802000UL, 0x00802081UL,
+    0x00000081UL, 0x00000000UL, 0x00000000UL, 0x00802000UL,
+    0x00002080UL, 0x00800080UL, 0x00800081UL, 0x00000001UL,
+    0x00802001UL, 0x00002081UL, 0x00002081UL, 0x00000080UL,
+    0x00802081UL, 0x00000081UL, 0x00000001UL, 0x00002000UL,
+    0x00800001UL, 0x00002001UL, 0x00802080UL, 0x00800081UL,
+    0x00002001UL, 0x00002080UL, 0x00800000UL, 0x00802001UL,
+    0x00000080UL, 0x00800000UL, 0x00002000UL, 0x00802080UL
+};
+
+static const ulong32 SP5[64] =
+{
+    0x00000100UL, 0x02080100UL, 0x02080000UL, 0x42000100UL,
+    0x00080000UL, 0x00000100UL, 0x40000000UL, 0x02080000UL,
+    0x40080100UL, 0x00080000UL, 0x02000100UL, 0x40080100UL,
+    0x42000100UL, 0x42080000UL, 0x00080100UL, 0x40000000UL,
+    0x02000000UL, 0x40080000UL, 0x40080000UL, 0x00000000UL,
+    0x40000100UL, 0x42080100UL, 0x42080100UL, 0x02000100UL,
+    0x42080000UL, 0x40000100UL, 0x00000000UL, 0x42000000UL,
+    0x02080100UL, 0x02000000UL, 0x42000000UL, 0x00080100UL,
+    0x00080000UL, 0x42000100UL, 0x00000100UL, 0x02000000UL,
+    0x40000000UL, 0x02080000UL, 0x42000100UL, 0x40080100UL,
+    0x02000100UL, 0x40000000UL, 0x42080000UL, 0x02080100UL,
+    0x40080100UL, 0x00000100UL, 0x02000000UL, 0x42080000UL,
+    0x42080100UL, 0x00080100UL, 0x42000000UL, 0x42080100UL,
+    0x02080000UL, 0x00000000UL, 0x40080000UL, 0x42000000UL,
+    0x00080100UL, 0x02000100UL, 0x40000100UL, 0x00080000UL,
+    0x00000000UL, 0x40080000UL, 0x02080100UL, 0x40000100UL
+};
+
+static const ulong32 SP6[64] =
+{
+    0x20000010UL, 0x20400000UL, 0x00004000UL, 0x20404010UL,
+    0x20400000UL, 0x00000010UL, 0x20404010UL, 0x00400000UL,
+    0x20004000UL, 0x00404010UL, 0x00400000UL, 0x20000010UL,
+    0x00400010UL, 0x20004000UL, 0x20000000UL, 0x00004010UL,
+    0x00000000UL, 0x00400010UL, 0x20004010UL, 0x00004000UL,
+    0x00404000UL, 0x20004010UL, 0x00000010UL, 0x20400010UL,
+    0x20400010UL, 0x00000000UL, 0x00404010UL, 0x20404000UL,
+    0x00004010UL, 0x00404000UL, 0x20404000UL, 0x20000000UL,
+    0x20004000UL, 0x00000010UL, 0x20400010UL, 0x00404000UL,
+    0x20404010UL, 0x00400000UL, 0x00004010UL, 0x20000010UL,
+    0x00400000UL, 0x20004000UL, 0x20000000UL, 0x00004010UL,
+    0x20000010UL, 0x20404010UL, 0x00404000UL, 0x20400000UL,
+    0x00404010UL, 0x20404000UL, 0x00000000UL, 0x20400010UL,
+    0x00000010UL, 0x00004000UL, 0x20400000UL, 0x00404010UL,
+    0x00004000UL, 0x00400010UL, 0x20004010UL, 0x00000000UL,
+    0x20404000UL, 0x20000000UL, 0x00400010UL, 0x20004010UL
+};
+
+static const ulong32 SP7[64] =
+{
+    0x00200000UL, 0x04200002UL, 0x04000802UL, 0x00000000UL,
+    0x00000800UL, 0x04000802UL, 0x00200802UL, 0x04200800UL,
+    0x04200802UL, 0x00200000UL, 0x00000000UL, 0x04000002UL,
+    0x00000002UL, 0x04000000UL, 0x04200002UL, 0x00000802UL,
+    0x04000800UL, 0x00200802UL, 0x00200002UL, 0x04000800UL,
+    0x04000002UL, 0x04200000UL, 0x04200800UL, 0x00200002UL,
+    0x04200000UL, 0x00000800UL, 0x00000802UL, 0x04200802UL,
+    0x00200800UL, 0x00000002UL, 0x04000000UL, 0x00200800UL,
+    0x04000000UL, 0x00200800UL, 0x00200000UL, 0x04000802UL,
+    0x04000802UL, 0x04200002UL, 0x04200002UL, 0x00000002UL,
+    0x00200002UL, 0x04000000UL, 0x04000800UL, 0x00200000UL,
+    0x04200800UL, 0x00000802UL, 0x00200802UL, 0x04200800UL,
+    0x00000802UL, 0x04000002UL, 0x04200802UL, 0x04200000UL,
+    0x00200800UL, 0x00000000UL, 0x00000002UL, 0x04200802UL,
+    0x00000000UL, 0x00200802UL, 0x04200000UL, 0x00000800UL,
+    0x04000002UL, 0x04000800UL, 0x00000800UL, 0x00200002UL
+};
+
+static const ulong32 SP8[64] =
+{
+    0x10001040UL, 0x00001000UL, 0x00040000UL, 0x10041040UL,
+    0x10000000UL, 0x10001040UL, 0x00000040UL, 0x10000000UL,
+    0x00040040UL, 0x10040000UL, 0x10041040UL, 0x00041000UL,
+    0x10041000UL, 0x00041040UL, 0x00001000UL, 0x00000040UL,
+    0x10040000UL, 0x10000040UL, 0x10001000UL, 0x00001040UL,
+    0x00041000UL, 0x00040040UL, 0x10040040UL, 0x10041000UL,
+    0x00001040UL, 0x00000000UL, 0x00000000UL, 0x10040040UL,
+    0x10000040UL, 0x10001000UL, 0x00041040UL, 0x00040000UL,
+    0x00041040UL, 0x00040000UL, 0x10041000UL, 0x00001000UL,
+    0x00000040UL, 0x10040040UL, 0x00001000UL, 0x00041040UL,
+    0x10001000UL, 0x00000040UL, 0x10000040UL, 0x10040000UL,
+    0x10040040UL, 0x10000000UL, 0x00040000UL, 0x10001040UL,
+    0x00000000UL, 0x10041040UL, 0x00040040UL, 0x10000040UL,
+    0x10040000UL, 0x10001000UL, 0x10001040UL, 0x00000000UL,
+    0x10041040UL, 0x00041000UL, 0x00041000UL, 0x00001040UL,
+    0x00001040UL, 0x00040040UL, 0x10000000UL, 0x10041000UL
+};
+
+#ifndef LTC_SMALL_CODE
+
+static const ulong64 des_ip[8][256] = {
+
+{ CONST64(0x0000000000000000), CONST64(0x0000001000000000), CONST64(0x0000000000000010), CONST64(0x0000001000000010), 
+  CONST64(0x0000100000000000), CONST64(0x0000101000000000), CONST64(0x0000100000000010), CONST64(0x0000101000000010), 
+  CONST64(0x0000000000001000), CONST64(0x0000001000001000), CONST64(0x0000000000001010), CONST64(0x0000001000001010), 
+  CONST64(0x0000100000001000), CONST64(0x0000101000001000), CONST64(0x0000100000001010), CONST64(0x0000101000001010), 
+  CONST64(0x0010000000000000), CONST64(0x0010001000000000), CONST64(0x0010000000000010), CONST64(0x0010001000000010), 
+  CONST64(0x0010100000000000), CONST64(0x0010101000000000), CONST64(0x0010100000000010), CONST64(0x0010101000000010), 
+  CONST64(0x0010000000001000), CONST64(0x0010001000001000), CONST64(0x0010000000001010), CONST64(0x0010001000001010), 
+  CONST64(0x0010100000001000), CONST64(0x0010101000001000), CONST64(0x0010100000001010), CONST64(0x0010101000001010), 
+  CONST64(0x0000000000100000), CONST64(0x0000001000100000), CONST64(0x0000000000100010), CONST64(0x0000001000100010), 
+  CONST64(0x0000100000100000), CONST64(0x0000101000100000), CONST64(0x0000100000100010), CONST64(0x0000101000100010), 
+  CONST64(0x0000000000101000), CONST64(0x0000001000101000), CONST64(0x0000000000101010), CONST64(0x0000001000101010), 
+  CONST64(0x0000100000101000), CONST64(0x0000101000101000), CONST64(0x0000100000101010), CONST64(0x0000101000101010), 
+  CONST64(0x0010000000100000), CONST64(0x0010001000100000), CONST64(0x0010000000100010), CONST64(0x0010001000100010), 
+  CONST64(0x0010100000100000), CONST64(0x0010101000100000), CONST64(0x0010100000100010), CONST64(0x0010101000100010), 
+  CONST64(0x0010000000101000), CONST64(0x0010001000101000), CONST64(0x0010000000101010), CONST64(0x0010001000101010), 
+  CONST64(0x0010100000101000), CONST64(0x0010101000101000), CONST64(0x0010100000101010), CONST64(0x0010101000101010), 
+  CONST64(0x1000000000000000), CONST64(0x1000001000000000), CONST64(0x1000000000000010), CONST64(0x1000001000000010), 
+  CONST64(0x1000100000000000), CONST64(0x1000101000000000), CONST64(0x1000100000000010), CONST64(0x1000101000000010), 
+  CONST64(0x1000000000001000), CONST64(0x1000001000001000), CONST64(0x1000000000001010), CONST64(0x1000001000001010), 
+  CONST64(0x1000100000001000), CONST64(0x1000101000001000), CONST64(0x1000100000001010), CONST64(0x1000101000001010), 
+  CONST64(0x1010000000000000), CONST64(0x1010001000000000), CONST64(0x1010000000000010), CONST64(0x1010001000000010), 
+  CONST64(0x1010100000000000), CONST64(0x1010101000000000), CONST64(0x1010100000000010), CONST64(0x1010101000000010), 
+  CONST64(0x1010000000001000), CONST64(0x1010001000001000), CONST64(0x1010000000001010), CONST64(0x1010001000001010), 
+  CONST64(0x1010100000001000), CONST64(0x1010101000001000), CONST64(0x1010100000001010), CONST64(0x1010101000001010), 
+  CONST64(0x1000000000100000), CONST64(0x1000001000100000), CONST64(0x1000000000100010), CONST64(0x1000001000100010), 
+  CONST64(0x1000100000100000), CONST64(0x1000101000100000), CONST64(0x1000100000100010), CONST64(0x1000101000100010), 
+  CONST64(0x1000000000101000), CONST64(0x1000001000101000), CONST64(0x1000000000101010), CONST64(0x1000001000101010), 
+  CONST64(0x1000100000101000), CONST64(0x1000101000101000), CONST64(0x1000100000101010), CONST64(0x1000101000101010), 
+  CONST64(0x1010000000100000), CONST64(0x1010001000100000), CONST64(0x1010000000100010), CONST64(0x1010001000100010), 
+  CONST64(0x1010100000100000), CONST64(0x1010101000100000), CONST64(0x1010100000100010), CONST64(0x1010101000100010), 
+  CONST64(0x1010000000101000), CONST64(0x1010001000101000), CONST64(0x1010000000101010), CONST64(0x1010001000101010), 
+  CONST64(0x1010100000101000), CONST64(0x1010101000101000), CONST64(0x1010100000101010), CONST64(0x1010101000101010), 
+  CONST64(0x0000000010000000), CONST64(0x0000001010000000), CONST64(0x0000000010000010), CONST64(0x0000001010000010), 
+  CONST64(0x0000100010000000), CONST64(0x0000101010000000), CONST64(0x0000100010000010), CONST64(0x0000101010000010), 
+  CONST64(0x0000000010001000), CONST64(0x0000001010001000), CONST64(0x0000000010001010), CONST64(0x0000001010001010), 
+  CONST64(0x0000100010001000), CONST64(0x0000101010001000), CONST64(0x0000100010001010), CONST64(0x0000101010001010), 
+  CONST64(0x0010000010000000), CONST64(0x0010001010000000), CONST64(0x0010000010000010), CONST64(0x0010001010000010), 
+  CONST64(0x0010100010000000), CONST64(0x0010101010000000), CONST64(0x0010100010000010), CONST64(0x0010101010000010), 
+  CONST64(0x0010000010001000), CONST64(0x0010001010001000), CONST64(0x0010000010001010), CONST64(0x0010001010001010), 
+  CONST64(0x0010100010001000), CONST64(0x0010101010001000), CONST64(0x0010100010001010), CONST64(0x0010101010001010), 
+  CONST64(0x0000000010100000), CONST64(0x0000001010100000), CONST64(0x0000000010100010), CONST64(0x0000001010100010), 
+  CONST64(0x0000100010100000), CONST64(0x0000101010100000), CONST64(0x0000100010100010), CONST64(0x0000101010100010), 
+  CONST64(0x0000000010101000), CONST64(0x0000001010101000), CONST64(0x0000000010101010), CONST64(0x0000001010101010), 
+  CONST64(0x0000100010101000), CONST64(0x0000101010101000), CONST64(0x0000100010101010), CONST64(0x0000101010101010), 
+  CONST64(0x0010000010100000), CONST64(0x0010001010100000), CONST64(0x0010000010100010), CONST64(0x0010001010100010), 
+  CONST64(0x0010100010100000), CONST64(0x0010101010100000), CONST64(0x0010100010100010), CONST64(0x0010101010100010), 
+  CONST64(0x0010000010101000), CONST64(0x0010001010101000), CONST64(0x0010000010101010), CONST64(0x0010001010101010), 
+  CONST64(0x0010100010101000), CONST64(0x0010101010101000), CONST64(0x0010100010101010), CONST64(0x0010101010101010), 
+  CONST64(0x1000000010000000), CONST64(0x1000001010000000), CONST64(0x1000000010000010), CONST64(0x1000001010000010), 
+  CONST64(0x1000100010000000), CONST64(0x1000101010000000), CONST64(0x1000100010000010), CONST64(0x1000101010000010), 
+  CONST64(0x1000000010001000), CONST64(0x1000001010001000), CONST64(0x1000000010001010), CONST64(0x1000001010001010), 
+  CONST64(0x1000100010001000), CONST64(0x1000101010001000), CONST64(0x1000100010001010), CONST64(0x1000101010001010), 
+  CONST64(0x1010000010000000), CONST64(0x1010001010000000), CONST64(0x1010000010000010), CONST64(0x1010001010000010), 
+  CONST64(0x1010100010000000), CONST64(0x1010101010000000), CONST64(0x1010100010000010), CONST64(0x1010101010000010), 
+  CONST64(0x1010000010001000), CONST64(0x1010001010001000), CONST64(0x1010000010001010), CONST64(0x1010001010001010), 
+  CONST64(0x1010100010001000), CONST64(0x1010101010001000), CONST64(0x1010100010001010), CONST64(0x1010101010001010), 
+  CONST64(0x1000000010100000), CONST64(0x1000001010100000), CONST64(0x1000000010100010), CONST64(0x1000001010100010), 
+  CONST64(0x1000100010100000), CONST64(0x1000101010100000), CONST64(0x1000100010100010), CONST64(0x1000101010100010), 
+  CONST64(0x1000000010101000), CONST64(0x1000001010101000), CONST64(0x1000000010101010), CONST64(0x1000001010101010), 
+  CONST64(0x1000100010101000), CONST64(0x1000101010101000), CONST64(0x1000100010101010), CONST64(0x1000101010101010), 
+  CONST64(0x1010000010100000), CONST64(0x1010001010100000), CONST64(0x1010000010100010), CONST64(0x1010001010100010), 
+  CONST64(0x1010100010100000), CONST64(0x1010101010100000), CONST64(0x1010100010100010), CONST64(0x1010101010100010), 
+  CONST64(0x1010000010101000), CONST64(0x1010001010101000), CONST64(0x1010000010101010), CONST64(0x1010001010101010), 
+  CONST64(0x1010100010101000), CONST64(0x1010101010101000), CONST64(0x1010100010101010), CONST64(0x1010101010101010)
+  }, 
+{ CONST64(0x0000000000000000), CONST64(0x0000000800000000), CONST64(0x0000000000000008), CONST64(0x0000000800000008), 
+  CONST64(0x0000080000000000), CONST64(0x0000080800000000), CONST64(0x0000080000000008), CONST64(0x0000080800000008), 
+  CONST64(0x0000000000000800), CONST64(0x0000000800000800), CONST64(0x0000000000000808), CONST64(0x0000000800000808), 
+  CONST64(0x0000080000000800), CONST64(0x0000080800000800), CONST64(0x0000080000000808), CONST64(0x0000080800000808), 
+  CONST64(0x0008000000000000), CONST64(0x0008000800000000), CONST64(0x0008000000000008), CONST64(0x0008000800000008), 
+  CONST64(0x0008080000000000), CONST64(0x0008080800000000), CONST64(0x0008080000000008), CONST64(0x0008080800000008), 
+  CONST64(0x0008000000000800), CONST64(0x0008000800000800), CONST64(0x0008000000000808), CONST64(0x0008000800000808), 
+  CONST64(0x0008080000000800), CONST64(0x0008080800000800), CONST64(0x0008080000000808), CONST64(0x0008080800000808), 
+  CONST64(0x0000000000080000), CONST64(0x0000000800080000), CONST64(0x0000000000080008), CONST64(0x0000000800080008), 
+  CONST64(0x0000080000080000), CONST64(0x0000080800080000), CONST64(0x0000080000080008), CONST64(0x0000080800080008), 
+  CONST64(0x0000000000080800), CONST64(0x0000000800080800), CONST64(0x0000000000080808), CONST64(0x0000000800080808), 
+  CONST64(0x0000080000080800), CONST64(0x0000080800080800), CONST64(0x0000080000080808), CONST64(0x0000080800080808), 
+  CONST64(0x0008000000080000), CONST64(0x0008000800080000), CONST64(0x0008000000080008), CONST64(0x0008000800080008), 
+  CONST64(0x0008080000080000), CONST64(0x0008080800080000), CONST64(0x0008080000080008), CONST64(0x0008080800080008), 
+  CONST64(0x0008000000080800), CONST64(0x0008000800080800), CONST64(0x0008000000080808), CONST64(0x0008000800080808), 
+  CONST64(0x0008080000080800), CONST64(0x0008080800080800), CONST64(0x0008080000080808), CONST64(0x0008080800080808), 
+  CONST64(0x0800000000000000), CONST64(0x0800000800000000), CONST64(0x0800000000000008), CONST64(0x0800000800000008), 
+  CONST64(0x0800080000000000), CONST64(0x0800080800000000), CONST64(0x0800080000000008), CONST64(0x0800080800000008), 
+  CONST64(0x0800000000000800), CONST64(0x0800000800000800), CONST64(0x0800000000000808), CONST64(0x0800000800000808), 
+  CONST64(0x0800080000000800), CONST64(0x0800080800000800), CONST64(0x0800080000000808), CONST64(0x0800080800000808), 
+  CONST64(0x0808000000000000), CONST64(0x0808000800000000), CONST64(0x0808000000000008), CONST64(0x0808000800000008), 
+  CONST64(0x0808080000000000), CONST64(0x0808080800000000), CONST64(0x0808080000000008), CONST64(0x0808080800000008), 
+  CONST64(0x0808000000000800), CONST64(0x0808000800000800), CONST64(0x0808000000000808), CONST64(0x0808000800000808), 
+  CONST64(0x0808080000000800), CONST64(0x0808080800000800), CONST64(0x0808080000000808), CONST64(0x0808080800000808), 
+  CONST64(0x0800000000080000), CONST64(0x0800000800080000), CONST64(0x0800000000080008), CONST64(0x0800000800080008), 
+  CONST64(0x0800080000080000), CONST64(0x0800080800080000), CONST64(0x0800080000080008), CONST64(0x0800080800080008), 
+  CONST64(0x0800000000080800), CONST64(0x0800000800080800), CONST64(0x0800000000080808), CONST64(0x0800000800080808), 
+  CONST64(0x0800080000080800), CONST64(0x0800080800080800), CONST64(0x0800080000080808), CONST64(0x0800080800080808), 
+  CONST64(0x0808000000080000), CONST64(0x0808000800080000), CONST64(0x0808000000080008), CONST64(0x0808000800080008), 
+  CONST64(0x0808080000080000), CONST64(0x0808080800080000), CONST64(0x0808080000080008), CONST64(0x0808080800080008), 
+  CONST64(0x0808000000080800), CONST64(0x0808000800080800), CONST64(0x0808000000080808), CONST64(0x0808000800080808), 
+  CONST64(0x0808080000080800), CONST64(0x0808080800080800), CONST64(0x0808080000080808), CONST64(0x0808080800080808), 
+  CONST64(0x0000000008000000), CONST64(0x0000000808000000), CONST64(0x0000000008000008), CONST64(0x0000000808000008), 
+  CONST64(0x0000080008000000), CONST64(0x0000080808000000), CONST64(0x0000080008000008), CONST64(0x0000080808000008), 
+  CONST64(0x0000000008000800), CONST64(0x0000000808000800), CONST64(0x0000000008000808), CONST64(0x0000000808000808), 
+  CONST64(0x0000080008000800), CONST64(0x0000080808000800), CONST64(0x0000080008000808), CONST64(0x0000080808000808), 
+  CONST64(0x0008000008000000), CONST64(0x0008000808000000), CONST64(0x0008000008000008), CONST64(0x0008000808000008), 
+  CONST64(0x0008080008000000), CONST64(0x0008080808000000), CONST64(0x0008080008000008), CONST64(0x0008080808000008), 
+  CONST64(0x0008000008000800), CONST64(0x0008000808000800), CONST64(0x0008000008000808), CONST64(0x0008000808000808), 
+  CONST64(0x0008080008000800), CONST64(0x0008080808000800), CONST64(0x0008080008000808), CONST64(0x0008080808000808), 
+  CONST64(0x0000000008080000), CONST64(0x0000000808080000), CONST64(0x0000000008080008), CONST64(0x0000000808080008), 
+  CONST64(0x0000080008080000), CONST64(0x0000080808080000), CONST64(0x0000080008080008), CONST64(0x0000080808080008), 
+  CONST64(0x0000000008080800), CONST64(0x0000000808080800), CONST64(0x0000000008080808), CONST64(0x0000000808080808), 
+  CONST64(0x0000080008080800), CONST64(0x0000080808080800), CONST64(0x0000080008080808), CONST64(0x0000080808080808), 
+  CONST64(0x0008000008080000), CONST64(0x0008000808080000), CONST64(0x0008000008080008), CONST64(0x0008000808080008), 
+  CONST64(0x0008080008080000), CONST64(0x0008080808080000), CONST64(0x0008080008080008), CONST64(0x0008080808080008), 
+  CONST64(0x0008000008080800), CONST64(0x0008000808080800), CONST64(0x0008000008080808), CONST64(0x0008000808080808), 
+  CONST64(0x0008080008080800), CONST64(0x0008080808080800), CONST64(0x0008080008080808), CONST64(0x0008080808080808), 
+  CONST64(0x0800000008000000), CONST64(0x0800000808000000), CONST64(0x0800000008000008), CONST64(0x0800000808000008), 
+  CONST64(0x0800080008000000), CONST64(0x0800080808000000), CONST64(0x0800080008000008), CONST64(0x0800080808000008), 
+  CONST64(0x0800000008000800), CONST64(0x0800000808000800), CONST64(0x0800000008000808), CONST64(0x0800000808000808), 
+  CONST64(0x0800080008000800), CONST64(0x0800080808000800), CONST64(0x0800080008000808), CONST64(0x0800080808000808), 
+  CONST64(0x0808000008000000), CONST64(0x0808000808000000), CONST64(0x0808000008000008), CONST64(0x0808000808000008), 
+  CONST64(0x0808080008000000), CONST64(0x0808080808000000), CONST64(0x0808080008000008), CONST64(0x0808080808000008), 
+  CONST64(0x0808000008000800), CONST64(0x0808000808000800), CONST64(0x0808000008000808), CONST64(0x0808000808000808), 
+  CONST64(0x0808080008000800), CONST64(0x0808080808000800), CONST64(0x0808080008000808), CONST64(0x0808080808000808), 
+  CONST64(0x0800000008080000), CONST64(0x0800000808080000), CONST64(0x0800000008080008), CONST64(0x0800000808080008), 
+  CONST64(0x0800080008080000), CONST64(0x0800080808080000), CONST64(0x0800080008080008), CONST64(0x0800080808080008), 
+  CONST64(0x0800000008080800), CONST64(0x0800000808080800), CONST64(0x0800000008080808), CONST64(0x0800000808080808), 
+  CONST64(0x0800080008080800), CONST64(0x0800080808080800), CONST64(0x0800080008080808), CONST64(0x0800080808080808), 
+  CONST64(0x0808000008080000), CONST64(0x0808000808080000), CONST64(0x0808000008080008), CONST64(0x0808000808080008), 
+  CONST64(0x0808080008080000), CONST64(0x0808080808080000), CONST64(0x0808080008080008), CONST64(0x0808080808080008), 
+  CONST64(0x0808000008080800), CONST64(0x0808000808080800), CONST64(0x0808000008080808), CONST64(0x0808000808080808), 
+  CONST64(0x0808080008080800), CONST64(0x0808080808080800), CONST64(0x0808080008080808), CONST64(0x0808080808080808)
+  }, 
+{ CONST64(0x0000000000000000), CONST64(0x0000000400000000), CONST64(0x0000000000000004), CONST64(0x0000000400000004), 
+  CONST64(0x0000040000000000), CONST64(0x0000040400000000), CONST64(0x0000040000000004), CONST64(0x0000040400000004), 
+  CONST64(0x0000000000000400), CONST64(0x0000000400000400), CONST64(0x0000000000000404), CONST64(0x0000000400000404), 
+  CONST64(0x0000040000000400), CONST64(0x0000040400000400), CONST64(0x0000040000000404), CONST64(0x0000040400000404), 
+  CONST64(0x0004000000000000), CONST64(0x0004000400000000), CONST64(0x0004000000000004), CONST64(0x0004000400000004), 
+  CONST64(0x0004040000000000), CONST64(0x0004040400000000), CONST64(0x0004040000000004), CONST64(0x0004040400000004), 
+  CONST64(0x0004000000000400), CONST64(0x0004000400000400), CONST64(0x0004000000000404), CONST64(0x0004000400000404), 
+  CONST64(0x0004040000000400), CONST64(0x0004040400000400), CONST64(0x0004040000000404), CONST64(0x0004040400000404), 
+  CONST64(0x0000000000040000), CONST64(0x0000000400040000), CONST64(0x0000000000040004), CONST64(0x0000000400040004), 
+  CONST64(0x0000040000040000), CONST64(0x0000040400040000), CONST64(0x0000040000040004), CONST64(0x0000040400040004), 
+  CONST64(0x0000000000040400), CONST64(0x0000000400040400), CONST64(0x0000000000040404), CONST64(0x0000000400040404), 
+  CONST64(0x0000040000040400), CONST64(0x0000040400040400), CONST64(0x0000040000040404), CONST64(0x0000040400040404), 
+  CONST64(0x0004000000040000), CONST64(0x0004000400040000), CONST64(0x0004000000040004), CONST64(0x0004000400040004), 
+  CONST64(0x0004040000040000), CONST64(0x0004040400040000), CONST64(0x0004040000040004), CONST64(0x0004040400040004), 
+  CONST64(0x0004000000040400), CONST64(0x0004000400040400), CONST64(0x0004000000040404), CONST64(0x0004000400040404), 
+  CONST64(0x0004040000040400), CONST64(0x0004040400040400), CONST64(0x0004040000040404), CONST64(0x0004040400040404), 
+  CONST64(0x0400000000000000), CONST64(0x0400000400000000), CONST64(0x0400000000000004), CONST64(0x0400000400000004), 
+  CONST64(0x0400040000000000), CONST64(0x0400040400000000), CONST64(0x0400040000000004), CONST64(0x0400040400000004), 
+  CONST64(0x0400000000000400), CONST64(0x0400000400000400), CONST64(0x0400000000000404), CONST64(0x0400000400000404), 
+  CONST64(0x0400040000000400), CONST64(0x0400040400000400), CONST64(0x0400040000000404), CONST64(0x0400040400000404), 
+  CONST64(0x0404000000000000), CONST64(0x0404000400000000), CONST64(0x0404000000000004), CONST64(0x0404000400000004), 
+  CONST64(0x0404040000000000), CONST64(0x0404040400000000), CONST64(0x0404040000000004), CONST64(0x0404040400000004), 
+  CONST64(0x0404000000000400), CONST64(0x0404000400000400), CONST64(0x0404000000000404), CONST64(0x0404000400000404), 
+  CONST64(0x0404040000000400), CONST64(0x0404040400000400), CONST64(0x0404040000000404), CONST64(0x0404040400000404), 
+  CONST64(0x0400000000040000), CONST64(0x0400000400040000), CONST64(0x0400000000040004), CONST64(0x0400000400040004), 
+  CONST64(0x0400040000040000), CONST64(0x0400040400040000), CONST64(0x0400040000040004), CONST64(0x0400040400040004), 
+  CONST64(0x0400000000040400), CONST64(0x0400000400040400), CONST64(0x0400000000040404), CONST64(0x0400000400040404), 
+  CONST64(0x0400040000040400), CONST64(0x0400040400040400), CONST64(0x0400040000040404), CONST64(0x0400040400040404), 
+  CONST64(0x0404000000040000), CONST64(0x0404000400040000), CONST64(0x0404000000040004), CONST64(0x0404000400040004), 
+  CONST64(0x0404040000040000), CONST64(0x0404040400040000), CONST64(0x0404040000040004), CONST64(0x0404040400040004), 
+  CONST64(0x0404000000040400), CONST64(0x0404000400040400), CONST64(0x0404000000040404), CONST64(0x0404000400040404), 
+  CONST64(0x0404040000040400), CONST64(0x0404040400040400), CONST64(0x0404040000040404), CONST64(0x0404040400040404), 
+  CONST64(0x0000000004000000), CONST64(0x0000000404000000), CONST64(0x0000000004000004), CONST64(0x0000000404000004), 
+  CONST64(0x0000040004000000), CONST64(0x0000040404000000), CONST64(0x0000040004000004), CONST64(0x0000040404000004), 
+  CONST64(0x0000000004000400), CONST64(0x0000000404000400), CONST64(0x0000000004000404), CONST64(0x0000000404000404), 
+  CONST64(0x0000040004000400), CONST64(0x0000040404000400), CONST64(0x0000040004000404), CONST64(0x0000040404000404), 
+  CONST64(0x0004000004000000), CONST64(0x0004000404000000), CONST64(0x0004000004000004), CONST64(0x0004000404000004), 
+  CONST64(0x0004040004000000), CONST64(0x0004040404000000), CONST64(0x0004040004000004), CONST64(0x0004040404000004), 
+  CONST64(0x0004000004000400), CONST64(0x0004000404000400), CONST64(0x0004000004000404), CONST64(0x0004000404000404), 
+  CONST64(0x0004040004000400), CONST64(0x0004040404000400), CONST64(0x0004040004000404), CONST64(0x0004040404000404), 
+  CONST64(0x0000000004040000), CONST64(0x0000000404040000), CONST64(0x0000000004040004), CONST64(0x0000000404040004), 
+  CONST64(0x0000040004040000), CONST64(0x0000040404040000), CONST64(0x0000040004040004), CONST64(0x0000040404040004), 
+  CONST64(0x0000000004040400), CONST64(0x0000000404040400), CONST64(0x0000000004040404), CONST64(0x0000000404040404), 
+  CONST64(0x0000040004040400), CONST64(0x0000040404040400), CONST64(0x0000040004040404), CONST64(0x0000040404040404), 
+  CONST64(0x0004000004040000), CONST64(0x0004000404040000), CONST64(0x0004000004040004), CONST64(0x0004000404040004), 
+  CONST64(0x0004040004040000), CONST64(0x0004040404040000), CONST64(0x0004040004040004), CONST64(0x0004040404040004), 
+  CONST64(0x0004000004040400), CONST64(0x0004000404040400), CONST64(0x0004000004040404), CONST64(0x0004000404040404), 
+  CONST64(0x0004040004040400), CONST64(0x0004040404040400), CONST64(0x0004040004040404), CONST64(0x0004040404040404), 
+  CONST64(0x0400000004000000), CONST64(0x0400000404000000), CONST64(0x0400000004000004), CONST64(0x0400000404000004), 
+  CONST64(0x0400040004000000), CONST64(0x0400040404000000), CONST64(0x0400040004000004), CONST64(0x0400040404000004), 
+  CONST64(0x0400000004000400), CONST64(0x0400000404000400), CONST64(0x0400000004000404), CONST64(0x0400000404000404), 
+  CONST64(0x0400040004000400), CONST64(0x0400040404000400), CONST64(0x0400040004000404), CONST64(0x0400040404000404), 
+  CONST64(0x0404000004000000), CONST64(0x0404000404000000), CONST64(0x0404000004000004), CONST64(0x0404000404000004), 
+  CONST64(0x0404040004000000), CONST64(0x0404040404000000), CONST64(0x0404040004000004), CONST64(0x0404040404000004), 
+  CONST64(0x0404000004000400), CONST64(0x0404000404000400), CONST64(0x0404000004000404), CONST64(0x0404000404000404), 
+  CONST64(0x0404040004000400), CONST64(0x0404040404000400), CONST64(0x0404040004000404), CONST64(0x0404040404000404), 
+  CONST64(0x0400000004040000), CONST64(0x0400000404040000), CONST64(0x0400000004040004), CONST64(0x0400000404040004), 
+  CONST64(0x0400040004040000), CONST64(0x0400040404040000), CONST64(0x0400040004040004), CONST64(0x0400040404040004), 
+  CONST64(0x0400000004040400), CONST64(0x0400000404040400), CONST64(0x0400000004040404), CONST64(0x0400000404040404), 
+  CONST64(0x0400040004040400), CONST64(0x0400040404040400), CONST64(0x0400040004040404), CONST64(0x0400040404040404), 
+  CONST64(0x0404000004040000), CONST64(0x0404000404040000), CONST64(0x0404000004040004), CONST64(0x0404000404040004), 
+  CONST64(0x0404040004040000), CONST64(0x0404040404040000), CONST64(0x0404040004040004), CONST64(0x0404040404040004), 
+  CONST64(0x0404000004040400), CONST64(0x0404000404040400), CONST64(0x0404000004040404), CONST64(0x0404000404040404), 
+  CONST64(0x0404040004040400), CONST64(0x0404040404040400), CONST64(0x0404040004040404), CONST64(0x0404040404040404)
+  }, 
+{ CONST64(0x0000000000000000), CONST64(0x0000000200000000), CONST64(0x0000000000000002), CONST64(0x0000000200000002), 
+  CONST64(0x0000020000000000), CONST64(0x0000020200000000), CONST64(0x0000020000000002), CONST64(0x0000020200000002), 
+  CONST64(0x0000000000000200), CONST64(0x0000000200000200), CONST64(0x0000000000000202), CONST64(0x0000000200000202), 
+  CONST64(0x0000020000000200), CONST64(0x0000020200000200), CONST64(0x0000020000000202), CONST64(0x0000020200000202), 
+  CONST64(0x0002000000000000), CONST64(0x0002000200000000), CONST64(0x0002000000000002), CONST64(0x0002000200000002), 
+  CONST64(0x0002020000000000), CONST64(0x0002020200000000), CONST64(0x0002020000000002), CONST64(0x0002020200000002), 
+  CONST64(0x0002000000000200), CONST64(0x0002000200000200), CONST64(0x0002000000000202), CONST64(0x0002000200000202), 
+  CONST64(0x0002020000000200), CONST64(0x0002020200000200), CONST64(0x0002020000000202), CONST64(0x0002020200000202), 
+  CONST64(0x0000000000020000), CONST64(0x0000000200020000), CONST64(0x0000000000020002), CONST64(0x0000000200020002), 
+  CONST64(0x0000020000020000), CONST64(0x0000020200020000), CONST64(0x0000020000020002), CONST64(0x0000020200020002), 
+  CONST64(0x0000000000020200), CONST64(0x0000000200020200), CONST64(0x0000000000020202), CONST64(0x0000000200020202), 
+  CONST64(0x0000020000020200), CONST64(0x0000020200020200), CONST64(0x0000020000020202), CONST64(0x0000020200020202), 
+  CONST64(0x0002000000020000), CONST64(0x0002000200020000), CONST64(0x0002000000020002), CONST64(0x0002000200020002), 
+  CONST64(0x0002020000020000), CONST64(0x0002020200020000), CONST64(0x0002020000020002), CONST64(0x0002020200020002), 
+  CONST64(0x0002000000020200), CONST64(0x0002000200020200), CONST64(0x0002000000020202), CONST64(0x0002000200020202), 
+  CONST64(0x0002020000020200), CONST64(0x0002020200020200), CONST64(0x0002020000020202), CONST64(0x0002020200020202), 
+  CONST64(0x0200000000000000), CONST64(0x0200000200000000), CONST64(0x0200000000000002), CONST64(0x0200000200000002), 
+  CONST64(0x0200020000000000), CONST64(0x0200020200000000), CONST64(0x0200020000000002), CONST64(0x0200020200000002), 
+  CONST64(0x0200000000000200), CONST64(0x0200000200000200), CONST64(0x0200000000000202), CONST64(0x0200000200000202), 
+  CONST64(0x0200020000000200), CONST64(0x0200020200000200), CONST64(0x0200020000000202), CONST64(0x0200020200000202), 
+  CONST64(0x0202000000000000), CONST64(0x0202000200000000), CONST64(0x0202000000000002), CONST64(0x0202000200000002), 
+  CONST64(0x0202020000000000), CONST64(0x0202020200000000), CONST64(0x0202020000000002), CONST64(0x0202020200000002), 
+  CONST64(0x0202000000000200), CONST64(0x0202000200000200), CONST64(0x0202000000000202), CONST64(0x0202000200000202), 
+  CONST64(0x0202020000000200), CONST64(0x0202020200000200), CONST64(0x0202020000000202), CONST64(0x0202020200000202), 
+  CONST64(0x0200000000020000), CONST64(0x0200000200020000), CONST64(0x0200000000020002), CONST64(0x0200000200020002), 
+  CONST64(0x0200020000020000), CONST64(0x0200020200020000), CONST64(0x0200020000020002), CONST64(0x0200020200020002), 
+  CONST64(0x0200000000020200), CONST64(0x0200000200020200), CONST64(0x0200000000020202), CONST64(0x0200000200020202), 
+  CONST64(0x0200020000020200), CONST64(0x0200020200020200), CONST64(0x0200020000020202), CONST64(0x0200020200020202), 
+  CONST64(0x0202000000020000), CONST64(0x0202000200020000), CONST64(0x0202000000020002), CONST64(0x0202000200020002), 
+  CONST64(0x0202020000020000), CONST64(0x0202020200020000), CONST64(0x0202020000020002), CONST64(0x0202020200020002), 
+  CONST64(0x0202000000020200), CONST64(0x0202000200020200), CONST64(0x0202000000020202), CONST64(0x0202000200020202), 
+  CONST64(0x0202020000020200), CONST64(0x0202020200020200), CONST64(0x0202020000020202), CONST64(0x0202020200020202), 
+  CONST64(0x0000000002000000), CONST64(0x0000000202000000), CONST64(0x0000000002000002), CONST64(0x0000000202000002), 
+  CONST64(0x0000020002000000), CONST64(0x0000020202000000), CONST64(0x0000020002000002), CONST64(0x0000020202000002), 
+  CONST64(0x0000000002000200), CONST64(0x0000000202000200), CONST64(0x0000000002000202), CONST64(0x0000000202000202), 
+  CONST64(0x0000020002000200), CONST64(0x0000020202000200), CONST64(0x0000020002000202), CONST64(0x0000020202000202), 
+  CONST64(0x0002000002000000), CONST64(0x0002000202000000), CONST64(0x0002000002000002), CONST64(0x0002000202000002), 
+  CONST64(0x0002020002000000), CONST64(0x0002020202000000), CONST64(0x0002020002000002), CONST64(0x0002020202000002), 
+  CONST64(0x0002000002000200), CONST64(0x0002000202000200), CONST64(0x0002000002000202), CONST64(0x0002000202000202), 
+  CONST64(0x0002020002000200), CONST64(0x0002020202000200), CONST64(0x0002020002000202), CONST64(0x0002020202000202), 
+  CONST64(0x0000000002020000), CONST64(0x0000000202020000), CONST64(0x0000000002020002), CONST64(0x0000000202020002), 
+  CONST64(0x0000020002020000), CONST64(0x0000020202020000), CONST64(0x0000020002020002), CONST64(0x0000020202020002), 
+  CONST64(0x0000000002020200), CONST64(0x0000000202020200), CONST64(0x0000000002020202), CONST64(0x0000000202020202), 
+  CONST64(0x0000020002020200), CONST64(0x0000020202020200), CONST64(0x0000020002020202), CONST64(0x0000020202020202), 
+  CONST64(0x0002000002020000), CONST64(0x0002000202020000), CONST64(0x0002000002020002), CONST64(0x0002000202020002), 
+  CONST64(0x0002020002020000), CONST64(0x0002020202020000), CONST64(0x0002020002020002), CONST64(0x0002020202020002), 
+  CONST64(0x0002000002020200), CONST64(0x0002000202020200), CONST64(0x0002000002020202), CONST64(0x0002000202020202), 
+  CONST64(0x0002020002020200), CONST64(0x0002020202020200), CONST64(0x0002020002020202), CONST64(0x0002020202020202), 
+  CONST64(0x0200000002000000), CONST64(0x0200000202000000), CONST64(0x0200000002000002), CONST64(0x0200000202000002), 
+  CONST64(0x0200020002000000), CONST64(0x0200020202000000), CONST64(0x0200020002000002), CONST64(0x0200020202000002), 
+  CONST64(0x0200000002000200), CONST64(0x0200000202000200), CONST64(0x0200000002000202), CONST64(0x0200000202000202), 
+  CONST64(0x0200020002000200), CONST64(0x0200020202000200), CONST64(0x0200020002000202), CONST64(0x0200020202000202), 
+  CONST64(0x0202000002000000), CONST64(0x0202000202000000), CONST64(0x0202000002000002), CONST64(0x0202000202000002), 
+  CONST64(0x0202020002000000), CONST64(0x0202020202000000), CONST64(0x0202020002000002), CONST64(0x0202020202000002), 
+  CONST64(0x0202000002000200), CONST64(0x0202000202000200), CONST64(0x0202000002000202), CONST64(0x0202000202000202), 
+  CONST64(0x0202020002000200), CONST64(0x0202020202000200), CONST64(0x0202020002000202), CONST64(0x0202020202000202), 
+  CONST64(0x0200000002020000), CONST64(0x0200000202020000), CONST64(0x0200000002020002), CONST64(0x0200000202020002), 
+  CONST64(0x0200020002020000), CONST64(0x0200020202020000), CONST64(0x0200020002020002), CONST64(0x0200020202020002), 
+  CONST64(0x0200000002020200), CONST64(0x0200000202020200), CONST64(0x0200000002020202), CONST64(0x0200000202020202), 
+  CONST64(0x0200020002020200), CONST64(0x0200020202020200), CONST64(0x0200020002020202), CONST64(0x0200020202020202), 
+  CONST64(0x0202000002020000), CONST64(0x0202000202020000), CONST64(0x0202000002020002), CONST64(0x0202000202020002), 
+  CONST64(0x0202020002020000), CONST64(0x0202020202020000), CONST64(0x0202020002020002), CONST64(0x0202020202020002), 
+  CONST64(0x0202000002020200), CONST64(0x0202000202020200), CONST64(0x0202000002020202), CONST64(0x0202000202020202), 
+  CONST64(0x0202020002020200), CONST64(0x0202020202020200), CONST64(0x0202020002020202), CONST64(0x0202020202020202)
+  }, 
+{ CONST64(0x0000000000000000), CONST64(0x0000010000000000), CONST64(0x0000000000000100), CONST64(0x0000010000000100), 
+  CONST64(0x0001000000000000), CONST64(0x0001010000000000), CONST64(0x0001000000000100), CONST64(0x0001010000000100), 
+  CONST64(0x0000000000010000), CONST64(0x0000010000010000), CONST64(0x0000000000010100), CONST64(0x0000010000010100), 
+  CONST64(0x0001000000010000), CONST64(0x0001010000010000), CONST64(0x0001000000010100), CONST64(0x0001010000010100), 
+  CONST64(0x0100000000000000), CONST64(0x0100010000000000), CONST64(0x0100000000000100), CONST64(0x0100010000000100), 
+  CONST64(0x0101000000000000), CONST64(0x0101010000000000), CONST64(0x0101000000000100), CONST64(0x0101010000000100), 
+  CONST64(0x0100000000010000), CONST64(0x0100010000010000), CONST64(0x0100000000010100), CONST64(0x0100010000010100), 
+  CONST64(0x0101000000010000), CONST64(0x0101010000010000), CONST64(0x0101000000010100), CONST64(0x0101010000010100), 
+  CONST64(0x0000000001000000), CONST64(0x0000010001000000), CONST64(0x0000000001000100), CONST64(0x0000010001000100), 
+  CONST64(0x0001000001000000), CONST64(0x0001010001000000), CONST64(0x0001000001000100), CONST64(0x0001010001000100), 
+  CONST64(0x0000000001010000), CONST64(0x0000010001010000), CONST64(0x0000000001010100), CONST64(0x0000010001010100), 
+  CONST64(0x0001000001010000), CONST64(0x0001010001010000), CONST64(0x0001000001010100), CONST64(0x0001010001010100), 
+  CONST64(0x0100000001000000), CONST64(0x0100010001000000), CONST64(0x0100000001000100), CONST64(0x0100010001000100), 
+  CONST64(0x0101000001000000), CONST64(0x0101010001000000), CONST64(0x0101000001000100), CONST64(0x0101010001000100), 
+  CONST64(0x0100000001010000), CONST64(0x0100010001010000), CONST64(0x0100000001010100), CONST64(0x0100010001010100), 
+  CONST64(0x0101000001010000), CONST64(0x0101010001010000), CONST64(0x0101000001010100), CONST64(0x0101010001010100), 
+  CONST64(0x0000000100000000), CONST64(0x0000010100000000), CONST64(0x0000000100000100), CONST64(0x0000010100000100), 
+  CONST64(0x0001000100000000), CONST64(0x0001010100000000), CONST64(0x0001000100000100), CONST64(0x0001010100000100), 
+  CONST64(0x0000000100010000), CONST64(0x0000010100010000), CONST64(0x0000000100010100), CONST64(0x0000010100010100), 
+  CONST64(0x0001000100010000), CONST64(0x0001010100010000), CONST64(0x0001000100010100), CONST64(0x0001010100010100), 
+  CONST64(0x0100000100000000), CONST64(0x0100010100000000), CONST64(0x0100000100000100), CONST64(0x0100010100000100), 
+  CONST64(0x0101000100000000), CONST64(0x0101010100000000), CONST64(0x0101000100000100), CONST64(0x0101010100000100), 
+  CONST64(0x0100000100010000), CONST64(0x0100010100010000), CONST64(0x0100000100010100), CONST64(0x0100010100010100), 
+  CONST64(0x0101000100010000), CONST64(0x0101010100010000), CONST64(0x0101000100010100), CONST64(0x0101010100010100), 
+  CONST64(0x0000000101000000), CONST64(0x0000010101000000), CONST64(0x0000000101000100), CONST64(0x0000010101000100), 
+  CONST64(0x0001000101000000), CONST64(0x0001010101000000), CONST64(0x0001000101000100), CONST64(0x0001010101000100), 
+  CONST64(0x0000000101010000), CONST64(0x0000010101010000), CONST64(0x0000000101010100), CONST64(0x0000010101010100), 
+  CONST64(0x0001000101010000), CONST64(0x0001010101010000), CONST64(0x0001000101010100), CONST64(0x0001010101010100), 
+  CONST64(0x0100000101000000), CONST64(0x0100010101000000), CONST64(0x0100000101000100), CONST64(0x0100010101000100), 
+  CONST64(0x0101000101000000), CONST64(0x0101010101000000), CONST64(0x0101000101000100), CONST64(0x0101010101000100), 
+  CONST64(0x0100000101010000), CONST64(0x0100010101010000), CONST64(0x0100000101010100), CONST64(0x0100010101010100), 
+  CONST64(0x0101000101010000), CONST64(0x0101010101010000), CONST64(0x0101000101010100), CONST64(0x0101010101010100), 
+  CONST64(0x0000000000000001), CONST64(0x0000010000000001), CONST64(0x0000000000000101), CONST64(0x0000010000000101), 
+  CONST64(0x0001000000000001), CONST64(0x0001010000000001), CONST64(0x0001000000000101), CONST64(0x0001010000000101), 
+  CONST64(0x0000000000010001), CONST64(0x0000010000010001), CONST64(0x0000000000010101), CONST64(0x0000010000010101), 
+  CONST64(0x0001000000010001), CONST64(0x0001010000010001), CONST64(0x0001000000010101), CONST64(0x0001010000010101), 
+  CONST64(0x0100000000000001), CONST64(0x0100010000000001), CONST64(0x0100000000000101), CONST64(0x0100010000000101), 
+  CONST64(0x0101000000000001), CONST64(0x0101010000000001), CONST64(0x0101000000000101), CONST64(0x0101010000000101), 
+  CONST64(0x0100000000010001), CONST64(0x0100010000010001), CONST64(0x0100000000010101), CONST64(0x0100010000010101), 
+  CONST64(0x0101000000010001), CONST64(0x0101010000010001), CONST64(0x0101000000010101), CONST64(0x0101010000010101), 
+  CONST64(0x0000000001000001), CONST64(0x0000010001000001), CONST64(0x0000000001000101), CONST64(0x0000010001000101), 
+  CONST64(0x0001000001000001), CONST64(0x0001010001000001), CONST64(0x0001000001000101), CONST64(0x0001010001000101), 
+  CONST64(0x0000000001010001), CONST64(0x0000010001010001), CONST64(0x0000000001010101), CONST64(0x0000010001010101), 
+  CONST64(0x0001000001010001), CONST64(0x0001010001010001), CONST64(0x0001000001010101), CONST64(0x0001010001010101), 
+  CONST64(0x0100000001000001), CONST64(0x0100010001000001), CONST64(0x0100000001000101), CONST64(0x0100010001000101), 
+  CONST64(0x0101000001000001), CONST64(0x0101010001000001), CONST64(0x0101000001000101), CONST64(0x0101010001000101), 
+  CONST64(0x0100000001010001), CONST64(0x0100010001010001), CONST64(0x0100000001010101), CONST64(0x0100010001010101), 
+  CONST64(0x0101000001010001), CONST64(0x0101010001010001), CONST64(0x0101000001010101), CONST64(0x0101010001010101), 
+  CONST64(0x0000000100000001), CONST64(0x0000010100000001), CONST64(0x0000000100000101), CONST64(0x0000010100000101), 
+  CONST64(0x0001000100000001), CONST64(0x0001010100000001), CONST64(0x0001000100000101), CONST64(0x0001010100000101), 
+  CONST64(0x0000000100010001), CONST64(0x0000010100010001), CONST64(0x0000000100010101), CONST64(0x0000010100010101), 
+  CONST64(0x0001000100010001), CONST64(0x0001010100010001), CONST64(0x0001000100010101), CONST64(0x0001010100010101), 
+  CONST64(0x0100000100000001), CONST64(0x0100010100000001), CONST64(0x0100000100000101), CONST64(0x0100010100000101), 
+  CONST64(0x0101000100000001), CONST64(0x0101010100000001), CONST64(0x0101000100000101), CONST64(0x0101010100000101), 
+  CONST64(0x0100000100010001), CONST64(0x0100010100010001), CONST64(0x0100000100010101), CONST64(0x0100010100010101), 
+  CONST64(0x0101000100010001), CONST64(0x0101010100010001), CONST64(0x0101000100010101), CONST64(0x0101010100010101), 
+  CONST64(0x0000000101000001), CONST64(0x0000010101000001), CONST64(0x0000000101000101), CONST64(0x0000010101000101), 
+  CONST64(0x0001000101000001), CONST64(0x0001010101000001), CONST64(0x0001000101000101), CONST64(0x0001010101000101), 
+  CONST64(0x0000000101010001), CONST64(0x0000010101010001), CONST64(0x0000000101010101), CONST64(0x0000010101010101), 
+  CONST64(0x0001000101010001), CONST64(0x0001010101010001), CONST64(0x0001000101010101), CONST64(0x0001010101010101), 
+  CONST64(0x0100000101000001), CONST64(0x0100010101000001), CONST64(0x0100000101000101), CONST64(0x0100010101000101), 
+  CONST64(0x0101000101000001), CONST64(0x0101010101000001), CONST64(0x0101000101000101), CONST64(0x0101010101000101), 
+  CONST64(0x0100000101010001), CONST64(0x0100010101010001), CONST64(0x0100000101010101), CONST64(0x0100010101010101), 
+  CONST64(0x0101000101010001), CONST64(0x0101010101010001), CONST64(0x0101000101010101), CONST64(0x0101010101010101)
+  }, 
+{ CONST64(0x0000000000000000), CONST64(0x0000008000000000), CONST64(0x0000000000000080), CONST64(0x0000008000000080), 
+  CONST64(0x0000800000000000), CONST64(0x0000808000000000), CONST64(0x0000800000000080), CONST64(0x0000808000000080), 
+  CONST64(0x0000000000008000), CONST64(0x0000008000008000), CONST64(0x0000000000008080), CONST64(0x0000008000008080), 
+  CONST64(0x0000800000008000), CONST64(0x0000808000008000), CONST64(0x0000800000008080), CONST64(0x0000808000008080), 
+  CONST64(0x0080000000000000), CONST64(0x0080008000000000), CONST64(0x0080000000000080), CONST64(0x0080008000000080), 
+  CONST64(0x0080800000000000), CONST64(0x0080808000000000), CONST64(0x0080800000000080), CONST64(0x0080808000000080), 
+  CONST64(0x0080000000008000), CONST64(0x0080008000008000), CONST64(0x0080000000008080), CONST64(0x0080008000008080), 
+  CONST64(0x0080800000008000), CONST64(0x0080808000008000), CONST64(0x0080800000008080), CONST64(0x0080808000008080), 
+  CONST64(0x0000000000800000), CONST64(0x0000008000800000), CONST64(0x0000000000800080), CONST64(0x0000008000800080), 
+  CONST64(0x0000800000800000), CONST64(0x0000808000800000), CONST64(0x0000800000800080), CONST64(0x0000808000800080), 
+  CONST64(0x0000000000808000), CONST64(0x0000008000808000), CONST64(0x0000000000808080), CONST64(0x0000008000808080), 
+  CONST64(0x0000800000808000), CONST64(0x0000808000808000), CONST64(0x0000800000808080), CONST64(0x0000808000808080), 
+  CONST64(0x0080000000800000), CONST64(0x0080008000800000), CONST64(0x0080000000800080), CONST64(0x0080008000800080), 
+  CONST64(0x0080800000800000), CONST64(0x0080808000800000), CONST64(0x0080800000800080), CONST64(0x0080808000800080), 
+  CONST64(0x0080000000808000), CONST64(0x0080008000808000), CONST64(0x0080000000808080), CONST64(0x0080008000808080), 
+  CONST64(0x0080800000808000), CONST64(0x0080808000808000), CONST64(0x0080800000808080), CONST64(0x0080808000808080), 
+  CONST64(0x8000000000000000), CONST64(0x8000008000000000), CONST64(0x8000000000000080), CONST64(0x8000008000000080), 
+  CONST64(0x8000800000000000), CONST64(0x8000808000000000), CONST64(0x8000800000000080), CONST64(0x8000808000000080), 
+  CONST64(0x8000000000008000), CONST64(0x8000008000008000), CONST64(0x8000000000008080), CONST64(0x8000008000008080), 
+  CONST64(0x8000800000008000), CONST64(0x8000808000008000), CONST64(0x8000800000008080), CONST64(0x8000808000008080), 
+  CONST64(0x8080000000000000), CONST64(0x8080008000000000), CONST64(0x8080000000000080), CONST64(0x8080008000000080), 
+  CONST64(0x8080800000000000), CONST64(0x8080808000000000), CONST64(0x8080800000000080), CONST64(0x8080808000000080), 
+  CONST64(0x8080000000008000), CONST64(0x8080008000008000), CONST64(0x8080000000008080), CONST64(0x8080008000008080), 
+  CONST64(0x8080800000008000), CONST64(0x8080808000008000), CONST64(0x8080800000008080), CONST64(0x8080808000008080), 
+  CONST64(0x8000000000800000), CONST64(0x8000008000800000), CONST64(0x8000000000800080), CONST64(0x8000008000800080), 
+  CONST64(0x8000800000800000), CONST64(0x8000808000800000), CONST64(0x8000800000800080), CONST64(0x8000808000800080), 
+  CONST64(0x8000000000808000), CONST64(0x8000008000808000), CONST64(0x8000000000808080), CONST64(0x8000008000808080), 
+  CONST64(0x8000800000808000), CONST64(0x8000808000808000), CONST64(0x8000800000808080), CONST64(0x8000808000808080), 
+  CONST64(0x8080000000800000), CONST64(0x8080008000800000), CONST64(0x8080000000800080), CONST64(0x8080008000800080), 
+  CONST64(0x8080800000800000), CONST64(0x8080808000800000), CONST64(0x8080800000800080), CONST64(0x8080808000800080), 
+  CONST64(0x8080000000808000), CONST64(0x8080008000808000), CONST64(0x8080000000808080), CONST64(0x8080008000808080), 
+  CONST64(0x8080800000808000), CONST64(0x8080808000808000), CONST64(0x8080800000808080), CONST64(0x8080808000808080), 
+  CONST64(0x0000000080000000), CONST64(0x0000008080000000), CONST64(0x0000000080000080), CONST64(0x0000008080000080), 
+  CONST64(0x0000800080000000), CONST64(0x0000808080000000), CONST64(0x0000800080000080), CONST64(0x0000808080000080), 
+  CONST64(0x0000000080008000), CONST64(0x0000008080008000), CONST64(0x0000000080008080), CONST64(0x0000008080008080), 
+  CONST64(0x0000800080008000), CONST64(0x0000808080008000), CONST64(0x0000800080008080), CONST64(0x0000808080008080), 
+  CONST64(0x0080000080000000), CONST64(0x0080008080000000), CONST64(0x0080000080000080), CONST64(0x0080008080000080), 
+  CONST64(0x0080800080000000), CONST64(0x0080808080000000), CONST64(0x0080800080000080), CONST64(0x0080808080000080), 
+  CONST64(0x0080000080008000), CONST64(0x0080008080008000), CONST64(0x0080000080008080), CONST64(0x0080008080008080), 
+  CONST64(0x0080800080008000), CONST64(0x0080808080008000), CONST64(0x0080800080008080), CONST64(0x0080808080008080), 
+  CONST64(0x0000000080800000), CONST64(0x0000008080800000), CONST64(0x0000000080800080), CONST64(0x0000008080800080), 
+  CONST64(0x0000800080800000), CONST64(0x0000808080800000), CONST64(0x0000800080800080), CONST64(0x0000808080800080), 
+  CONST64(0x0000000080808000), CONST64(0x0000008080808000), CONST64(0x0000000080808080), CONST64(0x0000008080808080), 
+  CONST64(0x0000800080808000), CONST64(0x0000808080808000), CONST64(0x0000800080808080), CONST64(0x0000808080808080), 
+  CONST64(0x0080000080800000), CONST64(0x0080008080800000), CONST64(0x0080000080800080), CONST64(0x0080008080800080), 
+  CONST64(0x0080800080800000), CONST64(0x0080808080800000), CONST64(0x0080800080800080), CONST64(0x0080808080800080), 
+  CONST64(0x0080000080808000), CONST64(0x0080008080808000), CONST64(0x0080000080808080), CONST64(0x0080008080808080), 
+  CONST64(0x0080800080808000), CONST64(0x0080808080808000), CONST64(0x0080800080808080), CONST64(0x0080808080808080), 
+  CONST64(0x8000000080000000), CONST64(0x8000008080000000), CONST64(0x8000000080000080), CONST64(0x8000008080000080), 
+  CONST64(0x8000800080000000), CONST64(0x8000808080000000), CONST64(0x8000800080000080), CONST64(0x8000808080000080), 
+  CONST64(0x8000000080008000), CONST64(0x8000008080008000), CONST64(0x8000000080008080), CONST64(0x8000008080008080), 
+  CONST64(0x8000800080008000), CONST64(0x8000808080008000), CONST64(0x8000800080008080), CONST64(0x8000808080008080), 
+  CONST64(0x8080000080000000), CONST64(0x8080008080000000), CONST64(0x8080000080000080), CONST64(0x8080008080000080), 
+  CONST64(0x8080800080000000), CONST64(0x8080808080000000), CONST64(0x8080800080000080), CONST64(0x8080808080000080), 
+  CONST64(0x8080000080008000), CONST64(0x8080008080008000), CONST64(0x8080000080008080), CONST64(0x8080008080008080), 
+  CONST64(0x8080800080008000), CONST64(0x8080808080008000), CONST64(0x8080800080008080), CONST64(0x8080808080008080), 
+  CONST64(0x8000000080800000), CONST64(0x8000008080800000), CONST64(0x8000000080800080), CONST64(0x8000008080800080), 
+  CONST64(0x8000800080800000), CONST64(0x8000808080800000), CONST64(0x8000800080800080), CONST64(0x8000808080800080), 
+  CONST64(0x8000000080808000), CONST64(0x8000008080808000), CONST64(0x8000000080808080), CONST64(0x8000008080808080), 
+  CONST64(0x8000800080808000), CONST64(0x8000808080808000), CONST64(0x8000800080808080), CONST64(0x8000808080808080), 
+  CONST64(0x8080000080800000), CONST64(0x8080008080800000), CONST64(0x8080000080800080), CONST64(0x8080008080800080), 
+  CONST64(0x8080800080800000), CONST64(0x8080808080800000), CONST64(0x8080800080800080), CONST64(0x8080808080800080), 
+  CONST64(0x8080000080808000), CONST64(0x8080008080808000), CONST64(0x8080000080808080), CONST64(0x8080008080808080), 
+  CONST64(0x8080800080808000), CONST64(0x8080808080808000), CONST64(0x8080800080808080), CONST64(0x8080808080808080)
+  }, 
+{ CONST64(0x0000000000000000), CONST64(0x0000004000000000), CONST64(0x0000000000000040), CONST64(0x0000004000000040), 
+  CONST64(0x0000400000000000), CONST64(0x0000404000000000), CONST64(0x0000400000000040), CONST64(0x0000404000000040), 
+  CONST64(0x0000000000004000), CONST64(0x0000004000004000), CONST64(0x0000000000004040), CONST64(0x0000004000004040), 
+  CONST64(0x0000400000004000), CONST64(0x0000404000004000), CONST64(0x0000400000004040), CONST64(0x0000404000004040), 
+  CONST64(0x0040000000000000), CONST64(0x0040004000000000), CONST64(0x0040000000000040), CONST64(0x0040004000000040), 
+  CONST64(0x0040400000000000), CONST64(0x0040404000000000), CONST64(0x0040400000000040), CONST64(0x0040404000000040), 
+  CONST64(0x0040000000004000), CONST64(0x0040004000004000), CONST64(0x0040000000004040), CONST64(0x0040004000004040), 
+  CONST64(0x0040400000004000), CONST64(0x0040404000004000), CONST64(0x0040400000004040), CONST64(0x0040404000004040), 
+  CONST64(0x0000000000400000), CONST64(0x0000004000400000), CONST64(0x0000000000400040), CONST64(0x0000004000400040), 
+  CONST64(0x0000400000400000), CONST64(0x0000404000400000), CONST64(0x0000400000400040), CONST64(0x0000404000400040), 
+  CONST64(0x0000000000404000), CONST64(0x0000004000404000), CONST64(0x0000000000404040), CONST64(0x0000004000404040), 
+  CONST64(0x0000400000404000), CONST64(0x0000404000404000), CONST64(0x0000400000404040), CONST64(0x0000404000404040), 
+  CONST64(0x0040000000400000), CONST64(0x0040004000400000), CONST64(0x0040000000400040), CONST64(0x0040004000400040), 
+  CONST64(0x0040400000400000), CONST64(0x0040404000400000), CONST64(0x0040400000400040), CONST64(0x0040404000400040), 
+  CONST64(0x0040000000404000), CONST64(0x0040004000404000), CONST64(0x0040000000404040), CONST64(0x0040004000404040), 
+  CONST64(0x0040400000404000), CONST64(0x0040404000404000), CONST64(0x0040400000404040), CONST64(0x0040404000404040), 
+  CONST64(0x4000000000000000), CONST64(0x4000004000000000), CONST64(0x4000000000000040), CONST64(0x4000004000000040), 
+  CONST64(0x4000400000000000), CONST64(0x4000404000000000), CONST64(0x4000400000000040), CONST64(0x4000404000000040), 
+  CONST64(0x4000000000004000), CONST64(0x4000004000004000), CONST64(0x4000000000004040), CONST64(0x4000004000004040), 
+  CONST64(0x4000400000004000), CONST64(0x4000404000004000), CONST64(0x4000400000004040), CONST64(0x4000404000004040), 
+  CONST64(0x4040000000000000), CONST64(0x4040004000000000), CONST64(0x4040000000000040), CONST64(0x4040004000000040), 
+  CONST64(0x4040400000000000), CONST64(0x4040404000000000), CONST64(0x4040400000000040), CONST64(0x4040404000000040), 
+  CONST64(0x4040000000004000), CONST64(0x4040004000004000), CONST64(0x4040000000004040), CONST64(0x4040004000004040), 
+  CONST64(0x4040400000004000), CONST64(0x4040404000004000), CONST64(0x4040400000004040), CONST64(0x4040404000004040), 
+  CONST64(0x4000000000400000), CONST64(0x4000004000400000), CONST64(0x4000000000400040), CONST64(0x4000004000400040), 
+  CONST64(0x4000400000400000), CONST64(0x4000404000400000), CONST64(0x4000400000400040), CONST64(0x4000404000400040), 
+  CONST64(0x4000000000404000), CONST64(0x4000004000404000), CONST64(0x4000000000404040), CONST64(0x4000004000404040), 
+  CONST64(0x4000400000404000), CONST64(0x4000404000404000), CONST64(0x4000400000404040), CONST64(0x4000404000404040), 
+  CONST64(0x4040000000400000), CONST64(0x4040004000400000), CONST64(0x4040000000400040), CONST64(0x4040004000400040), 
+  CONST64(0x4040400000400000), CONST64(0x4040404000400000), CONST64(0x4040400000400040), CONST64(0x4040404000400040), 
+  CONST64(0x4040000000404000), CONST64(0x4040004000404000), CONST64(0x4040000000404040), CONST64(0x4040004000404040), 
+  CONST64(0x4040400000404000), CONST64(0x4040404000404000), CONST64(0x4040400000404040), CONST64(0x4040404000404040), 
+  CONST64(0x0000000040000000), CONST64(0x0000004040000000), CONST64(0x0000000040000040), CONST64(0x0000004040000040), 
+  CONST64(0x0000400040000000), CONST64(0x0000404040000000), CONST64(0x0000400040000040), CONST64(0x0000404040000040), 
+  CONST64(0x0000000040004000), CONST64(0x0000004040004000), CONST64(0x0000000040004040), CONST64(0x0000004040004040), 
+  CONST64(0x0000400040004000), CONST64(0x0000404040004000), CONST64(0x0000400040004040), CONST64(0x0000404040004040), 
+  CONST64(0x0040000040000000), CONST64(0x0040004040000000), CONST64(0x0040000040000040), CONST64(0x0040004040000040), 
+  CONST64(0x0040400040000000), CONST64(0x0040404040000000), CONST64(0x0040400040000040), CONST64(0x0040404040000040), 
+  CONST64(0x0040000040004000), CONST64(0x0040004040004000), CONST64(0x0040000040004040), CONST64(0x0040004040004040), 
+  CONST64(0x0040400040004000), CONST64(0x0040404040004000), CONST64(0x0040400040004040), CONST64(0x0040404040004040), 
+  CONST64(0x0000000040400000), CONST64(0x0000004040400000), CONST64(0x0000000040400040), CONST64(0x0000004040400040), 
+  CONST64(0x0000400040400000), CONST64(0x0000404040400000), CONST64(0x0000400040400040), CONST64(0x0000404040400040), 
+  CONST64(0x0000000040404000), CONST64(0x0000004040404000), CONST64(0x0000000040404040), CONST64(0x0000004040404040), 
+  CONST64(0x0000400040404000), CONST64(0x0000404040404000), CONST64(0x0000400040404040), CONST64(0x0000404040404040), 
+  CONST64(0x0040000040400000), CONST64(0x0040004040400000), CONST64(0x0040000040400040), CONST64(0x0040004040400040), 
+  CONST64(0x0040400040400000), CONST64(0x0040404040400000), CONST64(0x0040400040400040), CONST64(0x0040404040400040), 
+  CONST64(0x0040000040404000), CONST64(0x0040004040404000), CONST64(0x0040000040404040), CONST64(0x0040004040404040), 
+  CONST64(0x0040400040404000), CONST64(0x0040404040404000), CONST64(0x0040400040404040), CONST64(0x0040404040404040), 
+  CONST64(0x4000000040000000), CONST64(0x4000004040000000), CONST64(0x4000000040000040), CONST64(0x4000004040000040), 
+  CONST64(0x4000400040000000), CONST64(0x4000404040000000), CONST64(0x4000400040000040), CONST64(0x4000404040000040), 
+  CONST64(0x4000000040004000), CONST64(0x4000004040004000), CONST64(0x4000000040004040), CONST64(0x4000004040004040), 
+  CONST64(0x4000400040004000), CONST64(0x4000404040004000), CONST64(0x4000400040004040), CONST64(0x4000404040004040), 
+  CONST64(0x4040000040000000), CONST64(0x4040004040000000), CONST64(0x4040000040000040), CONST64(0x4040004040000040), 
+  CONST64(0x4040400040000000), CONST64(0x4040404040000000), CONST64(0x4040400040000040), CONST64(0x4040404040000040), 
+  CONST64(0x4040000040004000), CONST64(0x4040004040004000), CONST64(0x4040000040004040), CONST64(0x4040004040004040), 
+  CONST64(0x4040400040004000), CONST64(0x4040404040004000), CONST64(0x4040400040004040), CONST64(0x4040404040004040), 
+  CONST64(0x4000000040400000), CONST64(0x4000004040400000), CONST64(0x4000000040400040), CONST64(0x4000004040400040), 
+  CONST64(0x4000400040400000), CONST64(0x4000404040400000), CONST64(0x4000400040400040), CONST64(0x4000404040400040), 
+  CONST64(0x4000000040404000), CONST64(0x4000004040404000), CONST64(0x4000000040404040), CONST64(0x4000004040404040), 
+  CONST64(0x4000400040404000), CONST64(0x4000404040404000), CONST64(0x4000400040404040), CONST64(0x4000404040404040), 
+  CONST64(0x4040000040400000), CONST64(0x4040004040400000), CONST64(0x4040000040400040), CONST64(0x4040004040400040), 
+  CONST64(0x4040400040400000), CONST64(0x4040404040400000), CONST64(0x4040400040400040), CONST64(0x4040404040400040), 
+  CONST64(0x4040000040404000), CONST64(0x4040004040404000), CONST64(0x4040000040404040), CONST64(0x4040004040404040), 
+  CONST64(0x4040400040404000), CONST64(0x4040404040404000), CONST64(0x4040400040404040), CONST64(0x4040404040404040)
+  }, 
+{ CONST64(0x0000000000000000), CONST64(0x0000002000000000), CONST64(0x0000000000000020), CONST64(0x0000002000000020), 
+  CONST64(0x0000200000000000), CONST64(0x0000202000000000), CONST64(0x0000200000000020), CONST64(0x0000202000000020), 
+  CONST64(0x0000000000002000), CONST64(0x0000002000002000), CONST64(0x0000000000002020), CONST64(0x0000002000002020), 
+  CONST64(0x0000200000002000), CONST64(0x0000202000002000), CONST64(0x0000200000002020), CONST64(0x0000202000002020), 
+  CONST64(0x0020000000000000), CONST64(0x0020002000000000), CONST64(0x0020000000000020), CONST64(0x0020002000000020), 
+  CONST64(0x0020200000000000), CONST64(0x0020202000000000), CONST64(0x0020200000000020), CONST64(0x0020202000000020), 
+  CONST64(0x0020000000002000), CONST64(0x0020002000002000), CONST64(0x0020000000002020), CONST64(0x0020002000002020), 
+  CONST64(0x0020200000002000), CONST64(0x0020202000002000), CONST64(0x0020200000002020), CONST64(0x0020202000002020), 
+  CONST64(0x0000000000200000), CONST64(0x0000002000200000), CONST64(0x0000000000200020), CONST64(0x0000002000200020), 
+  CONST64(0x0000200000200000), CONST64(0x0000202000200000), CONST64(0x0000200000200020), CONST64(0x0000202000200020), 
+  CONST64(0x0000000000202000), CONST64(0x0000002000202000), CONST64(0x0000000000202020), CONST64(0x0000002000202020), 
+  CONST64(0x0000200000202000), CONST64(0x0000202000202000), CONST64(0x0000200000202020), CONST64(0x0000202000202020), 
+  CONST64(0x0020000000200000), CONST64(0x0020002000200000), CONST64(0x0020000000200020), CONST64(0x0020002000200020), 
+  CONST64(0x0020200000200000), CONST64(0x0020202000200000), CONST64(0x0020200000200020), CONST64(0x0020202000200020), 
+  CONST64(0x0020000000202000), CONST64(0x0020002000202000), CONST64(0x0020000000202020), CONST64(0x0020002000202020), 
+  CONST64(0x0020200000202000), CONST64(0x0020202000202000), CONST64(0x0020200000202020), CONST64(0x0020202000202020), 
+  CONST64(0x2000000000000000), CONST64(0x2000002000000000), CONST64(0x2000000000000020), CONST64(0x2000002000000020), 
+  CONST64(0x2000200000000000), CONST64(0x2000202000000000), CONST64(0x2000200000000020), CONST64(0x2000202000000020), 
+  CONST64(0x2000000000002000), CONST64(0x2000002000002000), CONST64(0x2000000000002020), CONST64(0x2000002000002020), 
+  CONST64(0x2000200000002000), CONST64(0x2000202000002000), CONST64(0x2000200000002020), CONST64(0x2000202000002020), 
+  CONST64(0x2020000000000000), CONST64(0x2020002000000000), CONST64(0x2020000000000020), CONST64(0x2020002000000020), 
+  CONST64(0x2020200000000000), CONST64(0x2020202000000000), CONST64(0x2020200000000020), CONST64(0x2020202000000020), 
+  CONST64(0x2020000000002000), CONST64(0x2020002000002000), CONST64(0x2020000000002020), CONST64(0x2020002000002020), 
+  CONST64(0x2020200000002000), CONST64(0x2020202000002000), CONST64(0x2020200000002020), CONST64(0x2020202000002020), 
+  CONST64(0x2000000000200000), CONST64(0x2000002000200000), CONST64(0x2000000000200020), CONST64(0x2000002000200020), 
+  CONST64(0x2000200000200000), CONST64(0x2000202000200000), CONST64(0x2000200000200020), CONST64(0x2000202000200020), 
+  CONST64(0x2000000000202000), CONST64(0x2000002000202000), CONST64(0x2000000000202020), CONST64(0x2000002000202020), 
+  CONST64(0x2000200000202000), CONST64(0x2000202000202000), CONST64(0x2000200000202020), CONST64(0x2000202000202020), 
+  CONST64(0x2020000000200000), CONST64(0x2020002000200000), CONST64(0x2020000000200020), CONST64(0x2020002000200020), 
+  CONST64(0x2020200000200000), CONST64(0x2020202000200000), CONST64(0x2020200000200020), CONST64(0x2020202000200020), 
+  CONST64(0x2020000000202000), CONST64(0x2020002000202000), CONST64(0x2020000000202020), CONST64(0x2020002000202020), 
+  CONST64(0x2020200000202000), CONST64(0x2020202000202000), CONST64(0x2020200000202020), CONST64(0x2020202000202020), 
+  CONST64(0x0000000020000000), CONST64(0x0000002020000000), CONST64(0x0000000020000020), CONST64(0x0000002020000020), 
+  CONST64(0x0000200020000000), CONST64(0x0000202020000000), CONST64(0x0000200020000020), CONST64(0x0000202020000020), 
+  CONST64(0x0000000020002000), CONST64(0x0000002020002000), CONST64(0x0000000020002020), CONST64(0x0000002020002020), 
+  CONST64(0x0000200020002000), CONST64(0x0000202020002000), CONST64(0x0000200020002020), CONST64(0x0000202020002020), 
+  CONST64(0x0020000020000000), CONST64(0x0020002020000000), CONST64(0x0020000020000020), CONST64(0x0020002020000020), 
+  CONST64(0x0020200020000000), CONST64(0x0020202020000000), CONST64(0x0020200020000020), CONST64(0x0020202020000020), 
+  CONST64(0x0020000020002000), CONST64(0x0020002020002000), CONST64(0x0020000020002020), CONST64(0x0020002020002020), 
+  CONST64(0x0020200020002000), CONST64(0x0020202020002000), CONST64(0x0020200020002020), CONST64(0x0020202020002020), 
+  CONST64(0x0000000020200000), CONST64(0x0000002020200000), CONST64(0x0000000020200020), CONST64(0x0000002020200020), 
+  CONST64(0x0000200020200000), CONST64(0x0000202020200000), CONST64(0x0000200020200020), CONST64(0x0000202020200020), 
+  CONST64(0x0000000020202000), CONST64(0x0000002020202000), CONST64(0x0000000020202020), CONST64(0x0000002020202020), 
+  CONST64(0x0000200020202000), CONST64(0x0000202020202000), CONST64(0x0000200020202020), CONST64(0x0000202020202020), 
+  CONST64(0x0020000020200000), CONST64(0x0020002020200000), CONST64(0x0020000020200020), CONST64(0x0020002020200020), 
+  CONST64(0x0020200020200000), CONST64(0x0020202020200000), CONST64(0x0020200020200020), CONST64(0x0020202020200020), 
+  CONST64(0x0020000020202000), CONST64(0x0020002020202000), CONST64(0x0020000020202020), CONST64(0x0020002020202020), 
+  CONST64(0x0020200020202000), CONST64(0x0020202020202000), CONST64(0x0020200020202020), CONST64(0x0020202020202020), 
+  CONST64(0x2000000020000000), CONST64(0x2000002020000000), CONST64(0x2000000020000020), CONST64(0x2000002020000020), 
+  CONST64(0x2000200020000000), CONST64(0x2000202020000000), CONST64(0x2000200020000020), CONST64(0x2000202020000020), 
+  CONST64(0x2000000020002000), CONST64(0x2000002020002000), CONST64(0x2000000020002020), CONST64(0x2000002020002020), 
+  CONST64(0x2000200020002000), CONST64(0x2000202020002000), CONST64(0x2000200020002020), CONST64(0x2000202020002020), 
+  CONST64(0x2020000020000000), CONST64(0x2020002020000000), CONST64(0x2020000020000020), CONST64(0x2020002020000020), 
+  CONST64(0x2020200020000000), CONST64(0x2020202020000000), CONST64(0x2020200020000020), CONST64(0x2020202020000020), 
+  CONST64(0x2020000020002000), CONST64(0x2020002020002000), CONST64(0x2020000020002020), CONST64(0x2020002020002020), 
+  CONST64(0x2020200020002000), CONST64(0x2020202020002000), CONST64(0x2020200020002020), CONST64(0x2020202020002020), 
+  CONST64(0x2000000020200000), CONST64(0x2000002020200000), CONST64(0x2000000020200020), CONST64(0x2000002020200020), 
+  CONST64(0x2000200020200000), CONST64(0x2000202020200000), CONST64(0x2000200020200020), CONST64(0x2000202020200020), 
+  CONST64(0x2000000020202000), CONST64(0x2000002020202000), CONST64(0x2000000020202020), CONST64(0x2000002020202020), 
+  CONST64(0x2000200020202000), CONST64(0x2000202020202000), CONST64(0x2000200020202020), CONST64(0x2000202020202020), 
+  CONST64(0x2020000020200000), CONST64(0x2020002020200000), CONST64(0x2020000020200020), CONST64(0x2020002020200020), 
+  CONST64(0x2020200020200000), CONST64(0x2020202020200000), CONST64(0x2020200020200020), CONST64(0x2020202020200020), 
+  CONST64(0x2020000020202000), CONST64(0x2020002020202000), CONST64(0x2020000020202020), CONST64(0x2020002020202020), 
+  CONST64(0x2020200020202000), CONST64(0x2020202020202000), CONST64(0x2020200020202020), CONST64(0x2020202020202020)
+  }};
+  
+static const ulong64 des_fp[8][256] = {
+
+{ CONST64(0x0000000000000000), CONST64(0x0000008000000000), CONST64(0x0000000002000000), CONST64(0x0000008002000000), 
+  CONST64(0x0000000000020000), CONST64(0x0000008000020000), CONST64(0x0000000002020000), CONST64(0x0000008002020000), 
+  CONST64(0x0000000000000200), CONST64(0x0000008000000200), CONST64(0x0000000002000200), CONST64(0x0000008002000200), 
+  CONST64(0x0000000000020200), CONST64(0x0000008000020200), CONST64(0x0000000002020200), CONST64(0x0000008002020200), 
+  CONST64(0x0000000000000002), CONST64(0x0000008000000002), CONST64(0x0000000002000002), CONST64(0x0000008002000002), 
+  CONST64(0x0000000000020002), CONST64(0x0000008000020002), CONST64(0x0000000002020002), CONST64(0x0000008002020002), 
+  CONST64(0x0000000000000202), CONST64(0x0000008000000202), CONST64(0x0000000002000202), CONST64(0x0000008002000202), 
+  CONST64(0x0000000000020202), CONST64(0x0000008000020202), CONST64(0x0000000002020202), CONST64(0x0000008002020202), 
+  CONST64(0x0200000000000000), CONST64(0x0200008000000000), CONST64(0x0200000002000000), CONST64(0x0200008002000000), 
+  CONST64(0x0200000000020000), CONST64(0x0200008000020000), CONST64(0x0200000002020000), CONST64(0x0200008002020000), 
+  CONST64(0x0200000000000200), CONST64(0x0200008000000200), CONST64(0x0200000002000200), CONST64(0x0200008002000200), 
+  CONST64(0x0200000000020200), CONST64(0x0200008000020200), CONST64(0x0200000002020200), CONST64(0x0200008002020200), 
+  CONST64(0x0200000000000002), CONST64(0x0200008000000002), CONST64(0x0200000002000002), CONST64(0x0200008002000002), 
+  CONST64(0x0200000000020002), CONST64(0x0200008000020002), CONST64(0x0200000002020002), CONST64(0x0200008002020002), 
+  CONST64(0x0200000000000202), CONST64(0x0200008000000202), CONST64(0x0200000002000202), CONST64(0x0200008002000202), 
+  CONST64(0x0200000000020202), CONST64(0x0200008000020202), CONST64(0x0200000002020202), CONST64(0x0200008002020202), 
+  CONST64(0x0002000000000000), CONST64(0x0002008000000000), CONST64(0x0002000002000000), CONST64(0x0002008002000000), 
+  CONST64(0x0002000000020000), CONST64(0x0002008000020000), CONST64(0x0002000002020000), CONST64(0x0002008002020000), 
+  CONST64(0x0002000000000200), CONST64(0x0002008000000200), CONST64(0x0002000002000200), CONST64(0x0002008002000200), 
+  CONST64(0x0002000000020200), CONST64(0x0002008000020200), CONST64(0x0002000002020200), CONST64(0x0002008002020200), 
+  CONST64(0x0002000000000002), CONST64(0x0002008000000002), CONST64(0x0002000002000002), CONST64(0x0002008002000002), 
+  CONST64(0x0002000000020002), CONST64(0x0002008000020002), CONST64(0x0002000002020002), CONST64(0x0002008002020002), 
+  CONST64(0x0002000000000202), CONST64(0x0002008000000202), CONST64(0x0002000002000202), CONST64(0x0002008002000202), 
+  CONST64(0x0002000000020202), CONST64(0x0002008000020202), CONST64(0x0002000002020202), CONST64(0x0002008002020202), 
+  CONST64(0x0202000000000000), CONST64(0x0202008000000000), CONST64(0x0202000002000000), CONST64(0x0202008002000000), 
+  CONST64(0x0202000000020000), CONST64(0x0202008000020000), CONST64(0x0202000002020000), CONST64(0x0202008002020000), 
+  CONST64(0x0202000000000200), CONST64(0x0202008000000200), CONST64(0x0202000002000200), CONST64(0x0202008002000200), 
+  CONST64(0x0202000000020200), CONST64(0x0202008000020200), CONST64(0x0202000002020200), CONST64(0x0202008002020200), 
+  CONST64(0x0202000000000002), CONST64(0x0202008000000002), CONST64(0x0202000002000002), CONST64(0x0202008002000002), 
+  CONST64(0x0202000000020002), CONST64(0x0202008000020002), CONST64(0x0202000002020002), CONST64(0x0202008002020002), 
+  CONST64(0x0202000000000202), CONST64(0x0202008000000202), CONST64(0x0202000002000202), CONST64(0x0202008002000202), 
+  CONST64(0x0202000000020202), CONST64(0x0202008000020202), CONST64(0x0202000002020202), CONST64(0x0202008002020202), 
+  CONST64(0x0000020000000000), CONST64(0x0000028000000000), CONST64(0x0000020002000000), CONST64(0x0000028002000000), 
+  CONST64(0x0000020000020000), CONST64(0x0000028000020000), CONST64(0x0000020002020000), CONST64(0x0000028002020000), 
+  CONST64(0x0000020000000200), CONST64(0x0000028000000200), CONST64(0x0000020002000200), CONST64(0x0000028002000200), 
+  CONST64(0x0000020000020200), CONST64(0x0000028000020200), CONST64(0x0000020002020200), CONST64(0x0000028002020200), 
+  CONST64(0x0000020000000002), CONST64(0x0000028000000002), CONST64(0x0000020002000002), CONST64(0x0000028002000002), 
+  CONST64(0x0000020000020002), CONST64(0x0000028000020002), CONST64(0x0000020002020002), CONST64(0x0000028002020002), 
+  CONST64(0x0000020000000202), CONST64(0x0000028000000202), CONST64(0x0000020002000202), CONST64(0x0000028002000202), 
+  CONST64(0x0000020000020202), CONST64(0x0000028000020202), CONST64(0x0000020002020202), CONST64(0x0000028002020202), 
+  CONST64(0x0200020000000000), CONST64(0x0200028000000000), CONST64(0x0200020002000000), CONST64(0x0200028002000000), 
+  CONST64(0x0200020000020000), CONST64(0x0200028000020000), CONST64(0x0200020002020000), CONST64(0x0200028002020000), 
+  CONST64(0x0200020000000200), CONST64(0x0200028000000200), CONST64(0x0200020002000200), CONST64(0x0200028002000200), 
+  CONST64(0x0200020000020200), CONST64(0x0200028000020200), CONST64(0x0200020002020200), CONST64(0x0200028002020200), 
+  CONST64(0x0200020000000002), CONST64(0x0200028000000002), CONST64(0x0200020002000002), CONST64(0x0200028002000002), 
+  CONST64(0x0200020000020002), CONST64(0x0200028000020002), CONST64(0x0200020002020002), CONST64(0x0200028002020002), 
+  CONST64(0x0200020000000202), CONST64(0x0200028000000202), CONST64(0x0200020002000202), CONST64(0x0200028002000202), 
+  CONST64(0x0200020000020202), CONST64(0x0200028000020202), CONST64(0x0200020002020202), CONST64(0x0200028002020202), 
+  CONST64(0x0002020000000000), CONST64(0x0002028000000000), CONST64(0x0002020002000000), CONST64(0x0002028002000000), 
+  CONST64(0x0002020000020000), CONST64(0x0002028000020000), CONST64(0x0002020002020000), CONST64(0x0002028002020000), 
+  CONST64(0x0002020000000200), CONST64(0x0002028000000200), CONST64(0x0002020002000200), CONST64(0x0002028002000200), 
+  CONST64(0x0002020000020200), CONST64(0x0002028000020200), CONST64(0x0002020002020200), CONST64(0x0002028002020200), 
+  CONST64(0x0002020000000002), CONST64(0x0002028000000002), CONST64(0x0002020002000002), CONST64(0x0002028002000002), 
+  CONST64(0x0002020000020002), CONST64(0x0002028000020002), CONST64(0x0002020002020002), CONST64(0x0002028002020002), 
+  CONST64(0x0002020000000202), CONST64(0x0002028000000202), CONST64(0x0002020002000202), CONST64(0x0002028002000202), 
+  CONST64(0x0002020000020202), CONST64(0x0002028000020202), CONST64(0x0002020002020202), CONST64(0x0002028002020202), 
+  CONST64(0x0202020000000000), CONST64(0x0202028000000000), CONST64(0x0202020002000000), CONST64(0x0202028002000000), 
+  CONST64(0x0202020000020000), CONST64(0x0202028000020000), CONST64(0x0202020002020000), CONST64(0x0202028002020000), 
+  CONST64(0x0202020000000200), CONST64(0x0202028000000200), CONST64(0x0202020002000200), CONST64(0x0202028002000200), 
+  CONST64(0x0202020000020200), CONST64(0x0202028000020200), CONST64(0x0202020002020200), CONST64(0x0202028002020200), 
+  CONST64(0x0202020000000002), CONST64(0x0202028000000002), CONST64(0x0202020002000002), CONST64(0x0202028002000002), 
+  CONST64(0x0202020000020002), CONST64(0x0202028000020002), CONST64(0x0202020002020002), CONST64(0x0202028002020002), 
+  CONST64(0x0202020000000202), CONST64(0x0202028000000202), CONST64(0x0202020002000202), CONST64(0x0202028002000202), 
+  CONST64(0x0202020000020202), CONST64(0x0202028000020202), CONST64(0x0202020002020202), CONST64(0x0202028002020202)
+  }, 
+{ CONST64(0x0000000000000000), CONST64(0x0000000200000000), CONST64(0x0000000008000000), CONST64(0x0000000208000000), 
+  CONST64(0x0000000000080000), CONST64(0x0000000200080000), CONST64(0x0000000008080000), CONST64(0x0000000208080000), 
+  CONST64(0x0000000000000800), CONST64(0x0000000200000800), CONST64(0x0000000008000800), CONST64(0x0000000208000800), 
+  CONST64(0x0000000000080800), CONST64(0x0000000200080800), CONST64(0x0000000008080800), CONST64(0x0000000208080800), 
+  CONST64(0x0000000000000008), CONST64(0x0000000200000008), CONST64(0x0000000008000008), CONST64(0x0000000208000008), 
+  CONST64(0x0000000000080008), CONST64(0x0000000200080008), CONST64(0x0000000008080008), CONST64(0x0000000208080008), 
+  CONST64(0x0000000000000808), CONST64(0x0000000200000808), CONST64(0x0000000008000808), CONST64(0x0000000208000808), 
+  CONST64(0x0000000000080808), CONST64(0x0000000200080808), CONST64(0x0000000008080808), CONST64(0x0000000208080808), 
+  CONST64(0x0800000000000000), CONST64(0x0800000200000000), CONST64(0x0800000008000000), CONST64(0x0800000208000000), 
+  CONST64(0x0800000000080000), CONST64(0x0800000200080000), CONST64(0x0800000008080000), CONST64(0x0800000208080000), 
+  CONST64(0x0800000000000800), CONST64(0x0800000200000800), CONST64(0x0800000008000800), CONST64(0x0800000208000800), 
+  CONST64(0x0800000000080800), CONST64(0x0800000200080800), CONST64(0x0800000008080800), CONST64(0x0800000208080800), 
+  CONST64(0x0800000000000008), CONST64(0x0800000200000008), CONST64(0x0800000008000008), CONST64(0x0800000208000008), 
+  CONST64(0x0800000000080008), CONST64(0x0800000200080008), CONST64(0x0800000008080008), CONST64(0x0800000208080008), 
+  CONST64(0x0800000000000808), CONST64(0x0800000200000808), CONST64(0x0800000008000808), CONST64(0x0800000208000808), 
+  CONST64(0x0800000000080808), CONST64(0x0800000200080808), CONST64(0x0800000008080808), CONST64(0x0800000208080808), 
+  CONST64(0x0008000000000000), CONST64(0x0008000200000000), CONST64(0x0008000008000000), CONST64(0x0008000208000000), 
+  CONST64(0x0008000000080000), CONST64(0x0008000200080000), CONST64(0x0008000008080000), CONST64(0x0008000208080000), 
+  CONST64(0x0008000000000800), CONST64(0x0008000200000800), CONST64(0x0008000008000800), CONST64(0x0008000208000800), 
+  CONST64(0x0008000000080800), CONST64(0x0008000200080800), CONST64(0x0008000008080800), CONST64(0x0008000208080800), 
+  CONST64(0x0008000000000008), CONST64(0x0008000200000008), CONST64(0x0008000008000008), CONST64(0x0008000208000008), 
+  CONST64(0x0008000000080008), CONST64(0x0008000200080008), CONST64(0x0008000008080008), CONST64(0x0008000208080008), 
+  CONST64(0x0008000000000808), CONST64(0x0008000200000808), CONST64(0x0008000008000808), CONST64(0x0008000208000808), 
+  CONST64(0x0008000000080808), CONST64(0x0008000200080808), CONST64(0x0008000008080808), CONST64(0x0008000208080808), 
+  CONST64(0x0808000000000000), CONST64(0x0808000200000000), CONST64(0x0808000008000000), CONST64(0x0808000208000000), 
+  CONST64(0x0808000000080000), CONST64(0x0808000200080000), CONST64(0x0808000008080000), CONST64(0x0808000208080000), 
+  CONST64(0x0808000000000800), CONST64(0x0808000200000800), CONST64(0x0808000008000800), CONST64(0x0808000208000800), 
+  CONST64(0x0808000000080800), CONST64(0x0808000200080800), CONST64(0x0808000008080800), CONST64(0x0808000208080800), 
+  CONST64(0x0808000000000008), CONST64(0x0808000200000008), CONST64(0x0808000008000008), CONST64(0x0808000208000008), 
+  CONST64(0x0808000000080008), CONST64(0x0808000200080008), CONST64(0x0808000008080008), CONST64(0x0808000208080008), 
+  CONST64(0x0808000000000808), CONST64(0x0808000200000808), CONST64(0x0808000008000808), CONST64(0x0808000208000808), 
+  CONST64(0x0808000000080808), CONST64(0x0808000200080808), CONST64(0x0808000008080808), CONST64(0x0808000208080808), 
+  CONST64(0x0000080000000000), CONST64(0x0000080200000000), CONST64(0x0000080008000000), CONST64(0x0000080208000000), 
+  CONST64(0x0000080000080000), CONST64(0x0000080200080000), CONST64(0x0000080008080000), CONST64(0x0000080208080000), 
+  CONST64(0x0000080000000800), CONST64(0x0000080200000800), CONST64(0x0000080008000800), CONST64(0x0000080208000800), 
+  CONST64(0x0000080000080800), CONST64(0x0000080200080800), CONST64(0x0000080008080800), CONST64(0x0000080208080800), 
+  CONST64(0x0000080000000008), CONST64(0x0000080200000008), CONST64(0x0000080008000008), CONST64(0x0000080208000008), 
+  CONST64(0x0000080000080008), CONST64(0x0000080200080008), CONST64(0x0000080008080008), CONST64(0x0000080208080008), 
+  CONST64(0x0000080000000808), CONST64(0x0000080200000808), CONST64(0x0000080008000808), CONST64(0x0000080208000808), 
+  CONST64(0x0000080000080808), CONST64(0x0000080200080808), CONST64(0x0000080008080808), CONST64(0x0000080208080808), 
+  CONST64(0x0800080000000000), CONST64(0x0800080200000000), CONST64(0x0800080008000000), CONST64(0x0800080208000000), 
+  CONST64(0x0800080000080000), CONST64(0x0800080200080000), CONST64(0x0800080008080000), CONST64(0x0800080208080000), 
+  CONST64(0x0800080000000800), CONST64(0x0800080200000800), CONST64(0x0800080008000800), CONST64(0x0800080208000800), 
+  CONST64(0x0800080000080800), CONST64(0x0800080200080800), CONST64(0x0800080008080800), CONST64(0x0800080208080800), 
+  CONST64(0x0800080000000008), CONST64(0x0800080200000008), CONST64(0x0800080008000008), CONST64(0x0800080208000008), 
+  CONST64(0x0800080000080008), CONST64(0x0800080200080008), CONST64(0x0800080008080008), CONST64(0x0800080208080008), 
+  CONST64(0x0800080000000808), CONST64(0x0800080200000808), CONST64(0x0800080008000808), CONST64(0x0800080208000808), 
+  CONST64(0x0800080000080808), CONST64(0x0800080200080808), CONST64(0x0800080008080808), CONST64(0x0800080208080808), 
+  CONST64(0x0008080000000000), CONST64(0x0008080200000000), CONST64(0x0008080008000000), CONST64(0x0008080208000000), 
+  CONST64(0x0008080000080000), CONST64(0x0008080200080000), CONST64(0x0008080008080000), CONST64(0x0008080208080000), 
+  CONST64(0x0008080000000800), CONST64(0x0008080200000800), CONST64(0x0008080008000800), CONST64(0x0008080208000800), 
+  CONST64(0x0008080000080800), CONST64(0x0008080200080800), CONST64(0x0008080008080800), CONST64(0x0008080208080800), 
+  CONST64(0x0008080000000008), CONST64(0x0008080200000008), CONST64(0x0008080008000008), CONST64(0x0008080208000008), 
+  CONST64(0x0008080000080008), CONST64(0x0008080200080008), CONST64(0x0008080008080008), CONST64(0x0008080208080008), 
+  CONST64(0x0008080000000808), CONST64(0x0008080200000808), CONST64(0x0008080008000808), CONST64(0x0008080208000808), 
+  CONST64(0x0008080000080808), CONST64(0x0008080200080808), CONST64(0x0008080008080808), CONST64(0x0008080208080808), 
+  CONST64(0x0808080000000000), CONST64(0x0808080200000000), CONST64(0x0808080008000000), CONST64(0x0808080208000000), 
+  CONST64(0x0808080000080000), CONST64(0x0808080200080000), CONST64(0x0808080008080000), CONST64(0x0808080208080000), 
+  CONST64(0x0808080000000800), CONST64(0x0808080200000800), CONST64(0x0808080008000800), CONST64(0x0808080208000800), 
+  CONST64(0x0808080000080800), CONST64(0x0808080200080800), CONST64(0x0808080008080800), CONST64(0x0808080208080800), 
+  CONST64(0x0808080000000008), CONST64(0x0808080200000008), CONST64(0x0808080008000008), CONST64(0x0808080208000008), 
+  CONST64(0x0808080000080008), CONST64(0x0808080200080008), CONST64(0x0808080008080008), CONST64(0x0808080208080008), 
+  CONST64(0x0808080000000808), CONST64(0x0808080200000808), CONST64(0x0808080008000808), CONST64(0x0808080208000808), 
+  CONST64(0x0808080000080808), CONST64(0x0808080200080808), CONST64(0x0808080008080808), CONST64(0x0808080208080808)
+  }, 
+{ CONST64(0x0000000000000000), CONST64(0x0000000800000000), CONST64(0x0000000020000000), CONST64(0x0000000820000000), 
+  CONST64(0x0000000000200000), CONST64(0x0000000800200000), CONST64(0x0000000020200000), CONST64(0x0000000820200000), 
+  CONST64(0x0000000000002000), CONST64(0x0000000800002000), CONST64(0x0000000020002000), CONST64(0x0000000820002000), 
+  CONST64(0x0000000000202000), CONST64(0x0000000800202000), CONST64(0x0000000020202000), CONST64(0x0000000820202000), 
+  CONST64(0x0000000000000020), CONST64(0x0000000800000020), CONST64(0x0000000020000020), CONST64(0x0000000820000020), 
+  CONST64(0x0000000000200020), CONST64(0x0000000800200020), CONST64(0x0000000020200020), CONST64(0x0000000820200020), 
+  CONST64(0x0000000000002020), CONST64(0x0000000800002020), CONST64(0x0000000020002020), CONST64(0x0000000820002020), 
+  CONST64(0x0000000000202020), CONST64(0x0000000800202020), CONST64(0x0000000020202020), CONST64(0x0000000820202020), 
+  CONST64(0x2000000000000000), CONST64(0x2000000800000000), CONST64(0x2000000020000000), CONST64(0x2000000820000000), 
+  CONST64(0x2000000000200000), CONST64(0x2000000800200000), CONST64(0x2000000020200000), CONST64(0x2000000820200000), 
+  CONST64(0x2000000000002000), CONST64(0x2000000800002000), CONST64(0x2000000020002000), CONST64(0x2000000820002000), 
+  CONST64(0x2000000000202000), CONST64(0x2000000800202000), CONST64(0x2000000020202000), CONST64(0x2000000820202000), 
+  CONST64(0x2000000000000020), CONST64(0x2000000800000020), CONST64(0x2000000020000020), CONST64(0x2000000820000020), 
+  CONST64(0x2000000000200020), CONST64(0x2000000800200020), CONST64(0x2000000020200020), CONST64(0x2000000820200020), 
+  CONST64(0x2000000000002020), CONST64(0x2000000800002020), CONST64(0x2000000020002020), CONST64(0x2000000820002020), 
+  CONST64(0x2000000000202020), CONST64(0x2000000800202020), CONST64(0x2000000020202020), CONST64(0x2000000820202020), 
+  CONST64(0x0020000000000000), CONST64(0x0020000800000000), CONST64(0x0020000020000000), CONST64(0x0020000820000000), 
+  CONST64(0x0020000000200000), CONST64(0x0020000800200000), CONST64(0x0020000020200000), CONST64(0x0020000820200000), 
+  CONST64(0x0020000000002000), CONST64(0x0020000800002000), CONST64(0x0020000020002000), CONST64(0x0020000820002000), 
+  CONST64(0x0020000000202000), CONST64(0x0020000800202000), CONST64(0x0020000020202000), CONST64(0x0020000820202000), 
+  CONST64(0x0020000000000020), CONST64(0x0020000800000020), CONST64(0x0020000020000020), CONST64(0x0020000820000020), 
+  CONST64(0x0020000000200020), CONST64(0x0020000800200020), CONST64(0x0020000020200020), CONST64(0x0020000820200020), 
+  CONST64(0x0020000000002020), CONST64(0x0020000800002020), CONST64(0x0020000020002020), CONST64(0x0020000820002020), 
+  CONST64(0x0020000000202020), CONST64(0x0020000800202020), CONST64(0x0020000020202020), CONST64(0x0020000820202020), 
+  CONST64(0x2020000000000000), CONST64(0x2020000800000000), CONST64(0x2020000020000000), CONST64(0x2020000820000000), 
+  CONST64(0x2020000000200000), CONST64(0x2020000800200000), CONST64(0x2020000020200000), CONST64(0x2020000820200000), 
+  CONST64(0x2020000000002000), CONST64(0x2020000800002000), CONST64(0x2020000020002000), CONST64(0x2020000820002000), 
+  CONST64(0x2020000000202000), CONST64(0x2020000800202000), CONST64(0x2020000020202000), CONST64(0x2020000820202000), 
+  CONST64(0x2020000000000020), CONST64(0x2020000800000020), CONST64(0x2020000020000020), CONST64(0x2020000820000020), 
+  CONST64(0x2020000000200020), CONST64(0x2020000800200020), CONST64(0x2020000020200020), CONST64(0x2020000820200020), 
+  CONST64(0x2020000000002020), CONST64(0x2020000800002020), CONST64(0x2020000020002020), CONST64(0x2020000820002020), 
+  CONST64(0x2020000000202020), CONST64(0x2020000800202020), CONST64(0x2020000020202020), CONST64(0x2020000820202020), 
+  CONST64(0x0000200000000000), CONST64(0x0000200800000000), CONST64(0x0000200020000000), CONST64(0x0000200820000000), 
+  CONST64(0x0000200000200000), CONST64(0x0000200800200000), CONST64(0x0000200020200000), CONST64(0x0000200820200000), 
+  CONST64(0x0000200000002000), CONST64(0x0000200800002000), CONST64(0x0000200020002000), CONST64(0x0000200820002000), 
+  CONST64(0x0000200000202000), CONST64(0x0000200800202000), CONST64(0x0000200020202000), CONST64(0x0000200820202000), 
+  CONST64(0x0000200000000020), CONST64(0x0000200800000020), CONST64(0x0000200020000020), CONST64(0x0000200820000020), 
+  CONST64(0x0000200000200020), CONST64(0x0000200800200020), CONST64(0x0000200020200020), CONST64(0x0000200820200020), 
+  CONST64(0x0000200000002020), CONST64(0x0000200800002020), CONST64(0x0000200020002020), CONST64(0x0000200820002020), 
+  CONST64(0x0000200000202020), CONST64(0x0000200800202020), CONST64(0x0000200020202020), CONST64(0x0000200820202020), 
+  CONST64(0x2000200000000000), CONST64(0x2000200800000000), CONST64(0x2000200020000000), CONST64(0x2000200820000000), 
+  CONST64(0x2000200000200000), CONST64(0x2000200800200000), CONST64(0x2000200020200000), CONST64(0x2000200820200000), 
+  CONST64(0x2000200000002000), CONST64(0x2000200800002000), CONST64(0x2000200020002000), CONST64(0x2000200820002000), 
+  CONST64(0x2000200000202000), CONST64(0x2000200800202000), CONST64(0x2000200020202000), CONST64(0x2000200820202000), 
+  CONST64(0x2000200000000020), CONST64(0x2000200800000020), CONST64(0x2000200020000020), CONST64(0x2000200820000020), 
+  CONST64(0x2000200000200020), CONST64(0x2000200800200020), CONST64(0x2000200020200020), CONST64(0x2000200820200020), 
+  CONST64(0x2000200000002020), CONST64(0x2000200800002020), CONST64(0x2000200020002020), CONST64(0x2000200820002020), 
+  CONST64(0x2000200000202020), CONST64(0x2000200800202020), CONST64(0x2000200020202020), CONST64(0x2000200820202020), 
+  CONST64(0x0020200000000000), CONST64(0x0020200800000000), CONST64(0x0020200020000000), CONST64(0x0020200820000000), 
+  CONST64(0x0020200000200000), CONST64(0x0020200800200000), CONST64(0x0020200020200000), CONST64(0x0020200820200000), 
+  CONST64(0x0020200000002000), CONST64(0x0020200800002000), CONST64(0x0020200020002000), CONST64(0x0020200820002000), 
+  CONST64(0x0020200000202000), CONST64(0x0020200800202000), CONST64(0x0020200020202000), CONST64(0x0020200820202000), 
+  CONST64(0x0020200000000020), CONST64(0x0020200800000020), CONST64(0x0020200020000020), CONST64(0x0020200820000020), 
+  CONST64(0x0020200000200020), CONST64(0x0020200800200020), CONST64(0x0020200020200020), CONST64(0x0020200820200020), 
+  CONST64(0x0020200000002020), CONST64(0x0020200800002020), CONST64(0x0020200020002020), CONST64(0x0020200820002020), 
+  CONST64(0x0020200000202020), CONST64(0x0020200800202020), CONST64(0x0020200020202020), CONST64(0x0020200820202020), 
+  CONST64(0x2020200000000000), CONST64(0x2020200800000000), CONST64(0x2020200020000000), CONST64(0x2020200820000000), 
+  CONST64(0x2020200000200000), CONST64(0x2020200800200000), CONST64(0x2020200020200000), CONST64(0x2020200820200000), 
+  CONST64(0x2020200000002000), CONST64(0x2020200800002000), CONST64(0x2020200020002000), CONST64(0x2020200820002000), 
+  CONST64(0x2020200000202000), CONST64(0x2020200800202000), CONST64(0x2020200020202000), CONST64(0x2020200820202000), 
+  CONST64(0x2020200000000020), CONST64(0x2020200800000020), CONST64(0x2020200020000020), CONST64(0x2020200820000020), 
+  CONST64(0x2020200000200020), CONST64(0x2020200800200020), CONST64(0x2020200020200020), CONST64(0x2020200820200020), 
+  CONST64(0x2020200000002020), CONST64(0x2020200800002020), CONST64(0x2020200020002020), CONST64(0x2020200820002020), 
+  CONST64(0x2020200000202020), CONST64(0x2020200800202020), CONST64(0x2020200020202020), CONST64(0x2020200820202020)
+  }, 
+{ CONST64(0x0000000000000000), CONST64(0x0000002000000000), CONST64(0x0000000080000000), CONST64(0x0000002080000000), 
+  CONST64(0x0000000000800000), CONST64(0x0000002000800000), CONST64(0x0000000080800000), CONST64(0x0000002080800000), 
+  CONST64(0x0000000000008000), CONST64(0x0000002000008000), CONST64(0x0000000080008000), CONST64(0x0000002080008000), 
+  CONST64(0x0000000000808000), CONST64(0x0000002000808000), CONST64(0x0000000080808000), CONST64(0x0000002080808000), 
+  CONST64(0x0000000000000080), CONST64(0x0000002000000080), CONST64(0x0000000080000080), CONST64(0x0000002080000080), 
+  CONST64(0x0000000000800080), CONST64(0x0000002000800080), CONST64(0x0000000080800080), CONST64(0x0000002080800080), 
+  CONST64(0x0000000000008080), CONST64(0x0000002000008080), CONST64(0x0000000080008080), CONST64(0x0000002080008080), 
+  CONST64(0x0000000000808080), CONST64(0x0000002000808080), CONST64(0x0000000080808080), CONST64(0x0000002080808080), 
+  CONST64(0x8000000000000000), CONST64(0x8000002000000000), CONST64(0x8000000080000000), CONST64(0x8000002080000000), 
+  CONST64(0x8000000000800000), CONST64(0x8000002000800000), CONST64(0x8000000080800000), CONST64(0x8000002080800000), 
+  CONST64(0x8000000000008000), CONST64(0x8000002000008000), CONST64(0x8000000080008000), CONST64(0x8000002080008000), 
+  CONST64(0x8000000000808000), CONST64(0x8000002000808000), CONST64(0x8000000080808000), CONST64(0x8000002080808000), 
+  CONST64(0x8000000000000080), CONST64(0x8000002000000080), CONST64(0x8000000080000080), CONST64(0x8000002080000080), 
+  CONST64(0x8000000000800080), CONST64(0x8000002000800080), CONST64(0x8000000080800080), CONST64(0x8000002080800080), 
+  CONST64(0x8000000000008080), CONST64(0x8000002000008080), CONST64(0x8000000080008080), CONST64(0x8000002080008080), 
+  CONST64(0x8000000000808080), CONST64(0x8000002000808080), CONST64(0x8000000080808080), CONST64(0x8000002080808080), 
+  CONST64(0x0080000000000000), CONST64(0x0080002000000000), CONST64(0x0080000080000000), CONST64(0x0080002080000000), 
+  CONST64(0x0080000000800000), CONST64(0x0080002000800000), CONST64(0x0080000080800000), CONST64(0x0080002080800000), 
+  CONST64(0x0080000000008000), CONST64(0x0080002000008000), CONST64(0x0080000080008000), CONST64(0x0080002080008000), 
+  CONST64(0x0080000000808000), CONST64(0x0080002000808000), CONST64(0x0080000080808000), CONST64(0x0080002080808000), 
+  CONST64(0x0080000000000080), CONST64(0x0080002000000080), CONST64(0x0080000080000080), CONST64(0x0080002080000080), 
+  CONST64(0x0080000000800080), CONST64(0x0080002000800080), CONST64(0x0080000080800080), CONST64(0x0080002080800080), 
+  CONST64(0x0080000000008080), CONST64(0x0080002000008080), CONST64(0x0080000080008080), CONST64(0x0080002080008080), 
+  CONST64(0x0080000000808080), CONST64(0x0080002000808080), CONST64(0x0080000080808080), CONST64(0x0080002080808080), 
+  CONST64(0x8080000000000000), CONST64(0x8080002000000000), CONST64(0x8080000080000000), CONST64(0x8080002080000000), 
+  CONST64(0x8080000000800000), CONST64(0x8080002000800000), CONST64(0x8080000080800000), CONST64(0x8080002080800000), 
+  CONST64(0x8080000000008000), CONST64(0x8080002000008000), CONST64(0x8080000080008000), CONST64(0x8080002080008000), 
+  CONST64(0x8080000000808000), CONST64(0x8080002000808000), CONST64(0x8080000080808000), CONST64(0x8080002080808000), 
+  CONST64(0x8080000000000080), CONST64(0x8080002000000080), CONST64(0x8080000080000080), CONST64(0x8080002080000080), 
+  CONST64(0x8080000000800080), CONST64(0x8080002000800080), CONST64(0x8080000080800080), CONST64(0x8080002080800080), 
+  CONST64(0x8080000000008080), CONST64(0x8080002000008080), CONST64(0x8080000080008080), CONST64(0x8080002080008080), 
+  CONST64(0x8080000000808080), CONST64(0x8080002000808080), CONST64(0x8080000080808080), CONST64(0x8080002080808080), 
+  CONST64(0x0000800000000000), CONST64(0x0000802000000000), CONST64(0x0000800080000000), CONST64(0x0000802080000000), 
+  CONST64(0x0000800000800000), CONST64(0x0000802000800000), CONST64(0x0000800080800000), CONST64(0x0000802080800000), 
+  CONST64(0x0000800000008000), CONST64(0x0000802000008000), CONST64(0x0000800080008000), CONST64(0x0000802080008000), 
+  CONST64(0x0000800000808000), CONST64(0x0000802000808000), CONST64(0x0000800080808000), CONST64(0x0000802080808000), 
+  CONST64(0x0000800000000080), CONST64(0x0000802000000080), CONST64(0x0000800080000080), CONST64(0x0000802080000080), 
+  CONST64(0x0000800000800080), CONST64(0x0000802000800080), CONST64(0x0000800080800080), CONST64(0x0000802080800080), 
+  CONST64(0x0000800000008080), CONST64(0x0000802000008080), CONST64(0x0000800080008080), CONST64(0x0000802080008080), 
+  CONST64(0x0000800000808080), CONST64(0x0000802000808080), CONST64(0x0000800080808080), CONST64(0x0000802080808080), 
+  CONST64(0x8000800000000000), CONST64(0x8000802000000000), CONST64(0x8000800080000000), CONST64(0x8000802080000000), 
+  CONST64(0x8000800000800000), CONST64(0x8000802000800000), CONST64(0x8000800080800000), CONST64(0x8000802080800000), 
+  CONST64(0x8000800000008000), CONST64(0x8000802000008000), CONST64(0x8000800080008000), CONST64(0x8000802080008000), 
+  CONST64(0x8000800000808000), CONST64(0x8000802000808000), CONST64(0x8000800080808000), CONST64(0x8000802080808000), 
+  CONST64(0x8000800000000080), CONST64(0x8000802000000080), CONST64(0x8000800080000080), CONST64(0x8000802080000080), 
+  CONST64(0x8000800000800080), CONST64(0x8000802000800080), CONST64(0x8000800080800080), CONST64(0x8000802080800080), 
+  CONST64(0x8000800000008080), CONST64(0x8000802000008080), CONST64(0x8000800080008080), CONST64(0x8000802080008080), 
+  CONST64(0x8000800000808080), CONST64(0x8000802000808080), CONST64(0x8000800080808080), CONST64(0x8000802080808080), 
+  CONST64(0x0080800000000000), CONST64(0x0080802000000000), CONST64(0x0080800080000000), CONST64(0x0080802080000000), 
+  CONST64(0x0080800000800000), CONST64(0x0080802000800000), CONST64(0x0080800080800000), CONST64(0x0080802080800000), 
+  CONST64(0x0080800000008000), CONST64(0x0080802000008000), CONST64(0x0080800080008000), CONST64(0x0080802080008000), 
+  CONST64(0x0080800000808000), CONST64(0x0080802000808000), CONST64(0x0080800080808000), CONST64(0x0080802080808000), 
+  CONST64(0x0080800000000080), CONST64(0x0080802000000080), CONST64(0x0080800080000080), CONST64(0x0080802080000080), 
+  CONST64(0x0080800000800080), CONST64(0x0080802000800080), CONST64(0x0080800080800080), CONST64(0x0080802080800080), 
+  CONST64(0x0080800000008080), CONST64(0x0080802000008080), CONST64(0x0080800080008080), CONST64(0x0080802080008080), 
+  CONST64(0x0080800000808080), CONST64(0x0080802000808080), CONST64(0x0080800080808080), CONST64(0x0080802080808080), 
+  CONST64(0x8080800000000000), CONST64(0x8080802000000000), CONST64(0x8080800080000000), CONST64(0x8080802080000000), 
+  CONST64(0x8080800000800000), CONST64(0x8080802000800000), CONST64(0x8080800080800000), CONST64(0x8080802080800000), 
+  CONST64(0x8080800000008000), CONST64(0x8080802000008000), CONST64(0x8080800080008000), CONST64(0x8080802080008000), 
+  CONST64(0x8080800000808000), CONST64(0x8080802000808000), CONST64(0x8080800080808000), CONST64(0x8080802080808000), 
+  CONST64(0x8080800000000080), CONST64(0x8080802000000080), CONST64(0x8080800080000080), CONST64(0x8080802080000080), 
+  CONST64(0x8080800000800080), CONST64(0x8080802000800080), CONST64(0x8080800080800080), CONST64(0x8080802080800080), 
+  CONST64(0x8080800000008080), CONST64(0x8080802000008080), CONST64(0x8080800080008080), CONST64(0x8080802080008080), 
+  CONST64(0x8080800000808080), CONST64(0x8080802000808080), CONST64(0x8080800080808080), CONST64(0x8080802080808080)
+  }, 
+{ CONST64(0x0000000000000000), CONST64(0x0000004000000000), CONST64(0x0000000001000000), CONST64(0x0000004001000000), 
+  CONST64(0x0000000000010000), CONST64(0x0000004000010000), CONST64(0x0000000001010000), CONST64(0x0000004001010000), 
+  CONST64(0x0000000000000100), CONST64(0x0000004000000100), CONST64(0x0000000001000100), CONST64(0x0000004001000100), 
+  CONST64(0x0000000000010100), CONST64(0x0000004000010100), CONST64(0x0000000001010100), CONST64(0x0000004001010100), 
+  CONST64(0x0000000000000001), CONST64(0x0000004000000001), CONST64(0x0000000001000001), CONST64(0x0000004001000001), 
+  CONST64(0x0000000000010001), CONST64(0x0000004000010001), CONST64(0x0000000001010001), CONST64(0x0000004001010001), 
+  CONST64(0x0000000000000101), CONST64(0x0000004000000101), CONST64(0x0000000001000101), CONST64(0x0000004001000101), 
+  CONST64(0x0000000000010101), CONST64(0x0000004000010101), CONST64(0x0000000001010101), CONST64(0x0000004001010101), 
+  CONST64(0x0100000000000000), CONST64(0x0100004000000000), CONST64(0x0100000001000000), CONST64(0x0100004001000000), 
+  CONST64(0x0100000000010000), CONST64(0x0100004000010000), CONST64(0x0100000001010000), CONST64(0x0100004001010000), 
+  CONST64(0x0100000000000100), CONST64(0x0100004000000100), CONST64(0x0100000001000100), CONST64(0x0100004001000100), 
+  CONST64(0x0100000000010100), CONST64(0x0100004000010100), CONST64(0x0100000001010100), CONST64(0x0100004001010100), 
+  CONST64(0x0100000000000001), CONST64(0x0100004000000001), CONST64(0x0100000001000001), CONST64(0x0100004001000001), 
+  CONST64(0x0100000000010001), CONST64(0x0100004000010001), CONST64(0x0100000001010001), CONST64(0x0100004001010001), 
+  CONST64(0x0100000000000101), CONST64(0x0100004000000101), CONST64(0x0100000001000101), CONST64(0x0100004001000101), 
+  CONST64(0x0100000000010101), CONST64(0x0100004000010101), CONST64(0x0100000001010101), CONST64(0x0100004001010101), 
+  CONST64(0x0001000000000000), CONST64(0x0001004000000000), CONST64(0x0001000001000000), CONST64(0x0001004001000000), 
+  CONST64(0x0001000000010000), CONST64(0x0001004000010000), CONST64(0x0001000001010000), CONST64(0x0001004001010000), 
+  CONST64(0x0001000000000100), CONST64(0x0001004000000100), CONST64(0x0001000001000100), CONST64(0x0001004001000100), 
+  CONST64(0x0001000000010100), CONST64(0x0001004000010100), CONST64(0x0001000001010100), CONST64(0x0001004001010100), 
+  CONST64(0x0001000000000001), CONST64(0x0001004000000001), CONST64(0x0001000001000001), CONST64(0x0001004001000001), 
+  CONST64(0x0001000000010001), CONST64(0x0001004000010001), CONST64(0x0001000001010001), CONST64(0x0001004001010001), 
+  CONST64(0x0001000000000101), CONST64(0x0001004000000101), CONST64(0x0001000001000101), CONST64(0x0001004001000101), 
+  CONST64(0x0001000000010101), CONST64(0x0001004000010101), CONST64(0x0001000001010101), CONST64(0x0001004001010101), 
+  CONST64(0x0101000000000000), CONST64(0x0101004000000000), CONST64(0x0101000001000000), CONST64(0x0101004001000000), 
+  CONST64(0x0101000000010000), CONST64(0x0101004000010000), CONST64(0x0101000001010000), CONST64(0x0101004001010000), 
+  CONST64(0x0101000000000100), CONST64(0x0101004000000100), CONST64(0x0101000001000100), CONST64(0x0101004001000100), 
+  CONST64(0x0101000000010100), CONST64(0x0101004000010100), CONST64(0x0101000001010100), CONST64(0x0101004001010100), 
+  CONST64(0x0101000000000001), CONST64(0x0101004000000001), CONST64(0x0101000001000001), CONST64(0x0101004001000001), 
+  CONST64(0x0101000000010001), CONST64(0x0101004000010001), CONST64(0x0101000001010001), CONST64(0x0101004001010001), 
+  CONST64(0x0101000000000101), CONST64(0x0101004000000101), CONST64(0x0101000001000101), CONST64(0x0101004001000101), 
+  CONST64(0x0101000000010101), CONST64(0x0101004000010101), CONST64(0x0101000001010101), CONST64(0x0101004001010101), 
+  CONST64(0x0000010000000000), CONST64(0x0000014000000000), CONST64(0x0000010001000000), CONST64(0x0000014001000000), 
+  CONST64(0x0000010000010000), CONST64(0x0000014000010000), CONST64(0x0000010001010000), CONST64(0x0000014001010000), 
+  CONST64(0x0000010000000100), CONST64(0x0000014000000100), CONST64(0x0000010001000100), CONST64(0x0000014001000100), 
+  CONST64(0x0000010000010100), CONST64(0x0000014000010100), CONST64(0x0000010001010100), CONST64(0x0000014001010100), 
+  CONST64(0x0000010000000001), CONST64(0x0000014000000001), CONST64(0x0000010001000001), CONST64(0x0000014001000001), 
+  CONST64(0x0000010000010001), CONST64(0x0000014000010001), CONST64(0x0000010001010001), CONST64(0x0000014001010001), 
+  CONST64(0x0000010000000101), CONST64(0x0000014000000101), CONST64(0x0000010001000101), CONST64(0x0000014001000101), 
+  CONST64(0x0000010000010101), CONST64(0x0000014000010101), CONST64(0x0000010001010101), CONST64(0x0000014001010101), 
+  CONST64(0x0100010000000000), CONST64(0x0100014000000000), CONST64(0x0100010001000000), CONST64(0x0100014001000000), 
+  CONST64(0x0100010000010000), CONST64(0x0100014000010000), CONST64(0x0100010001010000), CONST64(0x0100014001010000), 
+  CONST64(0x0100010000000100), CONST64(0x0100014000000100), CONST64(0x0100010001000100), CONST64(0x0100014001000100), 
+  CONST64(0x0100010000010100), CONST64(0x0100014000010100), CONST64(0x0100010001010100), CONST64(0x0100014001010100), 
+  CONST64(0x0100010000000001), CONST64(0x0100014000000001), CONST64(0x0100010001000001), CONST64(0x0100014001000001), 
+  CONST64(0x0100010000010001), CONST64(0x0100014000010001), CONST64(0x0100010001010001), CONST64(0x0100014001010001), 
+  CONST64(0x0100010000000101), CONST64(0x0100014000000101), CONST64(0x0100010001000101), CONST64(0x0100014001000101), 
+  CONST64(0x0100010000010101), CONST64(0x0100014000010101), CONST64(0x0100010001010101), CONST64(0x0100014001010101), 
+  CONST64(0x0001010000000000), CONST64(0x0001014000000000), CONST64(0x0001010001000000), CONST64(0x0001014001000000), 
+  CONST64(0x0001010000010000), CONST64(0x0001014000010000), CONST64(0x0001010001010000), CONST64(0x0001014001010000), 
+  CONST64(0x0001010000000100), CONST64(0x0001014000000100), CONST64(0x0001010001000100), CONST64(0x0001014001000100), 
+  CONST64(0x0001010000010100), CONST64(0x0001014000010100), CONST64(0x0001010001010100), CONST64(0x0001014001010100), 
+  CONST64(0x0001010000000001), CONST64(0x0001014000000001), CONST64(0x0001010001000001), CONST64(0x0001014001000001), 
+  CONST64(0x0001010000010001), CONST64(0x0001014000010001), CONST64(0x0001010001010001), CONST64(0x0001014001010001), 
+  CONST64(0x0001010000000101), CONST64(0x0001014000000101), CONST64(0x0001010001000101), CONST64(0x0001014001000101), 
+  CONST64(0x0001010000010101), CONST64(0x0001014000010101), CONST64(0x0001010001010101), CONST64(0x0001014001010101), 
+  CONST64(0x0101010000000000), CONST64(0x0101014000000000), CONST64(0x0101010001000000), CONST64(0x0101014001000000), 
+  CONST64(0x0101010000010000), CONST64(0x0101014000010000), CONST64(0x0101010001010000), CONST64(0x0101014001010000), 
+  CONST64(0x0101010000000100), CONST64(0x0101014000000100), CONST64(0x0101010001000100), CONST64(0x0101014001000100), 
+  CONST64(0x0101010000010100), CONST64(0x0101014000010100), CONST64(0x0101010001010100), CONST64(0x0101014001010100), 
+  CONST64(0x0101010000000001), CONST64(0x0101014000000001), CONST64(0x0101010001000001), CONST64(0x0101014001000001), 
+  CONST64(0x0101010000010001), CONST64(0x0101014000010001), CONST64(0x0101010001010001), CONST64(0x0101014001010001), 
+  CONST64(0x0101010000000101), CONST64(0x0101014000000101), CONST64(0x0101010001000101), CONST64(0x0101014001000101), 
+  CONST64(0x0101010000010101), CONST64(0x0101014000010101), CONST64(0x0101010001010101), CONST64(0x0101014001010101)
+  }, 
+{ CONST64(0x0000000000000000), CONST64(0x0000000100000000), CONST64(0x0000000004000000), CONST64(0x0000000104000000), 
+  CONST64(0x0000000000040000), CONST64(0x0000000100040000), CONST64(0x0000000004040000), CONST64(0x0000000104040000), 
+  CONST64(0x0000000000000400), CONST64(0x0000000100000400), CONST64(0x0000000004000400), CONST64(0x0000000104000400), 
+  CONST64(0x0000000000040400), CONST64(0x0000000100040400), CONST64(0x0000000004040400), CONST64(0x0000000104040400), 
+  CONST64(0x0000000000000004), CONST64(0x0000000100000004), CONST64(0x0000000004000004), CONST64(0x0000000104000004), 
+  CONST64(0x0000000000040004), CONST64(0x0000000100040004), CONST64(0x0000000004040004), CONST64(0x0000000104040004), 
+  CONST64(0x0000000000000404), CONST64(0x0000000100000404), CONST64(0x0000000004000404), CONST64(0x0000000104000404), 
+  CONST64(0x0000000000040404), CONST64(0x0000000100040404), CONST64(0x0000000004040404), CONST64(0x0000000104040404), 
+  CONST64(0x0400000000000000), CONST64(0x0400000100000000), CONST64(0x0400000004000000), CONST64(0x0400000104000000), 
+  CONST64(0x0400000000040000), CONST64(0x0400000100040000), CONST64(0x0400000004040000), CONST64(0x0400000104040000), 
+  CONST64(0x0400000000000400), CONST64(0x0400000100000400), CONST64(0x0400000004000400), CONST64(0x0400000104000400), 
+  CONST64(0x0400000000040400), CONST64(0x0400000100040400), CONST64(0x0400000004040400), CONST64(0x0400000104040400), 
+  CONST64(0x0400000000000004), CONST64(0x0400000100000004), CONST64(0x0400000004000004), CONST64(0x0400000104000004), 
+  CONST64(0x0400000000040004), CONST64(0x0400000100040004), CONST64(0x0400000004040004), CONST64(0x0400000104040004), 
+  CONST64(0x0400000000000404), CONST64(0x0400000100000404), CONST64(0x0400000004000404), CONST64(0x0400000104000404), 
+  CONST64(0x0400000000040404), CONST64(0x0400000100040404), CONST64(0x0400000004040404), CONST64(0x0400000104040404), 
+  CONST64(0x0004000000000000), CONST64(0x0004000100000000), CONST64(0x0004000004000000), CONST64(0x0004000104000000), 
+  CONST64(0x0004000000040000), CONST64(0x0004000100040000), CONST64(0x0004000004040000), CONST64(0x0004000104040000), 
+  CONST64(0x0004000000000400), CONST64(0x0004000100000400), CONST64(0x0004000004000400), CONST64(0x0004000104000400), 
+  CONST64(0x0004000000040400), CONST64(0x0004000100040400), CONST64(0x0004000004040400), CONST64(0x0004000104040400), 
+  CONST64(0x0004000000000004), CONST64(0x0004000100000004), CONST64(0x0004000004000004), CONST64(0x0004000104000004), 
+  CONST64(0x0004000000040004), CONST64(0x0004000100040004), CONST64(0x0004000004040004), CONST64(0x0004000104040004), 
+  CONST64(0x0004000000000404), CONST64(0x0004000100000404), CONST64(0x0004000004000404), CONST64(0x0004000104000404), 
+  CONST64(0x0004000000040404), CONST64(0x0004000100040404), CONST64(0x0004000004040404), CONST64(0x0004000104040404), 
+  CONST64(0x0404000000000000), CONST64(0x0404000100000000), CONST64(0x0404000004000000), CONST64(0x0404000104000000), 
+  CONST64(0x0404000000040000), CONST64(0x0404000100040000), CONST64(0x0404000004040000), CONST64(0x0404000104040000), 
+  CONST64(0x0404000000000400), CONST64(0x0404000100000400), CONST64(0x0404000004000400), CONST64(0x0404000104000400), 
+  CONST64(0x0404000000040400), CONST64(0x0404000100040400), CONST64(0x0404000004040400), CONST64(0x0404000104040400), 
+  CONST64(0x0404000000000004), CONST64(0x0404000100000004), CONST64(0x0404000004000004), CONST64(0x0404000104000004), 
+  CONST64(0x0404000000040004), CONST64(0x0404000100040004), CONST64(0x0404000004040004), CONST64(0x0404000104040004), 
+  CONST64(0x0404000000000404), CONST64(0x0404000100000404), CONST64(0x0404000004000404), CONST64(0x0404000104000404), 
+  CONST64(0x0404000000040404), CONST64(0x0404000100040404), CONST64(0x0404000004040404), CONST64(0x0404000104040404), 
+  CONST64(0x0000040000000000), CONST64(0x0000040100000000), CONST64(0x0000040004000000), CONST64(0x0000040104000000), 
+  CONST64(0x0000040000040000), CONST64(0x0000040100040000), CONST64(0x0000040004040000), CONST64(0x0000040104040000), 
+  CONST64(0x0000040000000400), CONST64(0x0000040100000400), CONST64(0x0000040004000400), CONST64(0x0000040104000400), 
+  CONST64(0x0000040000040400), CONST64(0x0000040100040400), CONST64(0x0000040004040400), CONST64(0x0000040104040400), 
+  CONST64(0x0000040000000004), CONST64(0x0000040100000004), CONST64(0x0000040004000004), CONST64(0x0000040104000004), 
+  CONST64(0x0000040000040004), CONST64(0x0000040100040004), CONST64(0x0000040004040004), CONST64(0x0000040104040004), 
+  CONST64(0x0000040000000404), CONST64(0x0000040100000404), CONST64(0x0000040004000404), CONST64(0x0000040104000404), 
+  CONST64(0x0000040000040404), CONST64(0x0000040100040404), CONST64(0x0000040004040404), CONST64(0x0000040104040404), 
+  CONST64(0x0400040000000000), CONST64(0x0400040100000000), CONST64(0x0400040004000000), CONST64(0x0400040104000000), 
+  CONST64(0x0400040000040000), CONST64(0x0400040100040000), CONST64(0x0400040004040000), CONST64(0x0400040104040000), 
+  CONST64(0x0400040000000400), CONST64(0x0400040100000400), CONST64(0x0400040004000400), CONST64(0x0400040104000400), 
+  CONST64(0x0400040000040400), CONST64(0x0400040100040400), CONST64(0x0400040004040400), CONST64(0x0400040104040400), 
+  CONST64(0x0400040000000004), CONST64(0x0400040100000004), CONST64(0x0400040004000004), CONST64(0x0400040104000004), 
+  CONST64(0x0400040000040004), CONST64(0x0400040100040004), CONST64(0x0400040004040004), CONST64(0x0400040104040004), 
+  CONST64(0x0400040000000404), CONST64(0x0400040100000404), CONST64(0x0400040004000404), CONST64(0x0400040104000404), 
+  CONST64(0x0400040000040404), CONST64(0x0400040100040404), CONST64(0x0400040004040404), CONST64(0x0400040104040404), 
+  CONST64(0x0004040000000000), CONST64(0x0004040100000000), CONST64(0x0004040004000000), CONST64(0x0004040104000000), 
+  CONST64(0x0004040000040000), CONST64(0x0004040100040000), CONST64(0x0004040004040000), CONST64(0x0004040104040000), 
+  CONST64(0x0004040000000400), CONST64(0x0004040100000400), CONST64(0x0004040004000400), CONST64(0x0004040104000400), 
+  CONST64(0x0004040000040400), CONST64(0x0004040100040400), CONST64(0x0004040004040400), CONST64(0x0004040104040400), 
+  CONST64(0x0004040000000004), CONST64(0x0004040100000004), CONST64(0x0004040004000004), CONST64(0x0004040104000004), 
+  CONST64(0x0004040000040004), CONST64(0x0004040100040004), CONST64(0x0004040004040004), CONST64(0x0004040104040004), 
+  CONST64(0x0004040000000404), CONST64(0x0004040100000404), CONST64(0x0004040004000404), CONST64(0x0004040104000404), 
+  CONST64(0x0004040000040404), CONST64(0x0004040100040404), CONST64(0x0004040004040404), CONST64(0x0004040104040404), 
+  CONST64(0x0404040000000000), CONST64(0x0404040100000000), CONST64(0x0404040004000000), CONST64(0x0404040104000000), 
+  CONST64(0x0404040000040000), CONST64(0x0404040100040000), CONST64(0x0404040004040000), CONST64(0x0404040104040000), 
+  CONST64(0x0404040000000400), CONST64(0x0404040100000400), CONST64(0x0404040004000400), CONST64(0x0404040104000400), 
+  CONST64(0x0404040000040400), CONST64(0x0404040100040400), CONST64(0x0404040004040400), CONST64(0x0404040104040400), 
+  CONST64(0x0404040000000004), CONST64(0x0404040100000004), CONST64(0x0404040004000004), CONST64(0x0404040104000004), 
+  CONST64(0x0404040000040004), CONST64(0x0404040100040004), CONST64(0x0404040004040004), CONST64(0x0404040104040004), 
+  CONST64(0x0404040000000404), CONST64(0x0404040100000404), CONST64(0x0404040004000404), CONST64(0x0404040104000404), 
+  CONST64(0x0404040000040404), CONST64(0x0404040100040404), CONST64(0x0404040004040404), CONST64(0x0404040104040404)
+  }, 
+{ CONST64(0x0000000000000000), CONST64(0x0000000400000000), CONST64(0x0000000010000000), CONST64(0x0000000410000000), 
+  CONST64(0x0000000000100000), CONST64(0x0000000400100000), CONST64(0x0000000010100000), CONST64(0x0000000410100000), 
+  CONST64(0x0000000000001000), CONST64(0x0000000400001000), CONST64(0x0000000010001000), CONST64(0x0000000410001000), 
+  CONST64(0x0000000000101000), CONST64(0x0000000400101000), CONST64(0x0000000010101000), CONST64(0x0000000410101000), 
+  CONST64(0x0000000000000010), CONST64(0x0000000400000010), CONST64(0x0000000010000010), CONST64(0x0000000410000010), 
+  CONST64(0x0000000000100010), CONST64(0x0000000400100010), CONST64(0x0000000010100010), CONST64(0x0000000410100010), 
+  CONST64(0x0000000000001010), CONST64(0x0000000400001010), CONST64(0x0000000010001010), CONST64(0x0000000410001010), 
+  CONST64(0x0000000000101010), CONST64(0x0000000400101010), CONST64(0x0000000010101010), CONST64(0x0000000410101010), 
+  CONST64(0x1000000000000000), CONST64(0x1000000400000000), CONST64(0x1000000010000000), CONST64(0x1000000410000000), 
+  CONST64(0x1000000000100000), CONST64(0x1000000400100000), CONST64(0x1000000010100000), CONST64(0x1000000410100000), 
+  CONST64(0x1000000000001000), CONST64(0x1000000400001000), CONST64(0x1000000010001000), CONST64(0x1000000410001000), 
+  CONST64(0x1000000000101000), CONST64(0x1000000400101000), CONST64(0x1000000010101000), CONST64(0x1000000410101000), 
+  CONST64(0x1000000000000010), CONST64(0x1000000400000010), CONST64(0x1000000010000010), CONST64(0x1000000410000010), 
+  CONST64(0x1000000000100010), CONST64(0x1000000400100010), CONST64(0x1000000010100010), CONST64(0x1000000410100010), 
+  CONST64(0x1000000000001010), CONST64(0x1000000400001010), CONST64(0x1000000010001010), CONST64(0x1000000410001010), 
+  CONST64(0x1000000000101010), CONST64(0x1000000400101010), CONST64(0x1000000010101010), CONST64(0x1000000410101010), 
+  CONST64(0x0010000000000000), CONST64(0x0010000400000000), CONST64(0x0010000010000000), CONST64(0x0010000410000000), 
+  CONST64(0x0010000000100000), CONST64(0x0010000400100000), CONST64(0x0010000010100000), CONST64(0x0010000410100000), 
+  CONST64(0x0010000000001000), CONST64(0x0010000400001000), CONST64(0x0010000010001000), CONST64(0x0010000410001000), 
+  CONST64(0x0010000000101000), CONST64(0x0010000400101000), CONST64(0x0010000010101000), CONST64(0x0010000410101000), 
+  CONST64(0x0010000000000010), CONST64(0x0010000400000010), CONST64(0x0010000010000010), CONST64(0x0010000410000010), 
+  CONST64(0x0010000000100010), CONST64(0x0010000400100010), CONST64(0x0010000010100010), CONST64(0x0010000410100010), 
+  CONST64(0x0010000000001010), CONST64(0x0010000400001010), CONST64(0x0010000010001010), CONST64(0x0010000410001010), 
+  CONST64(0x0010000000101010), CONST64(0x0010000400101010), CONST64(0x0010000010101010), CONST64(0x0010000410101010), 
+  CONST64(0x1010000000000000), CONST64(0x1010000400000000), CONST64(0x1010000010000000), CONST64(0x1010000410000000), 
+  CONST64(0x1010000000100000), CONST64(0x1010000400100000), CONST64(0x1010000010100000), CONST64(0x1010000410100000), 
+  CONST64(0x1010000000001000), CONST64(0x1010000400001000), CONST64(0x1010000010001000), CONST64(0x1010000410001000), 
+  CONST64(0x1010000000101000), CONST64(0x1010000400101000), CONST64(0x1010000010101000), CONST64(0x1010000410101000), 
+  CONST64(0x1010000000000010), CONST64(0x1010000400000010), CONST64(0x1010000010000010), CONST64(0x1010000410000010), 
+  CONST64(0x1010000000100010), CONST64(0x1010000400100010), CONST64(0x1010000010100010), CONST64(0x1010000410100010), 
+  CONST64(0x1010000000001010), CONST64(0x1010000400001010), CONST64(0x1010000010001010), CONST64(0x1010000410001010), 
+  CONST64(0x1010000000101010), CONST64(0x1010000400101010), CONST64(0x1010000010101010), CONST64(0x1010000410101010), 
+  CONST64(0x0000100000000000), CONST64(0x0000100400000000), CONST64(0x0000100010000000), CONST64(0x0000100410000000), 
+  CONST64(0x0000100000100000), CONST64(0x0000100400100000), CONST64(0x0000100010100000), CONST64(0x0000100410100000), 
+  CONST64(0x0000100000001000), CONST64(0x0000100400001000), CONST64(0x0000100010001000), CONST64(0x0000100410001000), 
+  CONST64(0x0000100000101000), CONST64(0x0000100400101000), CONST64(0x0000100010101000), CONST64(0x0000100410101000), 
+  CONST64(0x0000100000000010), CONST64(0x0000100400000010), CONST64(0x0000100010000010), CONST64(0x0000100410000010), 
+  CONST64(0x0000100000100010), CONST64(0x0000100400100010), CONST64(0x0000100010100010), CONST64(0x0000100410100010), 
+  CONST64(0x0000100000001010), CONST64(0x0000100400001010), CONST64(0x0000100010001010), CONST64(0x0000100410001010), 
+  CONST64(0x0000100000101010), CONST64(0x0000100400101010), CONST64(0x0000100010101010), CONST64(0x0000100410101010), 
+  CONST64(0x1000100000000000), CONST64(0x1000100400000000), CONST64(0x1000100010000000), CONST64(0x1000100410000000), 
+  CONST64(0x1000100000100000), CONST64(0x1000100400100000), CONST64(0x1000100010100000), CONST64(0x1000100410100000), 
+  CONST64(0x1000100000001000), CONST64(0x1000100400001000), CONST64(0x1000100010001000), CONST64(0x1000100410001000), 
+  CONST64(0x1000100000101000), CONST64(0x1000100400101000), CONST64(0x1000100010101000), CONST64(0x1000100410101000), 
+  CONST64(0x1000100000000010), CONST64(0x1000100400000010), CONST64(0x1000100010000010), CONST64(0x1000100410000010), 
+  CONST64(0x1000100000100010), CONST64(0x1000100400100010), CONST64(0x1000100010100010), CONST64(0x1000100410100010), 
+  CONST64(0x1000100000001010), CONST64(0x1000100400001010), CONST64(0x1000100010001010), CONST64(0x1000100410001010), 
+  CONST64(0x1000100000101010), CONST64(0x1000100400101010), CONST64(0x1000100010101010), CONST64(0x1000100410101010), 
+  CONST64(0x0010100000000000), CONST64(0x0010100400000000), CONST64(0x0010100010000000), CONST64(0x0010100410000000), 
+  CONST64(0x0010100000100000), CONST64(0x0010100400100000), CONST64(0x0010100010100000), CONST64(0x0010100410100000), 
+  CONST64(0x0010100000001000), CONST64(0x0010100400001000), CONST64(0x0010100010001000), CONST64(0x0010100410001000), 
+  CONST64(0x0010100000101000), CONST64(0x0010100400101000), CONST64(0x0010100010101000), CONST64(0x0010100410101000), 
+  CONST64(0x0010100000000010), CONST64(0x0010100400000010), CONST64(0x0010100010000010), CONST64(0x0010100410000010), 
+  CONST64(0x0010100000100010), CONST64(0x0010100400100010), CONST64(0x0010100010100010), CONST64(0x0010100410100010), 
+  CONST64(0x0010100000001010), CONST64(0x0010100400001010), CONST64(0x0010100010001010), CONST64(0x0010100410001010), 
+  CONST64(0x0010100000101010), CONST64(0x0010100400101010), CONST64(0x0010100010101010), CONST64(0x0010100410101010), 
+  CONST64(0x1010100000000000), CONST64(0x1010100400000000), CONST64(0x1010100010000000), CONST64(0x1010100410000000), 
+  CONST64(0x1010100000100000), CONST64(0x1010100400100000), CONST64(0x1010100010100000), CONST64(0x1010100410100000), 
+  CONST64(0x1010100000001000), CONST64(0x1010100400001000), CONST64(0x1010100010001000), CONST64(0x1010100410001000), 
+  CONST64(0x1010100000101000), CONST64(0x1010100400101000), CONST64(0x1010100010101000), CONST64(0x1010100410101000), 
+  CONST64(0x1010100000000010), CONST64(0x1010100400000010), CONST64(0x1010100010000010), CONST64(0x1010100410000010), 
+  CONST64(0x1010100000100010), CONST64(0x1010100400100010), CONST64(0x1010100010100010), CONST64(0x1010100410100010), 
+  CONST64(0x1010100000001010), CONST64(0x1010100400001010), CONST64(0x1010100010001010), CONST64(0x1010100410001010), 
+  CONST64(0x1010100000101010), CONST64(0x1010100400101010), CONST64(0x1010100010101010), CONST64(0x1010100410101010)
+  }, 
+{ CONST64(0x0000000000000000), CONST64(0x0000001000000000), CONST64(0x0000000040000000), CONST64(0x0000001040000000), 
+  CONST64(0x0000000000400000), CONST64(0x0000001000400000), CONST64(0x0000000040400000), CONST64(0x0000001040400000), 
+  CONST64(0x0000000000004000), CONST64(0x0000001000004000), CONST64(0x0000000040004000), CONST64(0x0000001040004000), 
+  CONST64(0x0000000000404000), CONST64(0x0000001000404000), CONST64(0x0000000040404000), CONST64(0x0000001040404000), 
+  CONST64(0x0000000000000040), CONST64(0x0000001000000040), CONST64(0x0000000040000040), CONST64(0x0000001040000040), 
+  CONST64(0x0000000000400040), CONST64(0x0000001000400040), CONST64(0x0000000040400040), CONST64(0x0000001040400040), 
+  CONST64(0x0000000000004040), CONST64(0x0000001000004040), CONST64(0x0000000040004040), CONST64(0x0000001040004040), 
+  CONST64(0x0000000000404040), CONST64(0x0000001000404040), CONST64(0x0000000040404040), CONST64(0x0000001040404040), 
+  CONST64(0x4000000000000000), CONST64(0x4000001000000000), CONST64(0x4000000040000000), CONST64(0x4000001040000000), 
+  CONST64(0x4000000000400000), CONST64(0x4000001000400000), CONST64(0x4000000040400000), CONST64(0x4000001040400000), 
+  CONST64(0x4000000000004000), CONST64(0x4000001000004000), CONST64(0x4000000040004000), CONST64(0x4000001040004000), 
+  CONST64(0x4000000000404000), CONST64(0x4000001000404000), CONST64(0x4000000040404000), CONST64(0x4000001040404000), 
+  CONST64(0x4000000000000040), CONST64(0x4000001000000040), CONST64(0x4000000040000040), CONST64(0x4000001040000040), 
+  CONST64(0x4000000000400040), CONST64(0x4000001000400040), CONST64(0x4000000040400040), CONST64(0x4000001040400040), 
+  CONST64(0x4000000000004040), CONST64(0x4000001000004040), CONST64(0x4000000040004040), CONST64(0x4000001040004040), 
+  CONST64(0x4000000000404040), CONST64(0x4000001000404040), CONST64(0x4000000040404040), CONST64(0x4000001040404040), 
+  CONST64(0x0040000000000000), CONST64(0x0040001000000000), CONST64(0x0040000040000000), CONST64(0x0040001040000000), 
+  CONST64(0x0040000000400000), CONST64(0x0040001000400000), CONST64(0x0040000040400000), CONST64(0x0040001040400000), 
+  CONST64(0x0040000000004000), CONST64(0x0040001000004000), CONST64(0x0040000040004000), CONST64(0x0040001040004000), 
+  CONST64(0x0040000000404000), CONST64(0x0040001000404000), CONST64(0x0040000040404000), CONST64(0x0040001040404000), 
+  CONST64(0x0040000000000040), CONST64(0x0040001000000040), CONST64(0x0040000040000040), CONST64(0x0040001040000040), 
+  CONST64(0x0040000000400040), CONST64(0x0040001000400040), CONST64(0x0040000040400040), CONST64(0x0040001040400040), 
+  CONST64(0x0040000000004040), CONST64(0x0040001000004040), CONST64(0x0040000040004040), CONST64(0x0040001040004040), 
+  CONST64(0x0040000000404040), CONST64(0x0040001000404040), CONST64(0x0040000040404040), CONST64(0x0040001040404040), 
+  CONST64(0x4040000000000000), CONST64(0x4040001000000000), CONST64(0x4040000040000000), CONST64(0x4040001040000000), 
+  CONST64(0x4040000000400000), CONST64(0x4040001000400000), CONST64(0x4040000040400000), CONST64(0x4040001040400000), 
+  CONST64(0x4040000000004000), CONST64(0x4040001000004000), CONST64(0x4040000040004000), CONST64(0x4040001040004000), 
+  CONST64(0x4040000000404000), CONST64(0x4040001000404000), CONST64(0x4040000040404000), CONST64(0x4040001040404000), 
+  CONST64(0x4040000000000040), CONST64(0x4040001000000040), CONST64(0x4040000040000040), CONST64(0x4040001040000040), 
+  CONST64(0x4040000000400040), CONST64(0x4040001000400040), CONST64(0x4040000040400040), CONST64(0x4040001040400040), 
+  CONST64(0x4040000000004040), CONST64(0x4040001000004040), CONST64(0x4040000040004040), CONST64(0x4040001040004040), 
+  CONST64(0x4040000000404040), CONST64(0x4040001000404040), CONST64(0x4040000040404040), CONST64(0x4040001040404040), 
+  CONST64(0x0000400000000000), CONST64(0x0000401000000000), CONST64(0x0000400040000000), CONST64(0x0000401040000000), 
+  CONST64(0x0000400000400000), CONST64(0x0000401000400000), CONST64(0x0000400040400000), CONST64(0x0000401040400000), 
+  CONST64(0x0000400000004000), CONST64(0x0000401000004000), CONST64(0x0000400040004000), CONST64(0x0000401040004000), 
+  CONST64(0x0000400000404000), CONST64(0x0000401000404000), CONST64(0x0000400040404000), CONST64(0x0000401040404000), 
+  CONST64(0x0000400000000040), CONST64(0x0000401000000040), CONST64(0x0000400040000040), CONST64(0x0000401040000040), 
+  CONST64(0x0000400000400040), CONST64(0x0000401000400040), CONST64(0x0000400040400040), CONST64(0x0000401040400040), 
+  CONST64(0x0000400000004040), CONST64(0x0000401000004040), CONST64(0x0000400040004040), CONST64(0x0000401040004040), 
+  CONST64(0x0000400000404040), CONST64(0x0000401000404040), CONST64(0x0000400040404040), CONST64(0x0000401040404040), 
+  CONST64(0x4000400000000000), CONST64(0x4000401000000000), CONST64(0x4000400040000000), CONST64(0x4000401040000000), 
+  CONST64(0x4000400000400000), CONST64(0x4000401000400000), CONST64(0x4000400040400000), CONST64(0x4000401040400000), 
+  CONST64(0x4000400000004000), CONST64(0x4000401000004000), CONST64(0x4000400040004000), CONST64(0x4000401040004000), 
+  CONST64(0x4000400000404000), CONST64(0x4000401000404000), CONST64(0x4000400040404000), CONST64(0x4000401040404000), 
+  CONST64(0x4000400000000040), CONST64(0x4000401000000040), CONST64(0x4000400040000040), CONST64(0x4000401040000040), 
+  CONST64(0x4000400000400040), CONST64(0x4000401000400040), CONST64(0x4000400040400040), CONST64(0x4000401040400040), 
+  CONST64(0x4000400000004040), CONST64(0x4000401000004040), CONST64(0x4000400040004040), CONST64(0x4000401040004040), 
+  CONST64(0x4000400000404040), CONST64(0x4000401000404040), CONST64(0x4000400040404040), CONST64(0x4000401040404040), 
+  CONST64(0x0040400000000000), CONST64(0x0040401000000000), CONST64(0x0040400040000000), CONST64(0x0040401040000000), 
+  CONST64(0x0040400000400000), CONST64(0x0040401000400000), CONST64(0x0040400040400000), CONST64(0x0040401040400000), 
+  CONST64(0x0040400000004000), CONST64(0x0040401000004000), CONST64(0x0040400040004000), CONST64(0x0040401040004000), 
+  CONST64(0x0040400000404000), CONST64(0x0040401000404000), CONST64(0x0040400040404000), CONST64(0x0040401040404000), 
+  CONST64(0x0040400000000040), CONST64(0x0040401000000040), CONST64(0x0040400040000040), CONST64(0x0040401040000040), 
+  CONST64(0x0040400000400040), CONST64(0x0040401000400040), CONST64(0x0040400040400040), CONST64(0x0040401040400040), 
+  CONST64(0x0040400000004040), CONST64(0x0040401000004040), CONST64(0x0040400040004040), CONST64(0x0040401040004040), 
+  CONST64(0x0040400000404040), CONST64(0x0040401000404040), CONST64(0x0040400040404040), CONST64(0x0040401040404040), 
+  CONST64(0x4040400000000000), CONST64(0x4040401000000000), CONST64(0x4040400040000000), CONST64(0x4040401040000000), 
+  CONST64(0x4040400000400000), CONST64(0x4040401000400000), CONST64(0x4040400040400000), CONST64(0x4040401040400000), 
+  CONST64(0x4040400000004000), CONST64(0x4040401000004000), CONST64(0x4040400040004000), CONST64(0x4040401040004000), 
+  CONST64(0x4040400000404000), CONST64(0x4040401000404000), CONST64(0x4040400040404000), CONST64(0x4040401040404000), 
+  CONST64(0x4040400000000040), CONST64(0x4040401000000040), CONST64(0x4040400040000040), CONST64(0x4040401040000040), 
+  CONST64(0x4040400000400040), CONST64(0x4040401000400040), CONST64(0x4040400040400040), CONST64(0x4040401040400040), 
+  CONST64(0x4040400000004040), CONST64(0x4040401000004040), CONST64(0x4040400040004040), CONST64(0x4040401040004040), 
+  CONST64(0x4040400000404040), CONST64(0x4040401000404040), CONST64(0x4040400040404040), CONST64(0x4040401040404040)
+  }};
+  
+#endif
+
+
+static void cookey(const ulong32 *raw1, ulong32 *keyout);
+
+#ifdef LTC_CLEAN_STACK
+static void _deskey(const unsigned char *key, short edf, ulong32 *keyout)
+#else
+static void deskey(const unsigned char *key, short edf, ulong32 *keyout)
+#endif
+{
+    ulong32 i, j, l, m, n, kn[32];
+    unsigned char pc1m[56], pcr[56];
+
+    for (j=0; j < 56; j++) {
+        l = (ulong32)pc1[j];
+        m = l & 7;
+        pc1m[j] = (unsigned char)((key[l >> 3U] & bytebit[m]) == bytebit[m] ? 1 : 0);
+    }
+
+    for (i=0; i < 16; i++) {
+        if (edf == DE1) {
+           m = (15 - i) << 1;
+        } else {
+           m = i << 1;
+        }
+        n = m + 1;
+        kn[m] = kn[n] = 0L;
+        for (j=0; j < 28; j++) {
+            l = j + (ulong32)totrot[i];
+            if (l < 28) {
+               pcr[j] = pc1m[l];
+            } else {
+               pcr[j] = pc1m[l - 28];
+            }
+        }
+        for (/*j = 28*/; j < 56; j++) {
+            l = j + (ulong32)totrot[i];
+            if (l < 56) {
+               pcr[j] = pc1m[l];
+            } else {
+               pcr[j] = pc1m[l - 28];
+            }
+        }
+        for (j=0; j < 24; j++)  {
+            if ((int)pcr[(int)pc2[j]] != 0) {
+               kn[m] |= bigbyte[j];
+            }
+            if ((int)pcr[(int)pc2[j+24]] != 0) {
+               kn[n] |= bigbyte[j];
+            }
+        }
+    }
+
+    cookey(kn, keyout);
+}
+
+#ifdef LTC_CLEAN_STACK
+static void deskey(const unsigned char *key, short edf, ulong32 *keyout)
+{
+   _deskey(key, edf, keyout);
+   burn_stack(sizeof(int)*5 + sizeof(ulong32)*32 + sizeof(unsigned char)*112);
+}
+#endif
+
+#ifdef LTC_CLEAN_STACK
+static void _cookey(const ulong32 *raw1, ulong32 *keyout)
+#else
+static void cookey(const ulong32 *raw1, ulong32 *keyout)
+#endif
+{
+    ulong32 *cook;
+    const ulong32 *raw0;
+    ulong32 dough[32];
+    int i;
+
+    cook = dough;
+    for(i=0; i < 16; i++, raw1++)
+    {
+        raw0 = raw1++;
+        *cook    = (*raw0 & 0x00fc0000L) << 6;
+        *cook   |= (*raw0 & 0x00000fc0L) << 10;
+        *cook   |= (*raw1 & 0x00fc0000L) >> 10;
+        *cook++ |= (*raw1 & 0x00000fc0L) >> 6;
+        *cook    = (*raw0 & 0x0003f000L) << 12;
+        *cook   |= (*raw0 & 0x0000003fL) << 16;
+        *cook   |= (*raw1 & 0x0003f000L) >> 4;
+        *cook++ |= (*raw1 & 0x0000003fL);
+    }
+
+    XMEMCPY(keyout, dough, sizeof dough);
+}
+
+#ifdef LTC_CLEAN_STACK
+static void cookey(const ulong32 *raw1, ulong32 *keyout)
+{
+   _cookey(raw1, keyout);
+   burn_stack(sizeof(ulong32 *) * 2 + sizeof(ulong32)*32 + sizeof(int));
+}
+#endif
+
+#ifndef LTC_CLEAN_STACK
+static void desfunc(ulong32 *block, const ulong32 *keys)
+#else
+static void _desfunc(ulong32 *block, const ulong32 *keys)
+#endif
+{
+    ulong32 work, right, leftt;
+    int cur_round;
+
+    leftt = block[0];
+    right = block[1];
+
+#ifdef LTC_SMALL_CODE
+    work = ((leftt >> 4)  ^ right) & 0x0f0f0f0fL;
+    right ^= work;
+    leftt ^= (work << 4);
+
+    work = ((leftt >> 16) ^ right) & 0x0000ffffL;
+    right ^= work;
+    leftt ^= (work << 16);
+
+    work = ((right >> 2)  ^ leftt) & 0x33333333L;
+    leftt ^= work;
+    right ^= (work << 2);
+
+    work = ((right >> 8)  ^ leftt) & 0x00ff00ffL;
+    leftt ^= work;
+    right ^= (work << 8);
+
+    right = ROLc(right, 1);
+    work = (leftt ^ right) & 0xaaaaaaaaL;
+    
+    leftt ^= work;
+    right ^= work;
+    leftt = ROLc(leftt, 1);
+#else 
+   {
+      ulong64 tmp;
+      tmp = des_ip[0][byte(leftt, 0)] ^
+            des_ip[1][byte(leftt, 1)] ^
+            des_ip[2][byte(leftt, 2)] ^
+            des_ip[3][byte(leftt, 3)] ^
+            des_ip[4][byte(right, 0)] ^
+            des_ip[5][byte(right, 1)] ^
+            des_ip[6][byte(right, 2)] ^
+            des_ip[7][byte(right, 3)];
+      leftt = (ulong32)(tmp >> 32);
+      right = (ulong32)(tmp & 0xFFFFFFFFUL);
+   }
+#endif
+
+    for (cur_round = 0; cur_round < 8; cur_round++) {
+        work  = RORc(right, 4) ^ *keys++;
+        leftt ^= SP7[work        & 0x3fL]
+              ^ SP5[(work >>  8) & 0x3fL]
+              ^ SP3[(work >> 16) & 0x3fL]
+              ^ SP1[(work >> 24) & 0x3fL];
+        work  = right ^ *keys++;
+        leftt ^= SP8[ work        & 0x3fL]
+              ^  SP6[(work >>  8) & 0x3fL]
+              ^  SP4[(work >> 16) & 0x3fL]
+              ^  SP2[(work >> 24) & 0x3fL];
+
+        work = RORc(leftt, 4) ^ *keys++;
+        right ^= SP7[ work        & 0x3fL]
+              ^  SP5[(work >>  8) & 0x3fL]
+              ^  SP3[(work >> 16) & 0x3fL]
+              ^  SP1[(work >> 24) & 0x3fL];
+        work  = leftt ^ *keys++;
+        right ^= SP8[ work        & 0x3fL]
+              ^  SP6[(work >>  8) & 0x3fL]
+              ^  SP4[(work >> 16) & 0x3fL]
+              ^  SP2[(work >> 24) & 0x3fL];
+    }
+
+#ifdef LTC_SMALL_CODE    
+    right = RORc(right, 1);
+    work = (leftt ^ right) & 0xaaaaaaaaL;
+    leftt ^= work;
+    right ^= work;
+    leftt = RORc(leftt, 1);
+    work = ((leftt >> 8) ^ right) & 0x00ff00ffL;
+    right ^= work;
+    leftt ^= (work << 8);
+    /* -- */
+    work = ((leftt >> 2) ^ right) & 0x33333333L;
+    right ^= work;
+    leftt ^= (work << 2);
+    work = ((right >> 16) ^ leftt) & 0x0000ffffL;
+    leftt ^= work;
+    right ^= (work << 16);
+    work = ((right >> 4) ^ leftt) & 0x0f0f0f0fL;
+    leftt ^= work;
+    right ^= (work << 4);
+#else 
+   {
+      ulong64 tmp;
+      tmp = des_fp[0][byte(leftt, 0)] ^
+            des_fp[1][byte(leftt, 1)] ^
+            des_fp[2][byte(leftt, 2)] ^
+            des_fp[3][byte(leftt, 3)] ^
+            des_fp[4][byte(right, 0)] ^
+            des_fp[5][byte(right, 1)] ^
+            des_fp[6][byte(right, 2)] ^
+            des_fp[7][byte(right, 3)];
+      leftt = (ulong32)(tmp >> 32);
+      right = (ulong32)(tmp & 0xFFFFFFFFUL);
+   }
+#endif
+    
+    block[0] = right;
+    block[1] = leftt;
+}
+
+#ifdef LTC_CLEAN_STACK
+static void desfunc(ulong32 *block, const ulong32 *keys)
+{
+   _desfunc(block, keys);
+   burn_stack(sizeof(ulong32) * 4 + sizeof(int));
+}
+#endif
+
+ /**
+    Initialize the LTC_DES block cipher
+    @param key The symmetric key you wish to pass
+    @param keylen The key length in bytes
+    @param num_rounds The number of rounds desired (0 for default)
+    @param skey The key in as scheduled by this function.
+    @return CRYPT_OK if successful
+ */
+int des_setup(const unsigned char *key, int keylen, int num_rounds, symmetric_key *skey)
+{
+    LTC_ARGCHK(key != NULL);
+    LTC_ARGCHK(skey != NULL);
+
+    if (num_rounds != 0 && num_rounds != 16) {
+        return CRYPT_INVALID_ROUNDS;
+    }
+
+    if (keylen != 8) {
+        return CRYPT_INVALID_KEYSIZE;
+    }
+
+    deskey(key, EN0, skey->des.ek);
+    deskey(key, DE1, skey->des.dk);
+
+    return CRYPT_OK;
+}
+
+ /**
+    Initialize the 3LTC_DES-EDE block cipher
+    @param key The symmetric key you wish to pass
+    @param keylen The key length in bytes
+    @param num_rounds The number of rounds desired (0 for default)
+    @param skey The key in as scheduled by this function.
+    @return CRYPT_OK if successful
+ */
+int des3_setup(const unsigned char *key, int keylen, int num_rounds, symmetric_key *skey)
+{
+    LTC_ARGCHK(key != NULL);
+    LTC_ARGCHK(skey != NULL);
+
+    if(num_rounds != 0 && num_rounds != 16) {
+        return CRYPT_INVALID_ROUNDS;
+    }
+
+    if (keylen != 24 && keylen != 16) {
+        return CRYPT_INVALID_KEYSIZE;
+    }
+
+    deskey(key,    EN0, skey->des3.ek[0]);
+    deskey(key+8,  DE1, skey->des3.ek[1]);
+    if (keylen == 24) {
+        deskey(key+16, EN0, skey->des3.ek[2]);
+    } else {
+        /* two-key 3DES: K3=K1 */
+        deskey(key, EN0, skey->des3.ek[2]);
+    }
+
+    deskey(key,    DE1, skey->des3.dk[2]);
+    deskey(key+8,  EN0, skey->des3.dk[1]);
+    if (keylen == 24) {
+        deskey(key+16, DE1, skey->des3.dk[0]);
+    } else {
+        /* two-key 3DES: K3=K1 */
+        deskey(key, DE1, skey->des3.dk[0]);
+    }
+
+    return CRYPT_OK;
+}
+
+/**
+  Encrypts a block of text with LTC_DES
+  @param pt The input plaintext (8 bytes)
+  @param ct The output ciphertext (8 bytes)
+  @param skey The key as scheduled
+  @return CRYPT_OK if successful
+*/
+int des_ecb_encrypt(const unsigned char *pt, unsigned char *ct, symmetric_key *skey)
+{
+    ulong32 work[2];
+    LTC_ARGCHK(pt   != NULL);
+    LTC_ARGCHK(ct   != NULL);
+    LTC_ARGCHK(skey != NULL);
+    LOAD32H(work[0], pt+0);
+    LOAD32H(work[1], pt+4);
+    desfunc(work, skey->des.ek);
+    STORE32H(work[0],ct+0);
+    STORE32H(work[1],ct+4);
+    return CRYPT_OK;
+}
+
+/**
+  Decrypts a block of text with LTC_DES
+  @param ct The input ciphertext (8 bytes)
+  @param pt The output plaintext (8 bytes)
+  @param skey The key as scheduled 
+  @return CRYPT_OK if successful
+*/
+int des_ecb_decrypt(const unsigned char *ct, unsigned char *pt, symmetric_key *skey)
+{
+    ulong32 work[2];
+    LTC_ARGCHK(pt   != NULL);
+    LTC_ARGCHK(ct   != NULL);
+    LTC_ARGCHK(skey != NULL);
+    LOAD32H(work[0], ct+0);
+    LOAD32H(work[1], ct+4);
+    desfunc(work, skey->des.dk);
+    STORE32H(work[0],pt+0);
+    STORE32H(work[1],pt+4);  
+    return CRYPT_OK;
+}
+
+/**
+  Encrypts a block of text with 3LTC_DES-EDE
+  @param pt The input plaintext (8 bytes)
+  @param ct The output ciphertext (8 bytes)
+  @param skey The key as scheduled
+  @return CRYPT_OK if successful
+*/
+int des3_ecb_encrypt(const unsigned char *pt, unsigned char *ct, symmetric_key *skey)
+{
+    ulong32 work[2];
+    
+    LTC_ARGCHK(pt   != NULL);
+    LTC_ARGCHK(ct   != NULL);
+    LTC_ARGCHK(skey != NULL);
+    LOAD32H(work[0], pt+0);
+    LOAD32H(work[1], pt+4);
+    desfunc(work, skey->des3.ek[0]);
+    desfunc(work, skey->des3.ek[1]);
+    desfunc(work, skey->des3.ek[2]);
+    STORE32H(work[0],ct+0);
+    STORE32H(work[1],ct+4);
+    return CRYPT_OK;
+}
+
+/**
+  Decrypts a block of text with 3LTC_DES-EDE
+  @param ct The input ciphertext (8 bytes)
+  @param pt The output plaintext (8 bytes)
+  @param skey The key as scheduled 
+  @return CRYPT_OK if successful
+*/
+int des3_ecb_decrypt(const unsigned char *ct, unsigned char *pt, symmetric_key *skey)
+{
+    ulong32 work[2];
+    LTC_ARGCHK(pt   != NULL);
+    LTC_ARGCHK(ct   != NULL);
+    LTC_ARGCHK(skey != NULL);
+    LOAD32H(work[0], ct+0);
+    LOAD32H(work[1], ct+4);
+    desfunc(work, skey->des3.dk[0]);
+    desfunc(work, skey->des3.dk[1]);
+    desfunc(work, skey->des3.dk[2]);
+    STORE32H(work[0],pt+0);
+    STORE32H(work[1],pt+4);
+    return CRYPT_OK;
+}
+
+/**
+  Performs a self-test of the LTC_DES block cipher
+  @return CRYPT_OK if functional, CRYPT_NOP if self-test has been disabled
+*/
+int des_test(void)
+{
+ #ifndef LTC_TEST
+    return CRYPT_NOP;
+ #else    
+    int err;
+    static const struct des_test_case {
+        int num, mode; /* mode 1 = encrypt */
+        unsigned char key[8], txt[8], out[8];
+    } cases[] = {
+        { 1, 1,     { 0x10, 0x31, 0x6E, 0x02, 0x8C, 0x8F, 0x3B, 0x4A },
+                    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },
+                    { 0x82, 0xDC, 0xBA, 0xFB, 0xDE, 0xAB, 0x66, 0x02 } },
+        { 2, 1,     { 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01 },
+                    { 0x95, 0xF8, 0xA5, 0xE5, 0xDD, 0x31, 0xD9, 0x00 },
+                    { 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 } },
+        { 3, 1,     { 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01 },
+                    { 0xDD, 0x7F, 0x12, 0x1C, 0xA5, 0x01, 0x56, 0x19 },
+                    { 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 } },
+        { 4, 1,     { 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01 },
+                    { 0x2E, 0x86, 0x53, 0x10, 0x4F, 0x38, 0x34, 0xEA },
+                    { 0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 } },
+        { 5, 1,     { 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01 },
+                    { 0x4B, 0xD3, 0x88, 0xFF, 0x6C, 0xD8, 0x1D, 0x4F },
+                    { 0x10, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 } },
+        { 6, 1,     { 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01 },
+                    { 0x20, 0xB9, 0xE7, 0x67, 0xB2, 0xFB, 0x14, 0x56 },
+                    { 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 } },
+        { 7, 1,     { 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01 },
+                    { 0x55, 0x57, 0x93, 0x80, 0xD7, 0x71, 0x38, 0xEF },
+                    { 0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 } },
+        { 8, 1,     { 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01 },
+                    { 0x6C, 0xC5, 0xDE, 0xFA, 0xAF, 0x04, 0x51, 0x2F },
+                    { 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 } },
+        { 9, 1,     { 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01 },
+                    { 0x0D, 0x9F, 0x27, 0x9B, 0xA5, 0xD8, 0x72, 0x60 }, 
+                    { 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 } },
+        {10, 1,     { 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01 },
+                    { 0xD9, 0x03, 0x1B, 0x02, 0x71, 0xBD, 0x5A, 0x0A },
+                    { 0x00, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 } },
+
+        { 1, 0,     { 0x10, 0x31, 0x6E, 0x02, 0x8C, 0x8F, 0x3B, 0x4A },
+                    { 0x82, 0xDC, 0xBA, 0xFB, 0xDE, 0xAB, 0x66, 0x02 },
+                    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 } },
+        { 2, 0,     { 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01 },
+                    { 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },
+                    { 0x95, 0xF8, 0xA5, 0xE5, 0xDD, 0x31, 0xD9, 0x00 } },
+        { 3, 0,     { 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01 },
+                    { 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },
+                    { 0xDD, 0x7F, 0x12, 0x1C, 0xA5, 0x01, 0x56, 0x19 } },
+        { 4, 0,     { 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01 },
+                    { 0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },
+                    { 0x2E, 0x86, 0x53, 0x10, 0x4F, 0x38, 0x34, 0xEA } },
+        { 5, 0,     { 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01 },
+                    { 0x10, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },
+                    { 0x4B, 0xD3, 0x88, 0xFF, 0x6C, 0xD8, 0x1D, 0x4F } },
+        { 6, 0,     { 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01 },
+                    { 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },
+                    { 0x20, 0xB9, 0xE7, 0x67, 0xB2, 0xFB, 0x14, 0x56 } },
+        { 7, 0,     { 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01 },
+                    { 0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },
+                    { 0x55, 0x57, 0x93, 0x80, 0xD7, 0x71, 0x38, 0xEF } },
+        { 8, 0,     { 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01 },
+                    { 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },
+                    { 0x6C, 0xC5, 0xDE, 0xFA, 0xAF, 0x04, 0x51, 0x2F } },
+        { 9, 0,     { 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01 },
+                    { 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },
+                    { 0x0D, 0x9F, 0x27, 0x9B, 0xA5, 0xD8, 0x72, 0x60 } }, 
+        {10, 0,     { 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01 },
+                    { 0x00, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },
+                    { 0xD9, 0x03, 0x1B, 0x02, 0x71, 0xBD, 0x5A, 0x0A } },
+
+#ifdef LTC_TEST_EXT
+        { 0+11, 0,  { 0x80, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01 },
+                    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },
+                    { 0x95, 0xA8, 0xD7, 0x28, 0x13, 0xDA, 0xA9, 0x4D } },
+        { 1+11, 0,  { 0x40, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01 },
+                    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },
+                    { 0x0E, 0xEC, 0x14, 0x87, 0xDD, 0x8C, 0x26, 0xD5 } },
+        { 2+11, 0,  { 0x20, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01 },
+                    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },
+                    { 0x7A, 0xD1, 0x6F, 0xFB, 0x79, 0xC4, 0x59, 0x26 } },
+        { 3+11, 0,  { 0x10, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01 },
+                    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },
+                    { 0xD3, 0x74, 0x62, 0x94, 0xCA, 0x6A, 0x6C, 0xF3 } },
+        { 4+11, 0,  { 0x08, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01 },
+                    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },
+                    { 0x80, 0x9F, 0x5F, 0x87, 0x3C, 0x1F, 0xD7, 0x61 } },
+        { 5+11, 0,  { 0x04, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01 },
+                    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },
+                    { 0xC0, 0x2F, 0xAF, 0xFE, 0xC9, 0x89, 0xD1, 0xFC } },
+        { 6+11, 0,  { 0x02, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01 },
+                    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },
+                    { 0x46, 0x15, 0xAA, 0x1D, 0x33, 0xE7, 0x2F, 0x10 } },
+        { 7+11, 0,  { 0x01, 0x80, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01 },
+                    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },
+                    { 0x20, 0x55, 0x12, 0x33, 0x50, 0xC0, 0x08, 0x58 } },
+        { 8+11, 0,  { 0x01, 0x40, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01 },
+                    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },
+                    { 0xDF, 0x3B, 0x99, 0xD6, 0x57, 0x73, 0x97, 0xC8 } },
+        { 9+11, 0,  { 0x01, 0x20, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01 },
+                    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },
+                    { 0x31, 0xFE, 0x17, 0x36, 0x9B, 0x52, 0x88, 0xC9 } },
+        {10+11, 0,  { 0x01, 0x10, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01 },
+                    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },
+                    { 0xDF, 0xDD, 0x3C, 0xC6, 0x4D, 0xAE, 0x16, 0x42 } },
+        {11+11, 0,  { 0x01, 0x08, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01 },
+                    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },
+                    { 0x17, 0x8C, 0x83, 0xCE, 0x2B, 0x39, 0x9D, 0x94 } },
+        {12+11, 0,  { 0x01, 0x04, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01 },
+                    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },
+                    { 0x50, 0xF6, 0x36, 0x32, 0x4A, 0x9B, 0x7F, 0x80 } },
+        {13+11, 0,  { 0x01, 0x02, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01 },
+                    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },
+                    { 0xA8, 0x46, 0x8E, 0xE3, 0xBC, 0x18, 0xF0, 0x6D } },
+        {14+11, 0,  { 0x01, 0x01, 0x80, 0x01, 0x01, 0x01, 0x01, 0x01 },
+                    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },
+                    { 0xA2, 0xDC, 0x9E, 0x92, 0xFD, 0x3C, 0xDE, 0x92 } },
+        {15+11, 0,  { 0x01, 0x01, 0x40, 0x01, 0x01, 0x01, 0x01, 0x01 },
+                    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },
+                    { 0xCA, 0xC0, 0x9F, 0x79, 0x7D, 0x03, 0x12, 0x87 } },
+        {16+11, 0,  { 0x01, 0x01, 0x20, 0x01, 0x01, 0x01, 0x01, 0x01 },
+                    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },
+                    { 0x90, 0xBA, 0x68, 0x0B, 0x22, 0xAE, 0xB5, 0x25 } },
+        {17+11, 0,  { 0x01, 0x01, 0x10, 0x01, 0x01, 0x01, 0x01, 0x01 },
+                    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },
+                    { 0xCE, 0x7A, 0x24, 0xF3, 0x50, 0xE2, 0x80, 0xB6 } },
+        {18+11, 0,  { 0x01, 0x01, 0x08, 0x01, 0x01, 0x01, 0x01, 0x01 },
+                    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },
+                    { 0x88, 0x2B, 0xFF, 0x0A, 0xA0, 0x1A, 0x0B, 0x87 } },
+        {19+11, 0,  { 0x01, 0x01, 0x04, 0x01, 0x01, 0x01, 0x01, 0x01 },
+                    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },
+                    { 0x25, 0x61, 0x02, 0x88, 0x92, 0x45, 0x11, 0xC2 } },
+        {20+11, 0,  { 0x01, 0x01, 0x02, 0x01, 0x01, 0x01, 0x01, 0x01 },
+                    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },
+                    { 0xC7, 0x15, 0x16, 0xC2, 0x9C, 0x75, 0xD1, 0x70 } },
+        {21+11, 0,  { 0x01, 0x01, 0x01, 0x80, 0x01, 0x01, 0x01, 0x01 },
+                    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },
+                    { 0x51, 0x99, 0xC2, 0x9A, 0x52, 0xC9, 0xF0, 0x59 } },
+        {22+11, 0,  { 0x01, 0x01, 0x01, 0x40, 0x01, 0x01, 0x01, 0x01 },
+                    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },
+                    { 0xC2, 0x2F, 0x0A, 0x29, 0x4A, 0x71, 0xF2, 0x9F } },
+        {23+11, 0,  { 0x01, 0x01, 0x01, 0x20, 0x01, 0x01, 0x01, 0x01 },
+                    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },
+                    { 0xEE, 0x37, 0x14, 0x83, 0x71, 0x4C, 0x02, 0xEA } },
+        {24+11, 0,  { 0x01, 0x01, 0x01, 0x10, 0x01, 0x01, 0x01, 0x01 },
+                    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },
+                    { 0xA8, 0x1F, 0xBD, 0x44, 0x8F, 0x9E, 0x52, 0x2F } },
+        {25+11, 0,  { 0x01, 0x01, 0x01, 0x08, 0x01, 0x01, 0x01, 0x01 },
+                    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },
+                    { 0x4F, 0x64, 0x4C, 0x92, 0xE1, 0x92, 0xDF, 0xED } },
+        {26+11, 0,  { 0x01, 0x01, 0x01, 0x04, 0x01, 0x01, 0x01, 0x01 },
+                    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },
+                    { 0x1A, 0xFA, 0x9A, 0x66, 0xA6, 0xDF, 0x92, 0xAE } },
+        {27+11, 0,  { 0x01, 0x01, 0x01, 0x02, 0x01, 0x01, 0x01, 0x01 },
+                    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },
+                    { 0xB3, 0xC1, 0xCC, 0x71, 0x5C, 0xB8, 0x79, 0xD8 } },
+        {28+11, 0,  { 0x01, 0x01, 0x01, 0x01, 0x80, 0x01, 0x01, 0x01 },
+                    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },
+                    { 0x19, 0xD0, 0x32, 0xE6, 0x4A, 0xB0, 0xBD, 0x8B } },
+        {29+11, 0,  { 0x01, 0x01, 0x01, 0x01, 0x40, 0x01, 0x01, 0x01 },
+                    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },
+                    { 0x3C, 0xFA, 0xA7, 0xA7, 0xDC, 0x87, 0x20, 0xDC } },
+        {30+11, 0,  { 0x01, 0x01, 0x01, 0x01, 0x20, 0x01, 0x01, 0x01 },
+                    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },
+                    { 0xB7, 0x26, 0x5F, 0x7F, 0x44, 0x7A, 0xC6, 0xF3 } },
+        {31+11, 0,  { 0x01, 0x01, 0x01, 0x01, 0x10, 0x01, 0x01, 0x01 },
+                    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },
+                    { 0x9D, 0xB7, 0x3B, 0x3C, 0x0D, 0x16, 0x3F, 0x54 } },
+        {32+11, 0,  { 0x01, 0x01, 0x01, 0x01, 0x08, 0x01, 0x01, 0x01 },
+                    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },
+                    { 0x81, 0x81, 0xB6, 0x5B, 0xAB, 0xF4, 0xA9, 0x75 } },
+        {33+11, 0,  { 0x01, 0x01, 0x01, 0x01, 0x04, 0x01, 0x01, 0x01 },
+                    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },
+                    { 0x93, 0xC9, 0xB6, 0x40, 0x42, 0xEA, 0xA2, 0x40 } },
+        {34+11, 0,  { 0x01, 0x01, 0x01, 0x01, 0x02, 0x01, 0x01, 0x01 },
+                    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },
+                    { 0x55, 0x70, 0x53, 0x08, 0x29, 0x70, 0x55, 0x92 } },
+        {35+11, 0,  { 0x01, 0x01, 0x01, 0x01, 0x01, 0x80, 0x01, 0x01 },
+                    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },
+                    { 0x86, 0x38, 0x80, 0x9E, 0x87, 0x87, 0x87, 0xA0 } },
+        {36+11, 0,  { 0x01, 0x01, 0x01, 0x01, 0x01, 0x40, 0x01, 0x01 },
+                    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },
+                    { 0x41, 0xB9, 0xA7, 0x9A, 0xF7, 0x9A, 0xC2, 0x08 } },
+        {37+11, 0,  { 0x01, 0x01, 0x01, 0x01, 0x01, 0x20, 0x01, 0x01 },
+                    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },
+                    { 0x7A, 0x9B, 0xE4, 0x2F, 0x20, 0x09, 0xA8, 0x92 } },
+        {38+11, 0,  { 0x01, 0x01, 0x01, 0x01, 0x01, 0x10, 0x01, 0x01 },
+                    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },
+                    { 0x29, 0x03, 0x8D, 0x56, 0xBA, 0x6D, 0x27, 0x45 } },
+        {39+11, 0,  { 0x01, 0x01, 0x01, 0x01, 0x01, 0x08, 0x01, 0x01 },
+                    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },
+                    { 0x54, 0x95, 0xC6, 0xAB, 0xF1, 0xE5, 0xDF, 0x51 } },
+        {40+11, 0,  { 0x01, 0x01, 0x01, 0x01, 0x01, 0x04, 0x01, 0x01 },
+                    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },
+                    { 0xAE, 0x13, 0xDB, 0xD5, 0x61, 0x48, 0x89, 0x33 } },
+        {41+11, 0,  { 0x01, 0x01, 0x01, 0x01, 0x01, 0x02, 0x01, 0x01 },
+                    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },
+                    { 0x02, 0x4D, 0x1F, 0xFA, 0x89, 0x04, 0xE3, 0x89 } },
+        {42+11, 0,  { 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x80, 0x01 },
+                    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },
+                    { 0xD1, 0x39, 0x97, 0x12, 0xF9, 0x9B, 0xF0, 0x2E } },
+        {43+11, 0,  { 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x40, 0x01 },
+                    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },
+                    { 0x14, 0xC1, 0xD7, 0xC1, 0xCF, 0xFE, 0xC7, 0x9E } },
+        {44+11, 0,  { 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x20, 0x01 },
+                    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },
+                    { 0x1D, 0xE5, 0x27, 0x9D, 0xAE, 0x3B, 0xED, 0x6F } },
+        {45+11, 0,  { 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x10, 0x01 },
+                    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },
+                    { 0xE9, 0x41, 0xA3, 0x3F, 0x85, 0x50, 0x13, 0x03 } },
+        {46+11, 0,  { 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x08, 0x01 },
+                    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },
+                    { 0xDA, 0x99, 0xDB, 0xBC, 0x9A, 0x03, 0xF3, 0x79 } },
+        {47+11, 0,  { 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x04, 0x01 },
+                    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },
+                    { 0xB7, 0xFC, 0x92, 0xF9, 0x1D, 0x8E, 0x92, 0xE9 } },
+        {48+11, 0,  { 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x02, 0x01 },
+                    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },
+                    { 0xAE, 0x8E, 0x5C, 0xAA, 0x3C, 0xA0, 0x4E, 0x85 } },
+        {49+11, 0,  { 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x80 },
+                    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },
+                    { 0x9C, 0xC6, 0x2D, 0xF4, 0x3B, 0x6E, 0xED, 0x74 } },
+        {50+11, 0,  { 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x40 },
+                    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },
+                    { 0xD8, 0x63, 0xDB, 0xB5, 0xC5, 0x9A, 0x91, 0xA0 } },
+        {51+11, 0,  { 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x20 },
+                    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },
+                    { 0xA1, 0xAB, 0x21, 0x90, 0x54, 0x5B, 0x91, 0xD7 } },
+        {52+11, 0,  { 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x10 },
+                    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },
+                    { 0x08, 0x75, 0x04, 0x1E, 0x64, 0xC5, 0x70, 0xF7 } },
+        {53+11, 0,  { 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x08 },
+                    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },
+                    { 0x5A, 0x59, 0x45, 0x28, 0xBE, 0xBE, 0xF1, 0xCC } },
+        {54+11, 0,  { 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x04 },
+                    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },
+                    { 0xFC, 0xDB, 0x32, 0x91, 0xDE, 0x21, 0xF0, 0xC0 } },
+        {55+11, 0,  { 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x02 },
+                    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },
+                    { 0x86, 0x9E, 0xFD, 0x7F, 0x9F, 0x26, 0x5A, 0x09 } },
+#endif /* LTC_TEST_EXT */
+
+        /*** more test cases you could add if you are not convinced (the above test cases aren't really too good):
+
+                key              plaintext        ciphertext
+                0000000000000000 0000000000000000 8CA64DE9C1B123A7
+                FFFFFFFFFFFFFFFF FFFFFFFFFFFFFFFF 7359B2163E4EDC58
+                3000000000000000 1000000000000001 958E6E627A05557B
+                1111111111111111 1111111111111111 F40379AB9E0EC533
+                0123456789ABCDEF 1111111111111111 17668DFC7292532D
+                1111111111111111 0123456789ABCDEF 8A5AE1F81AB8F2DD
+                0000000000000000 0000000000000000 8CA64DE9C1B123A7
+                FEDCBA9876543210 0123456789ABCDEF ED39D950FA74BCC4
+                7CA110454A1A6E57 01A1D6D039776742 690F5B0D9A26939B
+                0131D9619DC1376E 5CD54CA83DEF57DA 7A389D10354BD271
+                07A1133E4A0B2686 0248D43806F67172 868EBB51CAB4599A
+                3849674C2602319E 51454B582DDF440A 7178876E01F19B2A
+                04B915BA43FEB5B6 42FD443059577FA2 AF37FB421F8C4095
+                0113B970FD34F2CE 059B5E0851CF143A 86A560F10EC6D85B
+                0170F175468FB5E6 0756D8E0774761D2 0CD3DA020021DC09
+                43297FAD38E373FE 762514B829BF486A EA676B2CB7DB2B7A
+                07A7137045DA2A16 3BDD119049372802 DFD64A815CAF1A0F
+                04689104C2FD3B2F 26955F6835AF609A 5C513C9C4886C088
+                37D06BB516CB7546 164D5E404F275232 0A2AEEAE3FF4AB77
+                1F08260D1AC2465E 6B056E18759F5CCA EF1BF03E5DFA575A
+                584023641ABA6176 004BD6EF09176062 88BF0DB6D70DEE56
+                025816164629B007 480D39006EE762F2 A1F9915541020B56
+                49793EBC79B3258F 437540C8698F3CFA 6FBF1CAFCFFD0556
+                4FB05E1515AB73A7 072D43A077075292 2F22E49BAB7CA1AC
+                49E95D6D4CA229BF 02FE55778117F12A 5A6B612CC26CCE4A
+                018310DC409B26D6 1D9D5C5018F728C2 5F4C038ED12B2E41
+                1C587F1C13924FEF 305532286D6F295A 63FAC0D034D9F793
+                0101010101010101 0123456789ABCDEF 617B3A0CE8F07100
+                1F1F1F1F0E0E0E0E 0123456789ABCDEF DB958605F8C8C606
+                E0FEE0FEF1FEF1FE 0123456789ABCDEF EDBFD1C66C29CCC7
+                0000000000000000 FFFFFFFFFFFFFFFF 355550B2150E2451
+                FFFFFFFFFFFFFFFF 0000000000000000 CAAAAF4DEAF1DBAE
+                0123456789ABCDEF 0000000000000000 D5D44FF720683D0D
+                FEDCBA9876543210 FFFFFFFFFFFFFFFF 2A2BB008DF97C2F2
+
+            http://www.ecs.soton.ac.uk/~prw99r/ez438/vectors.txt
+        ***/
+    };
+    int i, y;
+    unsigned char tmp[8];
+    symmetric_key des;
+
+    for(i=0; i < (int)(sizeof(cases)/sizeof(cases[0])); i++)
+    {
+        if ((err = des_setup(cases[i].key, 8, 0, &des)) != CRYPT_OK) {
+           return err;
+        }
+        if (cases[i].mode != 0) { 
+           des_ecb_encrypt(cases[i].txt, tmp, &des);
+        } else {
+           des_ecb_decrypt(cases[i].txt, tmp, &des);
+        }
+
+        if (XMEMCMP(cases[i].out, tmp, sizeof(tmp)) != 0) {
+           return CRYPT_FAIL_TESTVECTOR;
+        }
+
+      /* now see if we can encrypt all zero bytes 1000 times, decrypt and come back where we started */
+      for (y = 0; y < 8; y++) tmp[y] = 0;
+      for (y = 0; y < 1000; y++) des_ecb_encrypt(tmp, tmp, &des);
+      for (y = 0; y < 1000; y++) des_ecb_decrypt(tmp, tmp, &des);
+      for (y = 0; y < 8; y++) if (tmp[y] != 0) return CRYPT_FAIL_TESTVECTOR;
+}
+
+    return CRYPT_OK;
+  #endif
+}
+
+int des3_test(void)
+{
+ #ifndef LTC_TEST
+    return CRYPT_NOP;
+ #else    
+   unsigned char key[24], pt[8], ct[8], tmp[8];
+   symmetric_key skey;
+   int x, err;
+
+   if ((err = des_test()) != CRYPT_OK) {
+      return err;
+   }
+
+   for (x = 0; x < 8; x++) {
+       pt[x] = x;
+   }
+   
+   for (x = 0; x < 24; x++) {
+       key[x] = x;
+   }
+
+   if ((err = des3_setup(key, 24, 0, &skey)) != CRYPT_OK) {
+      return err;
+   }
+   
+   des3_ecb_encrypt(pt, ct, &skey);
+   des3_ecb_decrypt(ct, tmp, &skey);
+   
+   if (XMEMCMP(pt, tmp, 8) != 0) {
+      return CRYPT_FAIL_TESTVECTOR;
+   }
+   
+   return CRYPT_OK;
+ #endif
+}
+
+/** Terminate the context 
+   @param skey    The scheduled key
+*/
+void des_done(symmetric_key *skey)
+{
+  LTC_UNUSED_PARAM(skey);
+}
+
+/** Terminate the context 
+   @param skey    The scheduled key
+*/
+void des3_done(symmetric_key *skey)
+{
+  LTC_UNUSED_PARAM(skey);
+}
+
+
+/**
+  Gets suitable key size
+  @param keysize [in/out] The length of the recommended key (in bytes).  This function will store the suitable size back in this variable.
+  @return CRYPT_OK if the input key size is acceptable.
+*/
+int des_keysize(int *keysize)
+{
+    LTC_ARGCHK(keysize != NULL);
+    if(*keysize < 8) {
+        return CRYPT_INVALID_KEYSIZE;
+    }
+    *keysize = 8;
+    return CRYPT_OK;
+}
+
+/**
+  Gets suitable key size
+  @param keysize [in/out] The length of the recommended key (in bytes).  This function will store the suitable size back in this variable.
+  @return CRYPT_OK if the input key size is acceptable.
+*/
+int des3_keysize(int *keysize)
+{
+    LTC_ARGCHK(keysize != NULL);
+    if(*keysize < 24) {
+        return CRYPT_INVALID_KEYSIZE;
+    }
+    *keysize = 24;
+    return CRYPT_OK;
+}
+
+#endif
+
+
+/* $Source: /cvs/libtom/libtomcrypt/src/ciphers/des.c,v $ */
+/* $Revision: 1.15 $ */
+/* $Date: 2007/05/12 14:20:27 $ */
diff --git a/core/lib/libtomcrypt/src/ciphers/sub.mk b/core/lib/libtomcrypt/src/ciphers/sub.mk
new file mode 100644
index 0000000..6f1f1aa
--- /dev/null
+++ b/core/lib/libtomcrypt/src/ciphers/sub.mk
@@ -0,0 +1,17 @@
+cflags-y += -Wno-unused-parameter
+
+ifeq ($(CFG_CRYPTO_AES_ARM64_CE),y)
+srcs-y += aes_armv8a_ce.c
+cflags-aes_armv8a_ce.c-y += -march=armv8-a+crypto
+srcs-y += aes_modes_armv8a_ce_a64.S
+aflags-aes_modes_armv8a_ce_a64.S-y += -DINTERLEAVE=4
+else
+ifeq ($(CFG_CRYPTO_AES_ARM32_CE),y)
+srcs-y += aes_armv8a_ce.c
+srcs-y += aes_modes_armv8a_ce_a32.S
+else
+srcs-$(CFG_CRYPTO_AES) += aes.c
+endif
+endif
+
+srcs-$(CFG_CRYPTO_DES) += des.c
diff --git a/core/lib/libtomcrypt/src/encauth/ccm/ccm_add_aad.c b/core/lib/libtomcrypt/src/encauth/ccm/ccm_add_aad.c
new file mode 100644
index 0000000..03130ea
--- /dev/null
+++ b/core/lib/libtomcrypt/src/encauth/ccm/ccm_add_aad.c
@@ -0,0 +1,89 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+#ifdef LTC_CCM_MODE
+
+/**
+  Add AAD to the CCM state
+  @param ccm       The CCM state
+  @param adata     The additional authentication data to add to the CCM state
+  @param adatalen  The length of the AAD data.
+  @return CRYPT_OK on success
+ */
+int ccm_add_aad(ccm_state *ccm,
+                const unsigned char *adata,  unsigned long adatalen)
+{
+   unsigned long y;
+   int            err;
+
+   LTC_ARGCHK(ccm   != NULL);
+   LTC_ARGCHK(adata != NULL);
+
+   if (ccm->aadlen < ccm->current_aadlen + adatalen) {
+      return CRYPT_INVALID_ARG;
+   }
+   ccm->current_aadlen += adatalen;
+
+   /* now add the data */
+   for (y = 0; y < adatalen; y++) {
+      if (ccm->x == 16) {
+         /* full block so let's encrypt it */
+         if ((err = cipher_descriptor[ccm->cipher]->ecb_encrypt(ccm->PAD, ccm->PAD, &ccm->K)) != CRYPT_OK) {
+            return CRYPT_ERROR;
+         }
+         ccm->x = 0;
+      }
+      ccm->PAD[ccm->x++] ^= adata[y];
+   }
+
+   /* remainder? */
+   if (ccm->aadlen == ccm->current_aadlen) {
+      if (ccm->x != 0) {
+         if ((err = cipher_descriptor[ccm->cipher]->ecb_encrypt(ccm->PAD, ccm->PAD, &ccm->K)) != CRYPT_OK) {
+            return CRYPT_ERROR;
+         }
+      }
+      ccm->x = 0;
+   }
+
+   return CRYPT_OK;
+}
+
+#endif
diff --git a/core/lib/libtomcrypt/src/encauth/ccm/ccm_add_nonce.c b/core/lib/libtomcrypt/src/encauth/ccm/ccm_add_nonce.c
new file mode 100644
index 0000000..9097670
--- /dev/null
+++ b/core/lib/libtomcrypt/src/encauth/ccm/ccm_add_nonce.c
@@ -0,0 +1,139 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+#ifdef LTC_CCM_MODE
+
+/**
+  Add nonce data to the CCM state
+  @param ccm       The CCM state
+  @param nonce     The nonce data to add
+  @param noncelen  The length of the nonce
+  @return CRYPT_OK on success
+ */
+int ccm_add_nonce(ccm_state *ccm,
+                  const unsigned char *nonce,     unsigned long noncelen)
+{
+   unsigned long x, y, len;
+   int           err;
+
+   LTC_ARGCHK(ccm   != NULL);
+   LTC_ARGCHK(nonce != NULL);
+
+   /* increase L to match the nonce len */
+   ccm->noncelen = (noncelen > 13) ? 13 : noncelen;
+   if ((15 - ccm->noncelen) > ccm->L) {
+      ccm->L = 15 - ccm->noncelen;
+   }
+
+   /* decrease noncelen to match L */
+   if ((ccm->noncelen + ccm->L) > 15) {
+      ccm->noncelen = 15 - ccm->L;
+   }
+
+   /* form B_0 == flags | Nonce N | l(m) */
+   x = 0;
+   ccm->PAD[x++] = (unsigned char)(((ccm->aadlen > 0) ? (1<<6) : 0) |
+		   (((ccm->taglen - 2)>>1)<<3)        |
+		   (ccm->L-1));
+
+   /* nonce */
+   for (y = 0; y < (16 - (ccm->L + 1)); y++) {
+      ccm->PAD[x++] = nonce[y];
+   }
+
+   /* store len */
+   len = ccm->ptlen;
+
+   /* shift len so the upper bytes of len are the contents of the length */
+   for (y = ccm->L; y < 4; y++) {
+      len <<= 8;
+   }
+
+   /* store l(m) (only store 32-bits) */
+   for (y = 0; ccm->L > 4 && (ccm->L-y)>4; y++) {
+      ccm->PAD[x++] = 0;
+   }
+   for (; y < ccm->L; y++) {
+      ccm->PAD[x++] = (unsigned char)((len >> 24) & 255);
+      len <<= 8;
+   }
+
+   /* encrypt PAD */
+   if ((err = cipher_descriptor[ccm->cipher]->ecb_encrypt(ccm->PAD, ccm->PAD, &ccm->K)) != CRYPT_OK) {
+      return err;
+   }
+
+   /* handle header */
+   ccm->x = 0;
+   if (ccm->aadlen > 0) {
+      /* store length */
+      if (ccm->aadlen < ((1UL<<16) - (1UL<<8))) {
+         ccm->PAD[ccm->x++] ^= (ccm->aadlen>>8) & 255;
+         ccm->PAD[ccm->x++] ^= ccm->aadlen & 255;
+      } else {
+         ccm->PAD[ccm->x++] ^= 0xFF;
+         ccm->PAD[ccm->x++] ^= 0xFE;
+         ccm->PAD[ccm->x++] ^= (ccm->aadlen>>24) & 255;
+         ccm->PAD[ccm->x++] ^= (ccm->aadlen>>16) & 255;
+         ccm->PAD[ccm->x++] ^= (ccm->aadlen>>8) & 255;
+         ccm->PAD[ccm->x++] ^= ccm->aadlen & 255;
+      }
+   }
+
+   /* setup the ctr counter */
+   x = 0;
+
+   /* flags */
+   ccm->ctr[x++] = (unsigned char)ccm->L-1;
+
+   /* nonce */
+   for (y = 0; y < (16 - (ccm->L+1)); ++y) {
+      ccm->ctr[x++] = nonce[y];
+   }
+   /* offset */
+   while (x < 16) {
+      ccm->ctr[x++] = 0;
+   }
+
+   ccm->CTRlen = 16;
+   return CRYPT_OK;
+}
+
+#endif
diff --git a/core/lib/libtomcrypt/src/encauth/ccm/ccm_done.c b/core/lib/libtomcrypt/src/encauth/ccm/ccm_done.c
new file mode 100644
index 0000000..7a4e211
--- /dev/null
+++ b/core/lib/libtomcrypt/src/encauth/ccm/ccm_done.c
@@ -0,0 +1,91 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+#ifdef LTC_CCM_MODE
+
+/**
+  Terminate a CCM stream
+  @param ccm     The CCM state
+  @param tag     [out] The destination for the MAC tag
+  @param taglen  [in/out]  The length of the MAC tag
+  @return CRYPT_OK on success
+ */
+int ccm_done(ccm_state *ccm,
+             unsigned char *tag,    unsigned long *taglen)
+{
+   unsigned long x, y;
+   int            err;
+
+   LTC_ARGCHK(ccm != NULL);
+
+   /* Check all data have been processed */
+   if (ccm->ptlen != ccm->current_ptlen) {
+      return CRYPT_ERROR;
+   }
+
+   LTC_ARGCHK(tag    != NULL);
+   LTC_ARGCHK(taglen != NULL);
+
+   if (ccm->x != 0) {
+      if ((err = cipher_descriptor[ccm->cipher]->ecb_encrypt(ccm->PAD, ccm->PAD, &ccm->K)) != CRYPT_OK) {
+         return err;
+      }
+   }
+
+   /* setup CTR for the TAG (zero the count) */
+   for (y = 15; y > 15 - ccm->L; y--) {
+      ccm->ctr[y] = 0x00;
+   }
+   if ((err = cipher_descriptor[ccm->cipher]->ecb_encrypt(ccm->ctr, ccm->CTRPAD, &ccm->K)) != CRYPT_OK) {
+      return err;
+   }
+
+   cipher_descriptor[ccm->cipher]->done(&ccm->K);
+
+   /* store the TAG */
+   for (x = 0; x < 16 && x < *taglen; x++) {
+      tag[x] = ccm->PAD[x] ^ ccm->CTRPAD[x];
+   }
+   *taglen = x;
+
+   return CRYPT_OK;
+}
+
+#endif
diff --git a/core/lib/libtomcrypt/src/encauth/ccm/ccm_init.c b/core/lib/libtomcrypt/src/encauth/ccm/ccm_init.c
new file mode 100644
index 0000000..04a38c2
--- /dev/null
+++ b/core/lib/libtomcrypt/src/encauth/ccm/ccm_init.c
@@ -0,0 +1,107 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+#ifdef LTC_CCM_MODE
+
+/**
+  Initialize a CCM state
+  @param ccm     The CCM state to initialize
+  @param cipher  The index of the cipher to use
+  @param key     The secret key
+  @param keylen  The length of the secret key
+  @param ptlen   The length of the plain/cipher text that will be processed
+  @param taglen  The max length of the MAC tag
+  @param aadlen  The length of the AAD
+
+  @return CRYPT_OK on success
+ */
+int ccm_init(ccm_state *ccm, int cipher,
+             const unsigned char *key, int keylen, int ptlen, int taglen, int aadlen)
+{
+   int            err;
+
+   LTC_ARGCHK(ccm    != NULL);
+   LTC_ARGCHK(key    != NULL);
+   LTC_ARGCHK(taglen != 0);
+
+   XMEMSET(ccm, 0, sizeof(ccm_state));
+
+   /* check cipher input */
+   if ((err = cipher_is_valid(cipher)) != CRYPT_OK) {
+      return err;
+   }
+   if (cipher_descriptor[cipher]->block_length != 16) {
+      return CRYPT_INVALID_CIPHER;
+   }
+
+   /* make sure the taglen is even and <= 16 */
+   ccm->taglen = taglen;
+   ccm->taglen &= ~1;
+   if (ccm->taglen > 16) {
+      ccm->taglen = 16;
+   }
+
+   /* can't use < 4 */
+   if (ccm->taglen < 4) {
+      return CRYPT_INVALID_ARG;
+   }
+
+   /* schedule key */
+   if ((err = cipher_descriptor[cipher]->setup(key, keylen, 0, &ccm->K)) != CRYPT_OK) {
+      return err;
+   }
+   ccm->cipher = cipher;
+
+   /* let's get the L value */
+   ccm->ptlen = ptlen;
+   ccm->L   = 0;
+   while (ptlen) {
+      ++ccm->L;
+      ptlen >>= 8;
+   }
+   if (ccm->L <= 1) {
+      ccm->L = 2;
+   }
+
+   ccm->aadlen = aadlen;
+   return CRYPT_OK;
+}
+
+#endif
diff --git a/core/lib/libtomcrypt/src/encauth/ccm/ccm_memory.c b/core/lib/libtomcrypt/src/encauth/ccm/ccm_memory.c
new file mode 100644
index 0000000..d68988a
--- /dev/null
+++ b/core/lib/libtomcrypt/src/encauth/ccm/ccm_memory.c
@@ -0,0 +1,432 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file ccm_memory.c
+  CCM support, process a block of memory, Tom St Denis
+*/
+
+#ifdef LTC_CCM_MODE
+
+/**
+   CCM encrypt/decrypt and produce an authentication tag
+
+     *1 'pt', 'ct' and 'tag' can both be 'in' or 'out', depending on 'direction'
+
+   @param cipher     The index of the cipher desired
+   @param key        The secret key to use
+   @param keylen     The length of the secret key (octets)
+   @param uskey      A previously scheduled key [optional can be NULL]
+   @param nonce      The session nonce [use once]
+   @param noncelen   The length of the nonce
+   @param header     The header for the session
+   @param headerlen  The length of the header (octets)
+   @param pt         [*1] The plaintext
+   @param ptlen      The length of the plaintext (octets)
+   @param ct         [*1] The ciphertext
+   @param tag        [*1] The destination tag
+   @param taglen     The max size and resulting size of the authentication tag
+   @param direction  Encrypt or Decrypt direction (0 or 1)
+   @return CRYPT_OK if successful
+*/
+int ccm_memory(int cipher,
+    const unsigned char *key,    unsigned long keylen,
+    symmetric_key       *uskey,
+    const unsigned char *nonce,  unsigned long noncelen,
+    const unsigned char *header, unsigned long headerlen,
+          unsigned char *pt,     unsigned long ptlen,
+          unsigned char *ct,
+          unsigned char *tag,    unsigned long *taglen,
+                    int  direction)
+{
+   unsigned char  PAD[16], ctr[16], CTRPAD[16], ptTag[16], b, *pt_real;
+   unsigned char *pt_work = NULL;
+   symmetric_key *skey;
+   int            err;
+   unsigned long  len, L, x, y, z, CTRlen;
+#ifdef LTC_FAST
+   LTC_FAST_TYPE fastMask = -1; /* initialize fastMask at all zeroes */
+#endif
+   unsigned char mask = 0xff; /* initialize mask at all zeroes */
+
+   if (uskey == NULL) {
+      LTC_ARGCHK(key    != NULL);
+   }
+   LTC_ARGCHK(nonce  != NULL);
+   if (headerlen > 0) {
+      LTC_ARGCHK(header != NULL);
+   }
+   LTC_ARGCHK(pt     != NULL);
+   LTC_ARGCHK(ct     != NULL);
+   LTC_ARGCHK(tag    != NULL);
+   LTC_ARGCHK(taglen != NULL);
+
+   pt_real = pt;
+
+#ifdef LTC_FAST
+   if (16 % sizeof(LTC_FAST_TYPE)) {
+      return CRYPT_INVALID_ARG;
+   }
+#endif
+
+   /* check cipher input */
+   if ((err = cipher_is_valid(cipher)) != CRYPT_OK) {
+      return err;
+   }
+   if (cipher_descriptor[cipher].block_length != 16) {
+      return CRYPT_INVALID_CIPHER;
+   }
+
+   /* make sure the taglen is even and <= 16 */
+   *taglen &= ~1;
+   if (*taglen > 16) {
+      *taglen = 16;
+   }
+
+   /* can't use < 4 */
+   if (*taglen < 4) {
+      return CRYPT_INVALID_ARG;
+   }
+
+   /* is there an accelerator? */
+   if (cipher_descriptor[cipher].accel_ccm_memory != NULL) {
+       return cipher_descriptor[cipher].accel_ccm_memory(
+           key,    keylen,
+           uskey,
+           nonce,  noncelen,
+           header, headerlen,
+           pt,     ptlen,
+           ct,
+           tag,    taglen,
+           direction);
+   }
+
+   /* let's get the L value */
+   len = ptlen;
+   L   = 0;
+   while (len) {
+      ++L;
+      len >>= 8;
+   }
+   if (L <= 1) {
+      L = 2;
+   }
+
+   /* increase L to match the nonce len */
+   noncelen = (noncelen > 13) ? 13 : noncelen;
+   if ((15 - noncelen) > L) {
+      L = 15 - noncelen;
+   }
+
+   /* allocate mem for the symmetric key */
+   if (uskey == NULL) {
+      skey = XMALLOC(sizeof(*skey));
+      if (skey == NULL) {
+         return CRYPT_MEM;
+      }
+
+      /* initialize the cipher */
+      if ((err = cipher_descriptor[cipher].setup(key, keylen, 0, skey)) != CRYPT_OK) {
+         XFREE(skey);
+         return err;
+      }
+   } else {
+      skey = uskey;
+   }
+   
+   /* initialize buffer for pt */
+   if (direction == CCM_DECRYPT) {
+      pt_work = XMALLOC(ptlen);
+      if (pt_work == NULL) {
+         goto error;
+      }
+      pt = pt_work;
+   }
+
+   /* form B_0 == flags | Nonce N | l(m) */
+   x = 0;
+   PAD[x++] = (unsigned char)(((headerlen > 0) ? (1<<6) : 0) |
+            (((*taglen - 2)>>1)<<3)        |
+            (L-1));
+
+   /* nonce */
+   for (y = 0; y < (16 - (L + 1)); y++) {
+       PAD[x++] = nonce[y];
+   }
+
+   /* store len */
+   len = ptlen;
+
+   /* shift len so the upper bytes of len are the contents of the length */
+   for (y = L; y < 4; y++) {
+       len <<= 8;
+   }
+
+   /* store l(m) (only store 32-bits) */
+   for (y = 0; L > 4 && (L-y)>4; y++) {
+       PAD[x++] = 0;
+   }
+   for (; y < L; y++) {
+       PAD[x++] = (unsigned char)((len >> 24) & 255);
+       len <<= 8;
+   }
+
+   /* encrypt PAD */
+   if ((err = cipher_descriptor[cipher].ecb_encrypt(PAD, PAD, skey)) != CRYPT_OK) {
+       goto error;
+   }
+
+   /* handle header */
+   if (headerlen > 0) {
+      x = 0;
+
+      /* store length */
+      if (headerlen < ((1UL<<16) - (1UL<<8))) {
+         PAD[x++] ^= (headerlen>>8) & 255;
+         PAD[x++] ^= headerlen & 255;
+      } else {
+         PAD[x++] ^= 0xFF;
+         PAD[x++] ^= 0xFE;
+         PAD[x++] ^= (headerlen>>24) & 255;
+         PAD[x++] ^= (headerlen>>16) & 255;
+         PAD[x++] ^= (headerlen>>8) & 255;
+         PAD[x++] ^= headerlen & 255;
+      }
+
+      /* now add the data */
+      for (y = 0; y < headerlen; y++) {
+          if (x == 16) {
+             /* full block so let's encrypt it */
+             if ((err = cipher_descriptor[cipher].ecb_encrypt(PAD, PAD, skey)) != CRYPT_OK) {
+                goto error;
+             }
+             x = 0;
+          }
+          PAD[x++] ^= header[y];
+      }
+
+      /* remainder */
+      if ((err = cipher_descriptor[cipher].ecb_encrypt(PAD, PAD, skey)) != CRYPT_OK) {
+         goto error;
+      }
+   }
+
+   /* setup the ctr counter */
+   x = 0;
+
+   /* flags */
+   ctr[x++] = (unsigned char)L-1;
+
+   /* nonce */
+   for (y = 0; y < (16 - (L+1)); ++y) {
+      ctr[x++] = nonce[y];
+   }
+   /* offset */
+   while (x < 16) {
+      ctr[x++] = 0;
+   }
+
+   x      = 0;
+   CTRlen = 16;
+
+   /* now handle the PT */
+   if (ptlen > 0) {
+      y = 0;
+#ifdef LTC_FAST
+      if (ptlen & ~15)  {
+          if (direction == CCM_ENCRYPT) {
+             for (; y < (ptlen & ~15); y += 16) {
+                /* increment the ctr? */
+                for (z = 15; z > 15-L; z--) {
+                    ctr[z] = (ctr[z] + 1) & 255;
+                    if (ctr[z]) break;
+                }
+                if ((err = cipher_descriptor[cipher].ecb_encrypt(ctr, CTRPAD, skey)) != CRYPT_OK) {
+                   goto error;
+                }
+
+                /* xor the PT against the pad first */
+                for (z = 0; z < 16; z += sizeof(LTC_FAST_TYPE)) {
+                    *((LTC_FAST_TYPE*)(&PAD[z]))  ^= *((LTC_FAST_TYPE*)(&pt[y+z]));
+                    *((LTC_FAST_TYPE*)(&ct[y+z])) = *((LTC_FAST_TYPE*)(&pt[y+z])) ^ *((LTC_FAST_TYPE*)(&CTRPAD[z]));
+                }
+                if ((err = cipher_descriptor[cipher].ecb_encrypt(PAD, PAD, skey)) != CRYPT_OK) {
+                   goto error;
+                }
+             }
+          } else { /* direction == CCM_DECRYPT */
+             for (; y < (ptlen & ~15); y += 16) {
+                /* increment the ctr? */
+                for (z = 15; z > 15-L; z--) {
+                    ctr[z] = (ctr[z] + 1) & 255;
+                    if (ctr[z]) break;
+                }
+                if ((err = cipher_descriptor[cipher].ecb_encrypt(ctr, CTRPAD, skey)) != CRYPT_OK) {
+                   goto error;
+                }
+
+                /* xor the PT against the pad last */
+                for (z = 0; z < 16; z += sizeof(LTC_FAST_TYPE)) {
+                    *((LTC_FAST_TYPE*)(&pt[y+z])) = *((LTC_FAST_TYPE*)(&ct[y+z])) ^ *((LTC_FAST_TYPE*)(&CTRPAD[z]));
+                    *((LTC_FAST_TYPE*)(&PAD[z]))  ^= *((LTC_FAST_TYPE*)(&pt[y+z]));
+                }
+                if ((err = cipher_descriptor[cipher].ecb_encrypt(PAD, PAD, skey)) != CRYPT_OK) {
+                   goto error;
+                }
+             }
+         }
+     }
+#endif
+
+      for (; y < ptlen; y++) {
+          /* increment the ctr? */
+          if (CTRlen == 16) {
+             for (z = 15; z > 15-L; z--) {
+                 ctr[z] = (ctr[z] + 1) & 255;
+                 if (ctr[z]) break;
+             }
+             if ((err = cipher_descriptor[cipher].ecb_encrypt(ctr, CTRPAD, skey)) != CRYPT_OK) {
+                goto error;
+             }
+             CTRlen = 0;
+          }
+
+          /* if we encrypt we add the bytes to the MAC first */
+          if (direction == CCM_ENCRYPT) {
+             b     = pt[y];
+             ct[y] = b ^ CTRPAD[CTRlen++];
+          } else {
+             b     = ct[y] ^ CTRPAD[CTRlen++];
+             pt[y] = b;
+          }
+
+          if (x == 16) {
+             if ((err = cipher_descriptor[cipher].ecb_encrypt(PAD, PAD, skey)) != CRYPT_OK) {
+                goto error;
+             }
+             x = 0;
+          }
+          PAD[x++] ^= b;
+      }
+
+      if (x != 0) {
+         if ((err = cipher_descriptor[cipher].ecb_encrypt(PAD, PAD, skey)) != CRYPT_OK) {
+            goto error;
+         }
+      }
+   }
+
+   /* setup CTR for the TAG (zero the count) */
+   for (y = 15; y > 15 - L; y--) {
+      ctr[y] = 0x00;
+   }
+   if ((err = cipher_descriptor[cipher].ecb_encrypt(ctr, CTRPAD, skey)) != CRYPT_OK) {
+      goto error;
+   }
+
+   if (skey != uskey) {
+      cipher_descriptor[cipher].done(skey);
+   }
+
+   if (direction == CCM_ENCRYPT) {
+      /* store the TAG */
+      for (x = 0; x < 16 && x < *taglen; x++) {
+          tag[x] = PAD[x] ^ CTRPAD[x];
+      }
+      *taglen = x;
+   } else { /* direction == CCM_DECRYPT */
+      /* decrypt the tag */
+      for (x = 0; x < 16 && x < *taglen; x++) {
+         ptTag[x] = tag[x] ^ CTRPAD[x];
+      }
+      *taglen = x;
+
+      /* check validity of the decrypted tag against the computed PAD (in constant time) */
+      /* HACK: the boolean value of XMEM_NEQ becomes either 0 (CRYPT_OK) or 1 (CRYPT_ERR).
+       *       there should be a better way of setting the correct error code in constant
+       *       time.
+       */
+      err = XMEM_NEQ(ptTag, PAD, *taglen);
+
+      /* Zero the plaintext if the tag was invalid (in constant time) */
+      if (ptlen > 0) {
+         y = 0;
+         mask *= 1 - err; /* mask = ( err ? 0 : 0xff ) */
+#ifdef LTC_FAST
+         fastMask *= 1 - err;
+         if (ptlen & ~15) {
+            for (; y < (ptlen & ~15); y += 16) {
+              for (z = 0; z < 16; z += sizeof(LTC_FAST_TYPE)) {
+                *((LTC_FAST_TYPE*)(&pt_real[y+z])) = *((LTC_FAST_TYPE*)(&pt[y+z])) & fastMask;
+              }
+            }
+         }
+#endif
+         for (; y < ptlen; y++) {
+            pt_real[y] = pt[y] & mask;
+         }
+      }
+   }
+
+#ifdef LTC_CLEAN_STACK
+   fastMask = 0;
+   mask = 0;
+   zeromem(skey,   sizeof(*skey));
+   zeromem(PAD,    sizeof(PAD));
+   zeromem(CTRPAD, sizeof(CTRPAD));
+   if (pt_work != NULL) {
+     zeromem(pt_work, ptlen);
+   }
+#endif
+error:
+   if (pt_work) {
+      XFREE(pt_work);
+   }
+   if (skey != uskey) {
+      XFREE(skey);
+   }
+
+   return err;
+}
+
+#endif
+
+/* $Source$ */
+/* $Revision$ */
+/* $Date$ */
diff --git a/core/lib/libtomcrypt/src/encauth/ccm/ccm_process.c b/core/lib/libtomcrypt/src/encauth/ccm/ccm_process.c
new file mode 100644
index 0000000..772f5a4
--- /dev/null
+++ b/core/lib/libtomcrypt/src/encauth/ccm/ccm_process.c
@@ -0,0 +1,114 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+#ifdef LTC_CCM_MODE
+
+/**
+  Process plaintext/ciphertext through CCM
+  @param ccm       The CCM state
+  @param pt        The plaintext
+  @param ptlen     The plaintext length (ciphertext length is the same)
+  @param ct        The ciphertext
+  @param direction Encrypt or Decrypt mode (CCM_ENCRYPT or CCM_DECRYPT)
+  @return CRYPT_OK on success
+ */
+int ccm_process(ccm_state *ccm,
+                unsigned char *pt,     unsigned long ptlen,
+                unsigned char *ct,
+                int direction)
+{
+   unsigned char  y, z, b;
+   int err;
+
+   LTC_ARGCHK(ccm != NULL);
+
+   /* Check aad has been correctly added */
+   if (ccm->aadlen != ccm->current_aadlen) {
+      return CRYPT_ERROR;
+   }
+
+   /* Check we do not process too much data */
+   if (ccm->ptlen < ccm->current_ptlen + ptlen) {
+      return CRYPT_ERROR;
+   }
+   ccm->current_ptlen += ptlen;
+
+   /* now handle the PT */
+   if (ptlen > 0) {
+      LTC_ARGCHK(pt != NULL);
+      LTC_ARGCHK(ct != NULL);
+      y = 0;
+
+      for (; y < ptlen; y++) {
+         /* increment the ctr? */
+         if (ccm->CTRlen == 16) {
+            for (z = 15; z > 15-ccm->L; z--) {
+               ccm->ctr[z] = (ccm->ctr[z] + 1) & 255;
+               if (ccm->ctr[z]) break;
+            }
+            if ((err = cipher_descriptor[ccm->cipher]->ecb_encrypt(ccm->ctr, ccm->CTRPAD, &ccm->K)) != CRYPT_OK) {
+               return err;
+            }
+            ccm->CTRlen = 0;
+         }
+
+         /* if we encrypt we add the bytes to the MAC first */
+         if (direction == CCM_ENCRYPT) {
+            b     = pt[y];
+            ct[y] = b ^ ccm->CTRPAD[ccm->CTRlen++];
+         } else {
+            b     = ct[y] ^ ccm->CTRPAD[ccm->CTRlen++];
+            pt[y] = b;
+         }
+
+         if (ccm->x == 16) {
+            if ((err = cipher_descriptor[ccm->cipher]->ecb_encrypt(ccm->PAD, ccm->PAD, &ccm->K)) != CRYPT_OK) {
+               return err;
+            }
+            ccm->x = 0;
+         }
+         ccm->PAD[ccm->x++] ^= b;
+      }
+   }
+
+   return CRYPT_OK;
+}
+
+#endif
diff --git a/core/lib/libtomcrypt/src/encauth/ccm/ccm_reset.c b/core/lib/libtomcrypt/src/encauth/ccm/ccm_reset.c
new file mode 100644
index 0000000..308cd3d
--- /dev/null
+++ b/core/lib/libtomcrypt/src/encauth/ccm/ccm_reset.c
@@ -0,0 +1,61 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+#ifdef LTC_CCM_MODE
+
+/**
+  Reset a CCM state to as if you just called ccm_init().  This saves the initialization time.
+  @param ccm   The CCM state to reset
+  @return CRYPT_OK on success
+*/
+int ccm_reset(ccm_state *ccm)
+{
+   LTC_ARGCHK(ccm != NULL);
+   zeromem(ccm->PAD, sizeof(ccm->PAD));
+   zeromem(ccm->ctr, sizeof(ccm->ctr));
+   zeromem(ccm->CTRPAD, sizeof(ccm->CTRPAD));
+   ccm->CTRlen = 0;
+   ccm->current_ptlen = 0;
+   ccm->current_aadlen = 0;
+
+   return CRYPT_OK;
+}
+
+#endif
diff --git a/core/lib/libtomcrypt/src/encauth/ccm/sub.mk b/core/lib/libtomcrypt/src/encauth/ccm/sub.mk
new file mode 100644
index 0000000..d767248
--- /dev/null
+++ b/core/lib/libtomcrypt/src/encauth/ccm/sub.mk
@@ -0,0 +1,8 @@
+srcs-y += ccm_init.c
+srcs-y += ccm_add_nonce.c
+srcs-y += ccm_add_aad.c
+srcs-y += ccm_process.c
+srcs-y += ccm_done.c
+srcs-y += ccm_reset.c
+# srcs-y += ccm_memory.c
+# srcs-y += ccm_test.c
diff --git a/core/lib/libtomcrypt/src/encauth/eax/eax_addheader.c b/core/lib/libtomcrypt/src/encauth/eax/eax_addheader.c
new file mode 100644
index 0000000..0e68844
--- /dev/null
+++ b/core/lib/libtomcrypt/src/encauth/eax/eax_addheader.c
@@ -0,0 +1,65 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+/** 
+    @file eax_addheader.c
+    EAX implementation, add meta-data, by Tom St Denis 
+*/
+#include "tomcrypt.h"
+
+#ifdef LTC_EAX_MODE
+
+/** 
+    add header (metadata) to the stream 
+    @param eax    The current EAX state
+    @param header The header (meta-data) data you wish to add to the state
+    @param length The length of the header data
+    @return CRYPT_OK if successful
+*/
+int eax_addheader(eax_state *eax, const unsigned char *header, 
+                  unsigned long length)
+{
+   LTC_ARGCHK(eax    != NULL);
+   LTC_ARGCHK(header != NULL);
+   return omac_process(&eax->headeromac, header, length);
+}
+
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/encauth/eax/eax_addheader.c,v $ */
+/* $Revision: 1.6 $ */
+/* $Date: 2007/05/12 14:32:35 $ */
diff --git a/core/lib/libtomcrypt/src/encauth/eax/eax_decrypt.c b/core/lib/libtomcrypt/src/encauth/eax/eax_decrypt.c
new file mode 100644
index 0000000..ffe0d25
--- /dev/null
+++ b/core/lib/libtomcrypt/src/encauth/eax/eax_decrypt.c
@@ -0,0 +1,77 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+
+/** 
+    @file eax_decrypt.c
+    EAX implementation, decrypt block, by Tom St Denis
+*/
+#include "tomcrypt.h"
+
+#ifdef LTC_EAX_MODE
+
+/**  
+   Decrypt data with the EAX protocol
+   @param eax     The EAX state
+   @param ct      The ciphertext
+   @param pt      [out] The plaintext
+   @param length  The length (octets) of the ciphertext
+   @return CRYPT_OK if successful
+*/
+int eax_decrypt(eax_state *eax, const unsigned char *ct, unsigned char *pt, 
+                unsigned long length)
+{
+   int err;
+   
+   LTC_ARGCHK(eax != NULL);
+   LTC_ARGCHK(pt  != NULL);
+   LTC_ARGCHK(ct  != NULL);
+
+   /* omac ciphertext */
+   if ((err = omac_process(&eax->ctomac, ct, length)) != CRYPT_OK) {
+      return err;
+   }
+
+   /* decrypt  */
+   return ctr_decrypt(ct, pt, length, &eax->ctr);
+}
+
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/encauth/eax/eax_decrypt.c,v $ */
+/* $Revision: 1.6 $ */
+/* $Date: 2007/05/12 14:32:35 $ */
diff --git a/core/lib/libtomcrypt/src/encauth/eax/eax_decrypt_verify_memory.c b/core/lib/libtomcrypt/src/encauth/eax/eax_decrypt_verify_memory.c
new file mode 100644
index 0000000..30a81c8
--- /dev/null
+++ b/core/lib/libtomcrypt/src/encauth/eax/eax_decrypt_verify_memory.c
@@ -0,0 +1,135 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+
+/**
+    @file eax_decrypt_verify_memory.c
+    EAX implementation, decrypt block of memory, by Tom St Denis
+*/
+#include "tomcrypt.h"
+
+#ifdef LTC_EAX_MODE
+
+/**
+   Decrypt a block of memory and verify the provided MAC tag with EAX
+   @param cipher     The index of the cipher desired
+   @param key        The secret key
+   @param keylen     The length of the key (octets)
+   @param nonce      The nonce data (use once) for the session
+   @param noncelen   The length of the nonce data.
+   @param header     The session header data
+   @param headerlen  The length of the header (octets)
+   @param ct         The ciphertext
+   @param ctlen      The length of the ciphertext (octets)
+   @param pt         [out] The plaintext
+   @param tag        The authentication tag provided by the encoder
+   @param taglen     [in/out] The length of the tag (octets)
+   @param stat       [out] The result of the decryption (1==valid tag, 0==invalid)
+   @return CRYPT_OK if successful regardless of the resulting tag comparison
+*/
+int eax_decrypt_verify_memory(int cipher,
+    const unsigned char *key,    unsigned long keylen,
+    const unsigned char *nonce,  unsigned long noncelen,
+    const unsigned char *header, unsigned long headerlen,
+    const unsigned char *ct,     unsigned long ctlen,
+          unsigned char *pt,
+          unsigned char *tag,    unsigned long taglen,
+          int           *stat)
+{
+   int            err;
+   eax_state     *eax;
+   unsigned char *buf;
+   unsigned long  buflen;
+
+   LTC_ARGCHK(stat != NULL);
+   LTC_ARGCHK(key  != NULL);
+   LTC_ARGCHK(pt   != NULL);
+   LTC_ARGCHK(ct   != NULL);
+   LTC_ARGCHK(tag  != NULL);
+
+   /* default to zero */
+   *stat = 0;
+
+   /* allocate ram */
+   buf = XMALLOC(taglen);
+   eax = XMALLOC(sizeof(*eax));
+   if (eax == NULL || buf == NULL) {
+      if (eax != NULL) {
+         XFREE(eax);
+      }
+      if (buf != NULL) {
+         XFREE(buf);
+      }
+      return CRYPT_MEM;
+   }
+
+   if ((err = eax_init(eax, cipher, key, keylen, nonce, noncelen, header, headerlen)) != CRYPT_OK) {
+      goto LBL_ERR;
+   }
+
+   if ((err = eax_decrypt(eax, ct, pt, ctlen)) != CRYPT_OK) {
+      goto LBL_ERR;
+   }
+ 
+   buflen = taglen;
+   if ((err = eax_done(eax, buf, &buflen)) != CRYPT_OK) {
+      goto LBL_ERR;
+   }
+
+   /* compare tags */
+   if (buflen >= taglen && XMEMCMP(buf, tag, taglen) == 0) {
+      *stat = 1;
+   }
+   
+   err = CRYPT_OK;
+LBL_ERR:
+#ifdef LTC_CLEAN_STACK
+   zeromem(buf, taglen);
+   zeromem(eax, sizeof(*eax));
+#endif
+
+   XFREE(eax);
+   XFREE(buf);
+
+   return err;
+}
+
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/encauth/eax/eax_decrypt_verify_memory.c,v $ */
+/* $Revision: 1.7 $ */
+/* $Date: 2007/05/12 14:32:35 $ */
diff --git a/core/lib/libtomcrypt/src/encauth/eax/eax_done.c b/core/lib/libtomcrypt/src/encauth/eax/eax_done.c
new file mode 100644
index 0000000..63297b0
--- /dev/null
+++ b/core/lib/libtomcrypt/src/encauth/eax/eax_done.c
@@ -0,0 +1,121 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+
+/**
+   @file eax_done.c
+   EAX implementation, terminate session, by Tom St Denis
+*/
+#include "tomcrypt.h"
+
+#ifdef LTC_EAX_MODE
+
+/**
+   Terminate an EAX session and get the tag.
+   @param eax       The EAX state
+   @param tag       [out] The destination of the authentication tag
+   @param taglen    [in/out] The max length and resulting length of the authentication tag
+   @return CRYPT_OK if successful
+*/
+int eax_done(eax_state *eax, unsigned char *tag, unsigned long *taglen)
+{
+   int           err;
+   unsigned char *headermac, *ctmac;
+   unsigned long x, len;
+
+   LTC_ARGCHK(eax    != NULL);
+   LTC_ARGCHK(tag    != NULL);
+   LTC_ARGCHK(taglen != NULL);
+
+   /* allocate ram */
+   headermac = XMALLOC(MAXBLOCKSIZE);
+   ctmac     = XMALLOC(MAXBLOCKSIZE);
+
+   if (headermac == NULL || ctmac == NULL) {
+      if (headermac != NULL) {
+         XFREE(headermac);
+      }
+      if (ctmac != NULL) {
+         XFREE(ctmac);
+      }
+      return CRYPT_MEM;
+   }
+
+   /* finish ctomac */
+   len = MAXBLOCKSIZE;
+   if ((err = omac_done(&eax->ctomac, ctmac, &len)) != CRYPT_OK) {
+      goto LBL_ERR; 
+   }
+
+   /* finish headeromac */
+
+   /* note we specifically don't reset len so the two lens are minimal */
+
+   if ((err = omac_done(&eax->headeromac, headermac, &len)) != CRYPT_OK) {
+      goto LBL_ERR; 
+   }
+
+   /* terminate the CTR chain */
+   if ((err = ctr_done(&eax->ctr)) != CRYPT_OK) {
+      goto LBL_ERR;
+   }
+
+   /* compute N xor H xor C */
+   for (x = 0; x < len && x < *taglen; x++) {
+       tag[x] = eax->N[x] ^ headermac[x] ^ ctmac[x];
+   }
+   *taglen = x;
+
+   err = CRYPT_OK;
+LBL_ERR:
+#ifdef LTC_CLEAN_STACK
+   zeromem(ctmac,     MAXBLOCKSIZE);
+   zeromem(headermac, MAXBLOCKSIZE);
+   zeromem(eax,       sizeof(*eax));
+#endif
+
+   XFREE(ctmac);
+   XFREE(headermac);
+
+   return err;
+}
+
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/encauth/eax/eax_done.c,v $ */
+/* $Revision: 1.7 $ */
+/* $Date: 2007/05/12 14:32:35 $ */
diff --git a/core/lib/libtomcrypt/src/encauth/eax/eax_encrypt.c b/core/lib/libtomcrypt/src/encauth/eax/eax_encrypt.c
new file mode 100644
index 0000000..07572dc
--- /dev/null
+++ b/core/lib/libtomcrypt/src/encauth/eax/eax_encrypt.c
@@ -0,0 +1,78 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+
+/**
+   @file eax_encrypt.c
+   EAX implementation, encrypt block by Tom St Denis 
+*/
+#include "tomcrypt.h"
+
+#ifdef LTC_EAX_MODE
+
+/**
+   Encrypt with EAX a block of data.
+   @param eax        The EAX state
+   @param pt         The plaintext to encrypt
+   @param ct         [out] The ciphertext as encrypted
+   @param length     The length of the plaintext (octets)
+   @return CRYPT_OK if successful
+*/
+int eax_encrypt(eax_state *eax, const unsigned char *pt, unsigned char *ct, 
+                unsigned long length)
+{
+   int err;
+   
+   LTC_ARGCHK(eax != NULL);
+   LTC_ARGCHK(pt  != NULL);
+   LTC_ARGCHK(ct  != NULL);
+
+   /* encrypt */
+   if ((err = ctr_encrypt(pt, ct, length, &eax->ctr)) != CRYPT_OK) {
+      return err;
+   }
+
+   /* omac ciphertext */
+   return omac_process(&eax->ctomac, ct, length);
+}
+
+#endif
+
+
+/* $Source: /cvs/libtom/libtomcrypt/src/encauth/eax/eax_encrypt.c,v $ */
+/* $Revision: 1.6 $ */
+/* $Date: 2007/05/12 14:32:35 $ */
diff --git a/core/lib/libtomcrypt/src/encauth/eax/eax_encrypt_authenticate_memory.c b/core/lib/libtomcrypt/src/encauth/eax/eax_encrypt_authenticate_memory.c
new file mode 100644
index 0000000..5b40386
--- /dev/null
+++ b/core/lib/libtomcrypt/src/encauth/eax/eax_encrypt_authenticate_memory.c
@@ -0,0 +1,109 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+
+/**
+  @file eax_encrypt_authenticate_memory.c
+  EAX implementation, encrypt a block of memory, by Tom St Denis
+*/
+#include "tomcrypt.h"
+
+#ifdef LTC_EAX_MODE
+
+/**
+   EAX encrypt and produce an authentication tag
+   @param cipher     The index of the cipher desired
+   @param key        The secret key to use
+   @param keylen     The length of the secret key (octets)
+   @param nonce      The session nonce [use once]
+   @param noncelen   The length of the nonce
+   @param header     The header for the session
+   @param headerlen  The length of the header (octets)
+   @param pt         The plaintext
+   @param ptlen      The length of the plaintext (octets)
+   @param ct         [out] The ciphertext
+   @param tag        [out] The destination tag
+   @param taglen     [in/out] The max size and resulting size of the authentication tag
+   @return CRYPT_OK if successful
+*/
+int eax_encrypt_authenticate_memory(int cipher,
+    const unsigned char *key,    unsigned long keylen,
+    const unsigned char *nonce,  unsigned long noncelen,
+    const unsigned char *header, unsigned long headerlen,
+    const unsigned char *pt,     unsigned long ptlen,
+          unsigned char *ct,
+          unsigned char *tag,    unsigned long *taglen)
+{
+   int err;
+   eax_state *eax;
+
+   LTC_ARGCHK(key    != NULL);
+   LTC_ARGCHK(pt     != NULL);
+   LTC_ARGCHK(ct     != NULL);
+   LTC_ARGCHK(tag    != NULL);
+   LTC_ARGCHK(taglen != NULL);
+
+   eax = XMALLOC(sizeof(*eax));
+
+   if ((err = eax_init(eax, cipher, key, keylen, nonce, noncelen, header, headerlen)) != CRYPT_OK) {
+      goto LBL_ERR; 
+   }
+
+   if ((err = eax_encrypt(eax, pt, ct, ptlen)) != CRYPT_OK) {
+      goto LBL_ERR; 
+   }
+ 
+   if ((err = eax_done(eax, tag, taglen)) != CRYPT_OK) {
+      goto LBL_ERR; 
+   }
+
+   err = CRYPT_OK;
+LBL_ERR:
+#ifdef LTC_CLEAN_STACK
+   zeromem(eax, sizeof(*eax));
+#endif
+
+   XFREE(eax);
+
+   return err;   
+}
+
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/encauth/eax/eax_encrypt_authenticate_memory.c,v $ */
+/* $Revision: 1.6 $ */
+/* $Date: 2007/05/12 14:32:35 $ */
diff --git a/core/lib/libtomcrypt/src/encauth/eax/eax_init.c b/core/lib/libtomcrypt/src/encauth/eax/eax_init.c
new file mode 100644
index 0000000..1ae30d9
--- /dev/null
+++ b/core/lib/libtomcrypt/src/encauth/eax/eax_init.c
@@ -0,0 +1,171 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+
+/** 
+   @file eax_init.c
+   EAX implementation, initialized EAX state, by Tom St Denis 
+*/
+#include "tomcrypt.h"
+
+#ifdef LTC_EAX_MODE
+
+/** 
+   Initialized an EAX state
+   @param eax       [out] The EAX state to initialize
+   @param cipher    The index of the desired cipher
+   @param key       The secret key
+   @param keylen    The length of the secret key (octets)
+   @param nonce     The use-once nonce for the session
+   @param noncelen  The length of the nonce (octets)
+   @param header    The header for the EAX state
+   @param headerlen The header length (octets)
+   @return CRYPT_OK if successful
+*/
+int eax_init(eax_state *eax, int cipher, 
+             const unsigned char *key,    unsigned long keylen,
+             const unsigned char *nonce,  unsigned long noncelen,
+             const unsigned char *header, unsigned long headerlen)
+{
+   unsigned char *buf;
+   int           err, blklen;
+   omac_state    *omac;
+   unsigned long len;
+
+
+   LTC_ARGCHK(eax   != NULL);
+   LTC_ARGCHK(key   != NULL);
+   LTC_ARGCHK(nonce != NULL);
+   if (headerlen > 0) {
+      LTC_ARGCHK(header != NULL);
+   }
+
+   if ((err = cipher_is_valid(cipher)) != CRYPT_OK) {
+      return err;
+   }
+   blklen = cipher_descriptor[cipher].block_length;
+
+   /* allocate ram */
+   buf  = XMALLOC(MAXBLOCKSIZE);
+   omac = XMALLOC(sizeof(*omac));
+
+   if (buf == NULL || omac == NULL) {
+      if (buf != NULL) {
+         XFREE(buf);
+      }
+      if (omac != NULL) {
+         XFREE(omac);
+      }
+      return CRYPT_MEM;
+   }
+
+   /* N = LTC_OMAC_0K(nonce) */
+   zeromem(buf, MAXBLOCKSIZE);
+   if ((err = omac_init(omac, cipher, key, keylen)) != CRYPT_OK) {
+      goto LBL_ERR; 
+   }
+
+   /* omac the [0]_n */
+   if ((err = omac_process(omac, buf, blklen)) != CRYPT_OK) {
+      goto LBL_ERR; 
+   }
+   /* omac the nonce */
+   if ((err = omac_process(omac, nonce, noncelen)) != CRYPT_OK) {
+      goto LBL_ERR; 
+   }
+   /* store result */
+   len = sizeof(eax->N);
+   if ((err = omac_done(omac, eax->N, &len)) != CRYPT_OK) {
+      goto LBL_ERR; 
+   }
+
+   /* H = LTC_OMAC_1K(header) */
+   zeromem(buf, MAXBLOCKSIZE);
+   buf[blklen - 1] = 1;
+
+   if ((err = omac_init(&eax->headeromac, cipher, key, keylen)) != CRYPT_OK) {
+      goto LBL_ERR; 
+   }
+
+   /* omac the [1]_n */
+   if ((err = omac_process(&eax->headeromac, buf, blklen)) != CRYPT_OK) {
+      goto LBL_ERR; 
+   }
+   /* omac the header */
+   if (headerlen != 0) {
+      if ((err = omac_process(&eax->headeromac, header, headerlen)) != CRYPT_OK) {
+          goto LBL_ERR; 
+      }
+   }
+
+   /* note we don't finish the headeromac, this allows us to add more header later */
+
+   /* setup the CTR mode */
+   if ((err = ctr_start(cipher, eax->N, key, keylen, 0, CTR_COUNTER_BIG_ENDIAN, &eax->ctr)) != CRYPT_OK) {
+      goto LBL_ERR; 
+   }
+
+   /* setup the LTC_OMAC for the ciphertext */
+   if ((err = omac_init(&eax->ctomac, cipher, key, keylen)) != CRYPT_OK) { 
+      goto LBL_ERR; 
+   }
+
+   /* omac [2]_n */
+   zeromem(buf, MAXBLOCKSIZE);
+   buf[blklen-1] = 2;
+   if ((err = omac_process(&eax->ctomac, buf, blklen)) != CRYPT_OK) {
+      goto LBL_ERR; 
+   }
+
+   err = CRYPT_OK;
+LBL_ERR:
+#ifdef LTC_CLEAN_STACK
+   zeromem(buf,  MAXBLOCKSIZE);
+   zeromem(omac, sizeof(*omac));
+#endif
+
+   XFREE(omac);
+   XFREE(buf);
+
+   return err;
+}
+
+#endif 
+
+/* $Source: /cvs/libtom/libtomcrypt/src/encauth/eax/eax_init.c,v $ */
+/* $Revision: 1.8 $ */
+/* $Date: 2007/05/12 14:37:41 $ */
diff --git a/core/lib/libtomcrypt/src/encauth/gcm/gcm_add_aad.c b/core/lib/libtomcrypt/src/encauth/gcm/gcm_add_aad.c
new file mode 100644
index 0000000..b8e6285
--- /dev/null
+++ b/core/lib/libtomcrypt/src/encauth/gcm/gcm_add_aad.c
@@ -0,0 +1,151 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+
+/**
+   @file gcm_add_aad.c
+   GCM implementation, Add AAD data to the stream, by Tom St Denis
+*/
+#include "tomcrypt.h"
+
+#ifdef LTC_GCM_MODE
+
+/**
+  Add AAD to the GCM state
+  @param gcm       The GCM state
+  @param adata     The additional authentication data to add to the GCM state
+  @param adatalen  The length of the AAD data.
+  @return CRYPT_OK on success
+ */
+int gcm_add_aad(gcm_state *gcm,
+               const unsigned char *adata,  unsigned long adatalen)
+{
+   unsigned long x;
+   int           err;
+#ifdef LTC_FAST
+   unsigned long y;
+#endif
+
+   LTC_ARGCHK(gcm    != NULL);
+   if (adatalen > 0) {
+      LTC_ARGCHK(adata  != NULL);
+   }
+
+   if (gcm->buflen > 16 || gcm->buflen < 0) {
+      return CRYPT_INVALID_ARG;
+   }
+
+   if ((err = cipher_is_valid(gcm->cipher)) != CRYPT_OK) {
+      return err;
+   }
+
+   /* in IV mode? */
+   if (gcm->mode == LTC_GCM_MODE_IV) {
+      /* let's process the IV */
+      if (gcm->ivmode || gcm->buflen != 12) {
+         for (x = 0; x < (unsigned long)gcm->buflen; x++) {
+             gcm->X[x] ^= gcm->buf[x];
+         }
+         if (gcm->buflen) {
+            gcm->totlen += gcm->buflen * CONST64(8);
+            gcm_mult_h(gcm, gcm->X);
+         }
+
+         /* mix in the length */
+         zeromem(gcm->buf, 8);
+         STORE64H(gcm->totlen, gcm->buf+8);
+         for (x = 0; x < 16; x++) {
+             gcm->X[x] ^= gcm->buf[x];
+         }
+         gcm_mult_h(gcm, gcm->X);
+
+         /* copy counter out */ 
+         XMEMCPY(gcm->Y, gcm->X, 16);
+         zeromem(gcm->X, 16);
+      } else {
+         XMEMCPY(gcm->Y, gcm->buf, 12);
+         gcm->Y[12] = 0;
+         gcm->Y[13] = 0;
+         gcm->Y[14] = 0;
+         gcm->Y[15] = 1;
+      }
+      XMEMCPY(gcm->Y_0, gcm->Y, 16);
+      zeromem(gcm->buf, 16);
+      gcm->buflen = 0;
+      gcm->totlen = 0;
+      gcm->mode   = LTC_GCM_MODE_AAD;
+   }
+
+   if (gcm->mode != LTC_GCM_MODE_AAD || gcm->buflen >= 16) {
+      return CRYPT_INVALID_ARG;
+   }
+
+   x = 0;
+#ifdef LTC_FAST
+   if (gcm->buflen == 0) {
+      for (x = 0; x < (adatalen & ~15); x += 16) {
+          for (y = 0; y < 16; y += sizeof(LTC_FAST_TYPE)) {
+              *((LTC_FAST_TYPE*)(&gcm->X[y])) ^= *((LTC_FAST_TYPE*)(&adata[x + y]));
+          }
+          gcm_mult_h(gcm, gcm->X);
+          gcm->totlen += 128;
+      }
+      adata += x;
+   }
+#endif
+
+
+   /* start adding AAD data to the state */
+   for (; x < adatalen; x++) {
+       gcm->X[gcm->buflen++] ^= *adata++;
+
+       if (gcm->buflen == 16) {
+         /* GF mult it */
+         gcm_mult_h(gcm, gcm->X);
+         gcm->buflen = 0;
+         gcm->totlen += 128;
+      }
+   }
+
+   return CRYPT_OK;
+}
+#endif
+   
+
+/* $Source: /cvs/libtom/libtomcrypt/src/encauth/gcm/gcm_add_aad.c,v $ */
+/* $Revision: 1.18 $ */
+/* $Date: 2007/05/12 14:32:35 $ */
diff --git a/core/lib/libtomcrypt/src/encauth/gcm/gcm_add_iv.c b/core/lib/libtomcrypt/src/encauth/gcm/gcm_add_iv.c
new file mode 100644
index 0000000..1f0b897
--- /dev/null
+++ b/core/lib/libtomcrypt/src/encauth/gcm/gcm_add_iv.c
@@ -0,0 +1,121 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+
+/**
+   @file gcm_add_iv.c
+   GCM implementation, add IV data to the state, by Tom St Denis
+*/
+#include "tomcrypt.h"
+
+#ifdef LTC_GCM_MODE
+
+/**
+  Add IV data to the GCM state
+  @param gcm    The GCM state
+  @param IV     The initial value data to add
+  @param IVlen  The length of the IV
+  @return CRYPT_OK on success
+ */
+int gcm_add_iv(gcm_state *gcm, 
+               const unsigned char *IV,     unsigned long IVlen)
+{
+   unsigned long x, y;
+   int           err;
+
+   LTC_ARGCHK(gcm != NULL);
+   if (IVlen > 0) {
+      LTC_ARGCHK(IV  != NULL);
+   }
+
+   /* must be in IV mode */
+   if (gcm->mode != LTC_GCM_MODE_IV) {
+      return CRYPT_INVALID_ARG;
+   }
+ 
+   if (gcm->buflen >= 16 || gcm->buflen < 0) {
+      return CRYPT_INVALID_ARG;
+   }
+
+   if ((err = cipher_is_valid(gcm->cipher)) != CRYPT_OK) {
+      return err;
+   }
+
+
+   /* trip the ivmode flag */
+   if (IVlen + gcm->buflen > 12) {
+      gcm->ivmode |= 1;
+   }
+
+   x = 0;
+#ifdef LTC_FAST
+   if (gcm->buflen == 0) {
+      for (x = 0; x < (IVlen & ~15); x += 16) {
+          for (y = 0; y < 16; y += sizeof(LTC_FAST_TYPE)) {
+              *((LTC_FAST_TYPE*)(&gcm->X[y])) ^= *((LTC_FAST_TYPE*)(&IV[x + y]));
+          }
+          gcm_mult_h(gcm, gcm->X);
+          gcm->totlen += 128;
+      }
+      IV += x;
+   }
+#endif
+
+   /* start adding IV data to the state */
+   for (; x < IVlen; x++) {
+       gcm->buf[gcm->buflen++] = *IV++;
+
+       if (gcm->buflen == 16) {
+         /* GF mult it */
+         for (y = 0; y < 16; y++) {
+             gcm->X[y] ^= gcm->buf[y];
+         }
+         gcm_mult_h(gcm, gcm->X);
+         gcm->buflen = 0;
+         gcm->totlen += 128;
+      }
+   }
+
+   return CRYPT_OK;
+}
+
+#endif
+   
+
+/* $Source: /cvs/libtom/libtomcrypt/src/encauth/gcm/gcm_add_iv.c,v $ */
+/* $Revision: 1.9 $ */
+/* $Date: 2007/05/12 14:32:35 $ */
diff --git a/core/lib/libtomcrypt/src/encauth/gcm/gcm_done.c b/core/lib/libtomcrypt/src/encauth/gcm/gcm_done.c
new file mode 100644
index 0000000..28d0909
--- /dev/null
+++ b/core/lib/libtomcrypt/src/encauth/gcm/gcm_done.c
@@ -0,0 +1,110 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+
+/**
+   @file gcm_done.c
+   GCM implementation, Terminate the stream, by Tom St Denis
+*/
+#include "tomcrypt.h"
+
+#ifdef LTC_GCM_MODE
+
+/**
+  Terminate a GCM stream
+  @param gcm     The GCM state
+  @param tag     [out] The destination for the MAC tag
+  @param taglen  [in/out]  The length of the MAC tag
+  @return CRYPT_OK on success
+ */
+int gcm_done(gcm_state *gcm, 
+                     unsigned char *tag,    unsigned long *taglen)
+{
+   unsigned long x;
+   int err;
+
+   LTC_ARGCHK(gcm     != NULL);
+   LTC_ARGCHK(tag     != NULL);
+   LTC_ARGCHK(taglen  != NULL);
+
+   if (gcm->buflen > 16 || gcm->buflen < 0) {
+      return CRYPT_INVALID_ARG;
+   }
+
+   if ((err = cipher_is_valid(gcm->cipher)) != CRYPT_OK) {
+      return err;
+   }
+
+
+   if (gcm->mode != LTC_GCM_MODE_TEXT) {
+      return CRYPT_INVALID_ARG;
+   }
+
+   /* handle remaining ciphertext */
+   if (gcm->buflen) {
+      gcm->pttotlen += gcm->buflen * CONST64(8);
+      gcm_mult_h(gcm, gcm->X);
+   }
+
+   /* length */
+   STORE64H(gcm->totlen, gcm->buf);
+   STORE64H(gcm->pttotlen, gcm->buf+8);
+   for (x = 0; x < 16; x++) {
+       gcm->X[x] ^= gcm->buf[x];
+   }
+   gcm_mult_h(gcm, gcm->X);
+
+   /* encrypt original counter */
+   if ((err = cipher_descriptor[gcm->cipher]->ecb_encrypt(gcm->Y_0, gcm->buf, &gcm->K)) != CRYPT_OK) {
+      return err;
+   }
+   for (x = 0; x < 16 && x < *taglen; x++) {
+       tag[x] = gcm->buf[x] ^ gcm->X[x];
+   }
+   *taglen = x;
+
+   cipher_descriptor[gcm->cipher]->done(&gcm->K);
+
+   return CRYPT_OK;
+}
+
+#endif
+
+
+/* $Source: /cvs/libtom/libtomcrypt/src/encauth/gcm/gcm_done.c,v $ */
+/* $Revision: 1.11 $ */
+/* $Date: 2007/05/12 14:32:35 $ */
diff --git a/core/lib/libtomcrypt/src/encauth/gcm/gcm_gf_mult.c b/core/lib/libtomcrypt/src/encauth/gcm/gcm_gf_mult.c
new file mode 100644
index 0000000..00f0860
--- /dev/null
+++ b/core/lib/libtomcrypt/src/encauth/gcm/gcm_gf_mult.c
@@ -0,0 +1,248 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+
+/**
+   @file gcm_gf_mult.c
+   GCM implementation, do the GF mult, by Tom St Denis
+*/
+#include "tomcrypt.h"
+
+#if defined(LTC_GCM_TABLES) || defined(LTC_LRW_TABLES) || ((defined(LTC_GCM_MODE) || defined(LTC_GCM_MODE)) && defined(LTC_FAST))
+
+/* this is x*2^128 mod p(x) ... the results are 16 bytes each stored in a packed format.  Since only the 
+ * lower 16 bits are not zero'ed I removed the upper 14 bytes */
+const unsigned char gcm_shift_table[256*2] = {
+0x00, 0x00, 0x01, 0xc2, 0x03, 0x84, 0x02, 0x46, 0x07, 0x08, 0x06, 0xca, 0x04, 0x8c, 0x05, 0x4e,
+0x0e, 0x10, 0x0f, 0xd2, 0x0d, 0x94, 0x0c, 0x56, 0x09, 0x18, 0x08, 0xda, 0x0a, 0x9c, 0x0b, 0x5e,
+0x1c, 0x20, 0x1d, 0xe2, 0x1f, 0xa4, 0x1e, 0x66, 0x1b, 0x28, 0x1a, 0xea, 0x18, 0xac, 0x19, 0x6e,
+0x12, 0x30, 0x13, 0xf2, 0x11, 0xb4, 0x10, 0x76, 0x15, 0x38, 0x14, 0xfa, 0x16, 0xbc, 0x17, 0x7e,
+0x38, 0x40, 0x39, 0x82, 0x3b, 0xc4, 0x3a, 0x06, 0x3f, 0x48, 0x3e, 0x8a, 0x3c, 0xcc, 0x3d, 0x0e,
+0x36, 0x50, 0x37, 0x92, 0x35, 0xd4, 0x34, 0x16, 0x31, 0x58, 0x30, 0x9a, 0x32, 0xdc, 0x33, 0x1e,
+0x24, 0x60, 0x25, 0xa2, 0x27, 0xe4, 0x26, 0x26, 0x23, 0x68, 0x22, 0xaa, 0x20, 0xec, 0x21, 0x2e,
+0x2a, 0x70, 0x2b, 0xb2, 0x29, 0xf4, 0x28, 0x36, 0x2d, 0x78, 0x2c, 0xba, 0x2e, 0xfc, 0x2f, 0x3e,
+0x70, 0x80, 0x71, 0x42, 0x73, 0x04, 0x72, 0xc6, 0x77, 0x88, 0x76, 0x4a, 0x74, 0x0c, 0x75, 0xce,
+0x7e, 0x90, 0x7f, 0x52, 0x7d, 0x14, 0x7c, 0xd6, 0x79, 0x98, 0x78, 0x5a, 0x7a, 0x1c, 0x7b, 0xde,
+0x6c, 0xa0, 0x6d, 0x62, 0x6f, 0x24, 0x6e, 0xe6, 0x6b, 0xa8, 0x6a, 0x6a, 0x68, 0x2c, 0x69, 0xee,
+0x62, 0xb0, 0x63, 0x72, 0x61, 0x34, 0x60, 0xf6, 0x65, 0xb8, 0x64, 0x7a, 0x66, 0x3c, 0x67, 0xfe,
+0x48, 0xc0, 0x49, 0x02, 0x4b, 0x44, 0x4a, 0x86, 0x4f, 0xc8, 0x4e, 0x0a, 0x4c, 0x4c, 0x4d, 0x8e,
+0x46, 0xd0, 0x47, 0x12, 0x45, 0x54, 0x44, 0x96, 0x41, 0xd8, 0x40, 0x1a, 0x42, 0x5c, 0x43, 0x9e,
+0x54, 0xe0, 0x55, 0x22, 0x57, 0x64, 0x56, 0xa6, 0x53, 0xe8, 0x52, 0x2a, 0x50, 0x6c, 0x51, 0xae,
+0x5a, 0xf0, 0x5b, 0x32, 0x59, 0x74, 0x58, 0xb6, 0x5d, 0xf8, 0x5c, 0x3a, 0x5e, 0x7c, 0x5f, 0xbe,
+0xe1, 0x00, 0xe0, 0xc2, 0xe2, 0x84, 0xe3, 0x46, 0xe6, 0x08, 0xe7, 0xca, 0xe5, 0x8c, 0xe4, 0x4e,
+0xef, 0x10, 0xee, 0xd2, 0xec, 0x94, 0xed, 0x56, 0xe8, 0x18, 0xe9, 0xda, 0xeb, 0x9c, 0xea, 0x5e,
+0xfd, 0x20, 0xfc, 0xe2, 0xfe, 0xa4, 0xff, 0x66, 0xfa, 0x28, 0xfb, 0xea, 0xf9, 0xac, 0xf8, 0x6e,
+0xf3, 0x30, 0xf2, 0xf2, 0xf0, 0xb4, 0xf1, 0x76, 0xf4, 0x38, 0xf5, 0xfa, 0xf7, 0xbc, 0xf6, 0x7e,
+0xd9, 0x40, 0xd8, 0x82, 0xda, 0xc4, 0xdb, 0x06, 0xde, 0x48, 0xdf, 0x8a, 0xdd, 0xcc, 0xdc, 0x0e,
+0xd7, 0x50, 0xd6, 0x92, 0xd4, 0xd4, 0xd5, 0x16, 0xd0, 0x58, 0xd1, 0x9a, 0xd3, 0xdc, 0xd2, 0x1e,
+0xc5, 0x60, 0xc4, 0xa2, 0xc6, 0xe4, 0xc7, 0x26, 0xc2, 0x68, 0xc3, 0xaa, 0xc1, 0xec, 0xc0, 0x2e,
+0xcb, 0x70, 0xca, 0xb2, 0xc8, 0xf4, 0xc9, 0x36, 0xcc, 0x78, 0xcd, 0xba, 0xcf, 0xfc, 0xce, 0x3e,
+0x91, 0x80, 0x90, 0x42, 0x92, 0x04, 0x93, 0xc6, 0x96, 0x88, 0x97, 0x4a, 0x95, 0x0c, 0x94, 0xce,
+0x9f, 0x90, 0x9e, 0x52, 0x9c, 0x14, 0x9d, 0xd6, 0x98, 0x98, 0x99, 0x5a, 0x9b, 0x1c, 0x9a, 0xde,
+0x8d, 0xa0, 0x8c, 0x62, 0x8e, 0x24, 0x8f, 0xe6, 0x8a, 0xa8, 0x8b, 0x6a, 0x89, 0x2c, 0x88, 0xee,
+0x83, 0xb0, 0x82, 0x72, 0x80, 0x34, 0x81, 0xf6, 0x84, 0xb8, 0x85, 0x7a, 0x87, 0x3c, 0x86, 0xfe,
+0xa9, 0xc0, 0xa8, 0x02, 0xaa, 0x44, 0xab, 0x86, 0xae, 0xc8, 0xaf, 0x0a, 0xad, 0x4c, 0xac, 0x8e,
+0xa7, 0xd0, 0xa6, 0x12, 0xa4, 0x54, 0xa5, 0x96, 0xa0, 0xd8, 0xa1, 0x1a, 0xa3, 0x5c, 0xa2, 0x9e,
+0xb5, 0xe0, 0xb4, 0x22, 0xb6, 0x64, 0xb7, 0xa6, 0xb2, 0xe8, 0xb3, 0x2a, 0xb1, 0x6c, 0xb0, 0xae,
+0xbb, 0xf0, 0xba, 0x32, 0xb8, 0x74, 0xb9, 0xb6, 0xbc, 0xf8, 0xbd, 0x3a, 0xbf, 0x7c, 0xbe, 0xbe };
+
+#endif
+
+
+#if defined(LTC_GCM_MODE) || defined(LRW_MODE)
+
+#ifndef LTC_FAST
+/* right shift */
+static void gcm_rightshift(unsigned char *a)
+{
+   int x;
+   for (x = 15; x > 0; x--) {
+       a[x] = (a[x]>>1) | ((a[x-1]<<7)&0x80);
+   }
+   a[0] >>= 1;
+}
+
+/* c = b*a */
+static const unsigned char mask[] = { 0x80, 0x40, 0x20, 0x10, 0x08, 0x04, 0x02, 0x01 };
+static const unsigned char poly[] = { 0x00, 0xE1 };
+
+     
+/**
+  GCM GF multiplier (internal use only)  bitserial
+  @param a   First value
+  @param b   Second value
+  @param c   Destination for a * b
+ */  
+void gcm_gf_mult(const unsigned char *a, const unsigned char *b, unsigned char *c)
+{
+   unsigned char Z[16], V[16];
+   unsigned char x, y, z;
+
+   zeromem(Z, 16);
+   XMEMCPY(V, a, 16);
+   for (x = 0; x < 128; x++) {
+       if (b[x>>3] & mask[x&7]) {
+          for (y = 0; y < 16; y++) {
+              Z[y] ^= V[y]; 
+          }
+       }
+       z     = V[15] & 0x01;
+       gcm_rightshift(V);
+       V[0] ^= poly[z];
+   }
+   XMEMCPY(c, Z, 16);
+}
+
+#else
+
+/* map normal numbers to "ieee" way ... e.g. bit reversed */
+#define M(x) ( ((x&8)>>3) | ((x&4)>>1) | ((x&2)<<1) | ((x&1)<<3) )
+
+#define BPD (sizeof(LTC_FAST_TYPE) * 8)
+#define WPV (1 + (16 / sizeof(LTC_FAST_TYPE)))
+
+/**
+  GCM GF multiplier (internal use only)  word oriented
+  @param a   First value
+  @param b   Second value
+  @param c   Destination for a * b
+ */  
+void gcm_gf_mult(const unsigned char *a, const unsigned char *b, unsigned char *c)
+{
+   int i, j, k, u;
+   LTC_FAST_TYPE B[16][WPV], tmp[32 / sizeof(LTC_FAST_TYPE)], pB[16 / sizeof(LTC_FAST_TYPE)], zz, z;
+   unsigned char pTmp[32];
+
+   /* create simple tables */
+   zeromem(B[0],       sizeof(B[0]));
+   zeromem(B[M(1)],    sizeof(B[M(1)]));
+
+#ifdef ENDIAN_32BITWORD
+   for (i = 0; i < 4; i++) {
+       LOAD32H(B[M(1)][i], a + (i<<2));
+       LOAD32L(pB[i],      b + (i<<2));
+   }
+#else 
+   for (i = 0; i < 2; i++) {
+       LOAD64H(B[M(1)][i], a + (i<<3));
+       LOAD64L(pB[i],      b + (i<<3));
+   }
+#endif
+
+   /* now create 2, 4 and 8 */
+   B[M(2)][0] = B[M(1)][0] >> 1;
+   B[M(4)][0] = B[M(1)][0] >> 2;
+   B[M(8)][0] = B[M(1)][0] >> 3;
+   for (i = 1; i < (int)WPV; i++) {
+      B[M(2)][i] = (B[M(1)][i-1] << (BPD-1)) | (B[M(1)][i] >> 1);
+      B[M(4)][i] = (B[M(1)][i-1] << (BPD-2)) | (B[M(1)][i] >> 2);
+      B[M(8)][i] = (B[M(1)][i-1] << (BPD-3)) | (B[M(1)][i] >> 3);
+   }
+
+   /*  now all values with two bits which are 3, 5, 6, 9, 10, 12 */
+   for (i = 0; i < (int)WPV; i++) {
+      B[M(3)][i]  = B[M(1)][i] ^ B[M(2)][i];
+      B[M(5)][i]  = B[M(1)][i] ^ B[M(4)][i];
+      B[M(6)][i]  = B[M(2)][i] ^ B[M(4)][i];
+      B[M(9)][i]  = B[M(1)][i] ^ B[M(8)][i];
+      B[M(10)][i] = B[M(2)][i] ^ B[M(8)][i];
+      B[M(12)][i] = B[M(8)][i] ^ B[M(4)][i];
+   
+   /*  now all 3 bit values and the only 4 bit value: 7, 11, 13, 14, 15 */
+      B[M(7)][i]  = B[M(3)][i] ^ B[M(4)][i];
+      B[M(11)][i] = B[M(3)][i] ^ B[M(8)][i];
+      B[M(13)][i] = B[M(1)][i] ^ B[M(12)][i];
+      B[M(14)][i] = B[M(6)][i] ^ B[M(8)][i];
+      B[M(15)][i] = B[M(7)][i] ^ B[M(8)][i];
+   }
+
+   zeromem(tmp, sizeof(tmp));
+
+   /* compute product four bits of each word at a time */
+   /* for each nibble */
+   for (i = (BPD/4)-1; i >= 0; i--) {
+       /* for each word */
+       for (j = 0; j < (int)(WPV-1); j++) {
+        /* grab the 4 bits recall the nibbles are backwards so it's a shift by (i^1)*4 */
+           u = (pB[j] >> ((i^1)<<2)) & 15;
+
+        /* add offset by the word count the table looked up value to the result */
+           for (k = 0; k < (int)WPV; k++) {
+               tmp[k+j] ^= B[u][k];
+           }
+       }
+     /* shift result up by 4 bits */
+       if (i != 0) {
+          for (z = j = 0; j < (int)(32 / sizeof(LTC_FAST_TYPE)); j++) {
+              zz = tmp[j] << (BPD-4);
+              tmp[j] = (tmp[j] >> 4) | z;
+              z = zz;
+          }
+       }
+   }
+
+   /* store product */
+#ifdef ENDIAN_32BITWORD
+   for (i = 0; i < 8; i++) {
+       STORE32H(tmp[i], pTmp + (i<<2));
+   }
+#else 
+   for (i = 0; i < 4; i++) {
+       STORE64H(tmp[i], pTmp + (i<<3));
+   }
+#endif
+
+   /* reduce by taking most significant byte and adding the appropriate two byte sequence 16 bytes down */
+   for (i = 31; i >= 16; i--) {
+       pTmp[i-16] ^= gcm_shift_table[((unsigned)pTmp[i]<<1)];
+       pTmp[i-15] ^= gcm_shift_table[((unsigned)pTmp[i]<<1)+1];
+   }
+
+   for (i = 0; i < 16; i++) {
+       c[i] = pTmp[i];
+   }
+
+}
+
+#endif
+
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/encauth/gcm/gcm_gf_mult.c,v $ */
+/* $Revision: 1.25 $ */
+/* $Date: 2007/05/12 14:32:35 $ */
+ 
diff --git a/core/lib/libtomcrypt/src/encauth/gcm/gcm_init.c b/core/lib/libtomcrypt/src/encauth/gcm/gcm_init.c
new file mode 100644
index 0000000..056e112
--- /dev/null
+++ b/core/lib/libtomcrypt/src/encauth/gcm/gcm_init.c
@@ -0,0 +1,134 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+
+/**
+   @file gcm_init.c
+   GCM implementation, initialize state, by Tom St Denis
+*/
+#include "tomcrypt.h"
+
+#ifdef LTC_GCM_MODE
+
+/**
+  Initialize a GCM state
+  @param gcm     The GCM state to initialize
+  @param cipher  The index of the cipher to use
+  @param key     The secret key
+  @param keylen  The length of the secret key
+  @return CRYPT_OK on success
+ */
+int gcm_init(gcm_state *gcm, int cipher, 
+             const unsigned char *key,  int keylen)
+{
+   int           err;
+   unsigned char B[16];
+#ifdef LTC_GCM_TABLES
+   int           x, y, z, t;
+#endif
+
+   LTC_ARGCHK(gcm != NULL);
+   LTC_ARGCHK(key != NULL);
+
+#ifdef LTC_FAST
+   if (16 % sizeof(LTC_FAST_TYPE)) {
+      return CRYPT_INVALID_ARG;
+   }
+#endif
+
+   /* is cipher valid? */
+   if ((err = cipher_is_valid(cipher)) != CRYPT_OK) {
+      return err;
+   }
+   if (cipher_descriptor[cipher]->block_length != 16) {
+      return CRYPT_INVALID_CIPHER;
+   }
+
+   /* schedule key */
+   if ((err = cipher_descriptor[cipher]->setup(key, keylen, 0, &gcm->K)) != CRYPT_OK) {
+      return err;
+   }
+
+   /* H = E(0) */
+   zeromem(B, 16);
+   if ((err = cipher_descriptor[cipher]->ecb_encrypt(B, gcm->H, &gcm->K)) != CRYPT_OK) {
+      return err;
+   }
+
+   /* setup state */
+   zeromem(gcm->buf, sizeof(gcm->buf));
+   zeromem(gcm->X,   sizeof(gcm->X));
+   gcm->cipher   = cipher;
+   gcm->mode     = LTC_GCM_MODE_IV;
+   gcm->ivmode   = 0;
+   gcm->buflen   = 0;
+   gcm->totlen   = 0;
+   gcm->pttotlen = 0;
+
+#ifdef LTC_GCM_TABLES
+   /* setup tables */
+
+   /* generate the first table as it has no shifting (from which we make the other tables) */
+   zeromem(B, 16);
+   for (y = 0; y < 256; y++) {
+        B[0] = y;
+        gcm_gf_mult(gcm->H, B, &gcm->PC[0][y][0]);
+   }
+
+   /* now generate the rest of the tables based the previous table */
+   for (x = 1; x < 16; x++) {
+      for (y = 0; y < 256; y++) {
+         /* now shift it right by 8 bits */
+         t = gcm->PC[x-1][y][15];
+         for (z = 15; z > 0; z--) {
+             gcm->PC[x][y][z] = gcm->PC[x-1][y][z-1];
+         }
+         gcm->PC[x][y][0] = gcm_shift_table[t<<1];
+         gcm->PC[x][y][1] ^= gcm_shift_table[(t<<1)+1];
+     }
+  }
+
+#endif
+
+   return CRYPT_OK;
+}
+
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/encauth/gcm/gcm_init.c,v $ */
+/* $Revision: 1.20 $ */
+/* $Date: 2007/05/12 14:32:35 $ */
diff --git a/core/lib/libtomcrypt/src/encauth/gcm/gcm_memory.c b/core/lib/libtomcrypt/src/encauth/gcm/gcm_memory.c
new file mode 100644
index 0000000..e7764c3
--- /dev/null
+++ b/core/lib/libtomcrypt/src/encauth/gcm/gcm_memory.c
@@ -0,0 +1,136 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+
+/**
+   @file gcm_memory.c
+   GCM implementation, process a packet, by Tom St Denis
+*/
+#include "tomcrypt.h"
+
+#ifdef LTC_GCM_MODE
+
+/**
+  Process an entire GCM packet in one call.
+  @param cipher            Index of cipher to use
+  @param key               The secret key
+  @param keylen            The length of the secret key
+  @param IV                The initial vector 
+  @param IVlen             The length of the initial vector
+  @param adata             The additional authentication data (header)
+  @param adatalen          The length of the adata
+  @param pt                The plaintext
+  @param ptlen             The length of the plaintext (ciphertext length is the same)
+  @param ct                The ciphertext
+  @param tag               [out] The MAC tag
+  @param taglen            [in/out] The MAC tag length
+  @param direction         Encrypt or Decrypt mode (GCM_ENCRYPT or GCM_DECRYPT)
+  @return CRYPT_OK on success
+ */
+int gcm_memory(      int           cipher,
+               const unsigned char *key,    unsigned long keylen,
+               const unsigned char *IV,     unsigned long IVlen,
+               const unsigned char *adata,  unsigned long adatalen,
+                     unsigned char *pt,     unsigned long ptlen,
+                     unsigned char *ct, 
+                     unsigned char *tag,    unsigned long *taglen,
+                               int direction)
+{
+    void      *orig;
+    gcm_state *gcm;
+    int        err;
+
+    if ((err = cipher_is_valid(cipher)) != CRYPT_OK) {
+       return err;
+    }
+ 
+    if (cipher_descriptor[cipher]->accel_gcm_memory != NULL) {
+       return 
+         cipher_descriptor[cipher]->accel_gcm_memory
+                                          (key,   keylen,
+                                           IV,    IVlen,
+                                           adata, adatalen,
+                                           pt,    ptlen,
+                                           ct,
+                                           tag,   taglen,
+                                           direction);
+    }
+
+
+
+#ifndef LTC_GCM_TABLES_SSE2
+    orig = gcm = XMALLOC(sizeof(*gcm));
+#else
+    orig = gcm = XMALLOC(sizeof(*gcm) + 16);
+#endif
+    if (gcm == NULL) {
+        return CRYPT_MEM;
+    }
+
+   /* Force GCM to be on a multiple of 16 so we can use 128-bit aligned operations
+    * note that we only modify gcm and keep orig intact.  This code is not portable
+    * but again it's only for SSE2 anyways, so who cares?
+    */
+#ifdef LTC_GCM_TABLES_SSE2
+   if ((unsigned long)gcm & 15) {
+      gcm = (gcm_state *)((unsigned long)gcm + (16 - ((unsigned long)gcm & 15)));
+   }
+#endif
+
+    if ((err = gcm_init(gcm, cipher, key, keylen)) != CRYPT_OK) {
+       goto LTC_ERR;
+    }
+    if ((err = gcm_add_iv(gcm, IV, IVlen)) != CRYPT_OK) {
+       goto LTC_ERR;
+    }
+    if ((err = gcm_add_aad(gcm, adata, adatalen)) != CRYPT_OK) {
+       goto LTC_ERR;
+    }
+    if ((err = gcm_process(gcm, pt, ptlen, ct, direction)) != CRYPT_OK) {
+       goto LTC_ERR;
+    }
+    err = gcm_done(gcm, tag, taglen);
+LTC_ERR:
+    XFREE(orig);
+    return err;
+}
+#endif
+
+
+/* $Source: /cvs/libtom/libtomcrypt/src/encauth/gcm/gcm_memory.c,v $ */
+/* $Revision: 1.25 $ */
+/* $Date: 2007/05/12 14:32:35 $ */
diff --git a/core/lib/libtomcrypt/src/encauth/gcm/gcm_mult_h.c b/core/lib/libtomcrypt/src/encauth/gcm/gcm_mult_h.c
new file mode 100644
index 0000000..269112f
--- /dev/null
+++ b/core/lib/libtomcrypt/src/encauth/gcm/gcm_mult_h.c
@@ -0,0 +1,86 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+
+/**
+   @file gcm_mult_h.c
+   GCM implementation, do the GF mult, by Tom St Denis
+*/
+#include "tomcrypt.h"
+
+#if defined(LTC_GCM_MODE)
+/**
+  GCM multiply by H
+  @param gcm   The GCM state which holds the H value
+  @param I     The value to multiply H by
+ */
+void gcm_mult_h(gcm_state *gcm, unsigned char *I)
+{
+   unsigned char T[16];
+#ifdef LTC_GCM_TABLES
+   int x;
+#ifdef LTC_GCM_TABLES_SSE2
+   asm("movdqa (%0),%%xmm0"::"r"(&gcm->PC[0][I[0]][0]));
+   for (x = 1; x < 16; x++) {
+      asm("pxor (%0),%%xmm0"::"r"(&gcm->PC[x][I[x]][0]));
+   }
+   asm("movdqa %%xmm0,(%0)"::"r"(&T));
+#else
+   int y;
+   XMEMCPY(T, &gcm->PC[0][I[0]][0], 16);
+   for (x = 1; x < 16; x++) {
+#ifdef LTC_FAST
+       for (y = 0; y < 16; y += sizeof(LTC_FAST_TYPE)) {
+           *((LTC_FAST_TYPE *)(T + y)) ^= *((LTC_FAST_TYPE *)(&gcm->PC[x][I[x]][y]));
+       }
+#else
+       for (y = 0; y < 16; y++) {
+           T[y] ^= gcm->PC[x][I[x]][y];
+       }
+#endif /* LTC_FAST */
+   }
+#endif /* LTC_GCM_TABLES_SSE2 */
+#else     
+   gcm_gf_mult(gcm->H, I, T); 
+#endif
+   XMEMCPY(I, T, 16);
+}
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/encauth/gcm/gcm_mult_h.c,v $ */
+/* $Revision: 1.6 $ */
+/* $Date: 2007/05/12 14:32:35 $ */
diff --git a/core/lib/libtomcrypt/src/encauth/gcm/gcm_process.c b/core/lib/libtomcrypt/src/encauth/gcm/gcm_process.c
new file mode 100644
index 0000000..5745e50
--- /dev/null
+++ b/core/lib/libtomcrypt/src/encauth/gcm/gcm_process.c
@@ -0,0 +1,179 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+
+/**
+   @file gcm_process.c
+   GCM implementation, process message data, by Tom St Denis
+*/
+#include "tomcrypt.h"
+
+#ifdef LTC_GCM_MODE
+
+/** 
+  Process plaintext/ciphertext through GCM
+  @param gcm       The GCM state 
+  @param pt        The plaintext
+  @param ptlen     The plaintext length (ciphertext length is the same)
+  @param ct        The ciphertext
+  @param direction Encrypt or Decrypt mode (GCM_ENCRYPT or GCM_DECRYPT)
+  @return CRYPT_OK on success
+ */
+int gcm_process(gcm_state *gcm,
+                     unsigned char *pt,     unsigned long ptlen,
+                     unsigned char *ct,
+                     int direction)
+{
+   unsigned long x;
+   int           y, err;
+   unsigned char b;
+
+   LTC_ARGCHK(gcm != NULL);
+   if (ptlen > 0) {
+      LTC_ARGCHK(pt  != NULL);
+      LTC_ARGCHK(ct  != NULL);
+   }
+
+   if (gcm->buflen > 16 || gcm->buflen < 0) {
+      return CRYPT_INVALID_ARG;
+   }
+ 
+   if ((err = cipher_is_valid(gcm->cipher)) != CRYPT_OK) {
+      return err;
+   }
+
+   /* in AAD mode? */
+   if (gcm->mode == LTC_GCM_MODE_AAD) {
+      /* let's process the AAD */
+      if (gcm->buflen) {
+         gcm->totlen += gcm->buflen * CONST64(8);
+         gcm_mult_h(gcm, gcm->X);
+      }
+
+      /* increment counter */
+      for (y = 15; y >= 12; y--) {
+          if (++gcm->Y[y] & 255) { break; }
+      }
+      /* encrypt the counter */
+      if ((err = cipher_descriptor[gcm->cipher]->ecb_encrypt(gcm->Y, gcm->buf, &gcm->K)) != CRYPT_OK) {
+         return err;
+      }
+
+      gcm->buflen = 0;
+      gcm->mode   = LTC_GCM_MODE_TEXT;
+   }
+
+   if (gcm->mode != LTC_GCM_MODE_TEXT) {
+      return CRYPT_INVALID_ARG;
+   }
+
+   x = 0;
+#ifdef LTC_FAST
+   if (gcm->buflen == 0) {
+      if (direction == GCM_ENCRYPT) { 
+         for (x = 0; x < (ptlen & ~15); x += 16) {
+             /* ctr encrypt */
+             for (y = 0; y < 16; y += sizeof(LTC_FAST_TYPE)) {
+                 *((LTC_FAST_TYPE*)(&ct[x + y])) = *((LTC_FAST_TYPE*)(&pt[x+y])) ^ *((LTC_FAST_TYPE*)(&gcm->buf[y]));
+                 *((LTC_FAST_TYPE*)(&gcm->X[y])) ^= *((LTC_FAST_TYPE*)(&ct[x+y]));
+             }
+             /* GMAC it */
+             gcm->pttotlen += 128;
+             gcm_mult_h(gcm, gcm->X);
+             /* increment counter */
+             for (y = 15; y >= 12; y--) {
+                 if (++gcm->Y[y] & 255) { break; }
+             }
+             if ((err = cipher_descriptor[gcm->cipher]->ecb_encrypt(gcm->Y, gcm->buf, &gcm->K)) != CRYPT_OK) {
+                return err;
+             }
+         }
+      } else {
+         for (x = 0; x < (ptlen & ~15); x += 16) {
+             /* ctr encrypt */
+             for (y = 0; y < 16; y += sizeof(LTC_FAST_TYPE)) {
+                 *((LTC_FAST_TYPE*)(&gcm->X[y])) ^= *((LTC_FAST_TYPE*)(&ct[x+y]));
+                 *((LTC_FAST_TYPE*)(&pt[x + y])) = *((LTC_FAST_TYPE*)(&ct[x+y])) ^ *((LTC_FAST_TYPE*)(&gcm->buf[y]));
+             }
+             /* GMAC it */
+             gcm->pttotlen += 128;
+             gcm_mult_h(gcm, gcm->X);
+             /* increment counter */
+             for (y = 15; y >= 12; y--) {
+                 if (++gcm->Y[y] & 255) { break; }
+             }
+             if ((err = cipher_descriptor[gcm->cipher]->ecb_encrypt(gcm->Y, gcm->buf, &gcm->K)) != CRYPT_OK) {
+                return err;
+             }
+         }
+     }
+   }
+#endif        
+
+   /* process text */
+   for (; x < ptlen; x++) {
+       if (gcm->buflen == 16) {
+          gcm->pttotlen += 128;
+          gcm_mult_h(gcm, gcm->X);
+          
+          /* increment counter */
+          for (y = 15; y >= 12; y--) {
+              if (++gcm->Y[y] & 255) { break; }
+          }
+          if ((err = cipher_descriptor[gcm->cipher]->ecb_encrypt(gcm->Y, gcm->buf, &gcm->K)) != CRYPT_OK) {
+             return err;
+          }
+          gcm->buflen = 0;
+       }
+
+       if (direction == GCM_ENCRYPT) {
+          b = ct[x] = pt[x] ^ gcm->buf[gcm->buflen]; 
+       } else {
+          b = ct[x];
+          pt[x] = ct[x] ^ gcm->buf[gcm->buflen];
+       }
+       gcm->X[gcm->buflen++] ^= b;          
+   }
+
+   return CRYPT_OK;
+}
+
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/encauth/gcm/gcm_process.c,v $ */
+/* $Revision: 1.16 $ */
+/* $Date: 2007/05/12 14:32:35 $ */
diff --git a/core/lib/libtomcrypt/src/encauth/gcm/gcm_reset.c b/core/lib/libtomcrypt/src/encauth/gcm/gcm_reset.c
new file mode 100644
index 0000000..3b293b3
--- /dev/null
+++ b/core/lib/libtomcrypt/src/encauth/gcm/gcm_reset.c
@@ -0,0 +1,71 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+
+/**
+   @file gcm_reset.c
+   GCM implementation, reset a used state so it can accept IV data, by Tom St Denis
+*/
+#include "tomcrypt.h"
+
+#ifdef LTC_GCM_MODE
+
+/**
+  Reset a GCM state to as if you just called gcm_init().  This saves the initialization time.
+  @param gcm   The GCM state to reset
+  @return CRYPT_OK on success
+*/
+int gcm_reset(gcm_state *gcm)
+{
+   LTC_ARGCHK(gcm != NULL);
+
+   zeromem(gcm->buf, sizeof(gcm->buf));
+   zeromem(gcm->X,   sizeof(gcm->X));
+   gcm->mode     = LTC_GCM_MODE_IV;
+   gcm->ivmode   = 0;
+   gcm->buflen   = 0;
+   gcm->totlen   = 0;
+   gcm->pttotlen = 0;
+  
+   return CRYPT_OK;
+}
+
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/encauth/gcm/gcm_reset.c,v $ */
+/* $Revision: 1.6 $ */
+/* $Date: 2007/05/12 14:32:35 $ */
diff --git a/core/lib/libtomcrypt/src/encauth/gcm/sub.mk b/core/lib/libtomcrypt/src/encauth/gcm/sub.mk
new file mode 100644
index 0000000..62946f5
--- /dev/null
+++ b/core/lib/libtomcrypt/src/encauth/gcm/sub.mk
@@ -0,0 +1,10 @@
+srcs-y += gcm_add_aad.c
+srcs-y += gcm_add_iv.c
+srcs-y += gcm_done.c
+srcs-y += gcm_gf_mult.c
+srcs-y += gcm_init.c
+srcs-y += gcm_memory.c
+srcs-y += gcm_mult_h.c
+srcs-y += gcm_process.c
+srcs-y += gcm_reset.c
+# srcs-y += gcm_test.c
diff --git a/core/lib/libtomcrypt/src/encauth/ocb/ocb_decrypt.c b/core/lib/libtomcrypt/src/encauth/ocb/ocb_decrypt.c
new file mode 100644
index 0000000..e1d2d3a
--- /dev/null
+++ b/core/lib/libtomcrypt/src/encauth/ocb/ocb_decrypt.c
@@ -0,0 +1,106 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+
+/**
+   @file ocb_decrypt.c
+   OCB implementation, decrypt data, by Tom St Denis 
+*/
+#include "tomcrypt.h"
+
+#ifdef LTC_OCB_MODE
+
+/**
+  Decrypt a block with OCB.
+  @param ocb    The OCB state
+  @param ct     The ciphertext (length of the block size of the block cipher)
+  @param pt     [out] The plaintext (length of ct)
+  @return CRYPT_OK if successful
+*/
+int ocb_decrypt(ocb_state *ocb, const unsigned char *ct, unsigned char *pt)
+{
+   unsigned char Z[MAXBLOCKSIZE], tmp[MAXBLOCKSIZE];
+   int err, x;
+
+   LTC_ARGCHK(ocb != NULL);
+   LTC_ARGCHK(pt  != NULL);
+   LTC_ARGCHK(ct  != NULL);
+
+   /* check if valid cipher */
+   if ((err = cipher_is_valid(ocb->cipher)) != CRYPT_OK) {
+      return err;
+   }
+   LTC_ARGCHK(cipher_descriptor[ocb->cipher].ecb_decrypt != NULL);
+   
+   /* check length */
+   if (ocb->block_len != cipher_descriptor[ocb->cipher].block_length) {
+      return CRYPT_INVALID_ARG;
+   }
+
+   /* Get Z[i] value */
+   ocb_shift_xor(ocb, Z);
+
+   /* xor ct in, encrypt, xor Z out */
+   for (x = 0; x < ocb->block_len; x++) {
+       tmp[x] = ct[x] ^ Z[x];
+   }
+   if ((err = cipher_descriptor[ocb->cipher].ecb_decrypt(tmp, pt, &ocb->key)) != CRYPT_OK) {
+      return err;
+   }
+   for (x = 0; x < ocb->block_len; x++) {
+       pt[x] ^= Z[x];
+   }
+
+   /* compute checksum */
+   for (x = 0; x < ocb->block_len; x++) {
+       ocb->checksum[x] ^= pt[x];
+   }
+
+
+#ifdef LTC_CLEAN_STACK
+   zeromem(Z, sizeof(Z));
+   zeromem(tmp, sizeof(tmp));
+#endif
+   return CRYPT_OK;
+}
+
+#endif
+
+
+/* $Source: /cvs/libtom/libtomcrypt/src/encauth/ocb/ocb_decrypt.c,v $ */
+/* $Revision: 1.7 $ */
+/* $Date: 2007/05/12 14:32:35 $ */
diff --git a/core/lib/libtomcrypt/src/encauth/ocb/ocb_decrypt_verify_memory.c b/core/lib/libtomcrypt/src/encauth/ocb/ocb_decrypt_verify_memory.c
new file mode 100644
index 0000000..c3d0efb
--- /dev/null
+++ b/core/lib/libtomcrypt/src/encauth/ocb/ocb_decrypt_verify_memory.c
@@ -0,0 +1,113 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+
+/** 
+  @file ocb_decrypt_verify_memory.c
+  OCB implementation, helper to decrypt block of memory, by Tom St Denis 
+*/
+#include "tomcrypt.h"
+
+#ifdef LTC_OCB_MODE
+
+/**
+   Decrypt and compare the tag with OCB.
+   @param cipher     The index of the cipher desired
+   @param key        The secret key
+   @param keylen     The length of the secret key (octets)
+   @param nonce      The session nonce (length of the block size of the block cipher)
+   @param ct         The ciphertext
+   @param ctlen      The length of the ciphertext (octets)
+   @param pt         [out] The plaintext
+   @param tag        The tag to compare against
+   @param taglen     The length of the tag (octets)
+   @param stat       [out] The result of the tag comparison (1==valid, 0==invalid)
+   @return CRYPT_OK if successful regardless of the tag comparison
+*/
+int ocb_decrypt_verify_memory(int cipher,
+    const unsigned char *key,    unsigned long keylen,
+    const unsigned char *nonce,  
+    const unsigned char *ct,     unsigned long ctlen,
+          unsigned char *pt,
+    const unsigned char *tag,    unsigned long taglen,
+          int           *stat)
+{
+   int err;
+   ocb_state *ocb;
+
+   LTC_ARGCHK(key    != NULL);
+   LTC_ARGCHK(nonce  != NULL);
+   LTC_ARGCHK(pt     != NULL);
+   LTC_ARGCHK(ct     != NULL);
+   LTC_ARGCHK(tag    != NULL);
+   LTC_ARGCHK(stat    != NULL);
+
+   /* allocate memory */
+   ocb = XMALLOC(sizeof(ocb_state));
+   if (ocb == NULL) {
+      return CRYPT_MEM;
+   }
+
+   if ((err = ocb_init(ocb, cipher, key, keylen, nonce)) != CRYPT_OK) {
+      goto LBL_ERR; 
+   }
+
+   while (ctlen > (unsigned long)ocb->block_len) {
+        if ((err = ocb_decrypt(ocb, ct, pt)) != CRYPT_OK) {
+            goto LBL_ERR; 
+        }
+        ctlen   -= ocb->block_len;
+        pt      += ocb->block_len;
+        ct      += ocb->block_len;
+   }
+
+   err = ocb_done_decrypt(ocb, ct, ctlen, pt, tag, taglen, stat);
+LBL_ERR:
+#ifdef LTC_CLEAN_STACK
+   zeromem(ocb, sizeof(ocb_state));
+#endif
+ 
+   XFREE(ocb);
+
+   return err;
+}
+
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/encauth/ocb/ocb_decrypt_verify_memory.c,v $ */
+/* $Revision: 1.6 $ */
+/* $Date: 2007/05/12 14:32:35 $ */
diff --git a/core/lib/libtomcrypt/src/encauth/ocb/ocb_done_decrypt.c b/core/lib/libtomcrypt/src/encauth/ocb/ocb_done_decrypt.c
new file mode 100644
index 0000000..336ef86
--- /dev/null
+++ b/core/lib/libtomcrypt/src/encauth/ocb/ocb_done_decrypt.c
@@ -0,0 +1,107 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+
+/** 
+   @file ocb_done_decrypt.c
+   OCB implementation, terminate decryption, by Tom St Denis
+*/
+#include "tomcrypt.h"
+
+#ifdef LTC_OCB_MODE
+
+/**
+   Terminate a decrypting OCB state
+   @param ocb    The OCB state
+   @param ct     The ciphertext (if any)
+   @param ctlen  The length of the ciphertext (octets)
+   @param pt     [out] The plaintext
+   @param tag    The authentication tag (to compare against)
+   @param taglen The length of the authentication tag provided
+   @param stat    [out] The result of the tag comparison
+   @return CRYPT_OK if the process was successful regardless if the tag is valid
+*/
+int ocb_done_decrypt(ocb_state *ocb, 
+                     const unsigned char *ct,  unsigned long ctlen,
+                           unsigned char *pt, 
+                     const unsigned char *tag, unsigned long taglen, int *stat)
+{
+   int err;
+   unsigned char *tagbuf;
+   unsigned long tagbuflen;
+
+   LTC_ARGCHK(ocb  != NULL);
+   LTC_ARGCHK(pt   != NULL);
+   LTC_ARGCHK(ct   != NULL);
+   LTC_ARGCHK(tag  != NULL);
+   LTC_ARGCHK(stat != NULL);
+
+   /* default to failed */
+   *stat = 0;
+
+   /* allocate memory */
+   tagbuf = XMALLOC(MAXBLOCKSIZE);
+   if (tagbuf == NULL) {
+      return CRYPT_MEM;
+   }
+
+   tagbuflen = MAXBLOCKSIZE;
+   if ((err = s_ocb_done(ocb, ct, ctlen, pt, tagbuf, &tagbuflen, 1)) != CRYPT_OK) {
+      goto LBL_ERR;
+   }
+
+   if (taglen <= tagbuflen && XMEMCMP(tagbuf, tag, taglen) == 0) {
+      *stat = 1;
+   }
+
+   err = CRYPT_OK;
+LBL_ERR:
+#ifdef LTC_CLEAN_STACK
+   zeromem(tagbuf, MAXBLOCKSIZE);
+#endif
+
+   XFREE(tagbuf);
+
+   return err;
+}
+
+#endif
+
+
+/* $Source: /cvs/libtom/libtomcrypt/src/encauth/ocb/ocb_done_decrypt.c,v $ */
+/* $Revision: 1.7 $ */
+/* $Date: 2007/05/12 14:32:35 $ */
diff --git a/core/lib/libtomcrypt/src/encauth/ocb/ocb_done_encrypt.c b/core/lib/libtomcrypt/src/encauth/ocb/ocb_done_encrypt.c
new file mode 100644
index 0000000..ccc9129
--- /dev/null
+++ b/core/lib/libtomcrypt/src/encauth/ocb/ocb_done_encrypt.c
@@ -0,0 +1,73 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+
+/** 
+   @file ocb_done_encrypt.c
+   OCB implementation, terminate encryption, by Tom St Denis
+*/
+#include "tomcrypt.h"
+
+#ifdef LTC_OCB_MODE
+
+/** 
+   Terminate an encryption OCB state
+   @param ocb       The OCB state
+   @param pt        Remaining plaintext (if any)
+   @param ptlen     The length of the plaintext (octets)
+   @param ct        [out] The ciphertext (if any)
+   @param tag       [out] The tag for the OCB stream
+   @param taglen    [in/out] The max size and resulting size of the tag
+   @return CRYPT_OK if successful
+*/
+int ocb_done_encrypt(ocb_state *ocb, const unsigned char *pt, unsigned long ptlen,
+                     unsigned char *ct, unsigned char *tag, unsigned long *taglen)
+{
+   LTC_ARGCHK(ocb    != NULL);
+   LTC_ARGCHK(pt     != NULL);
+   LTC_ARGCHK(ct     != NULL);
+   LTC_ARGCHK(tag    != NULL);
+   LTC_ARGCHK(taglen != NULL);
+   return s_ocb_done(ocb, pt, ptlen, ct, tag, taglen, 0);
+}
+
+#endif
+
+
+/* $Source: /cvs/libtom/libtomcrypt/src/encauth/ocb/ocb_done_encrypt.c,v $ */
+/* $Revision: 1.6 $ */
+/* $Date: 2007/05/12 14:32:35 $ */
diff --git a/core/lib/libtomcrypt/src/encauth/ocb/ocb_encrypt.c b/core/lib/libtomcrypt/src/encauth/ocb/ocb_encrypt.c
new file mode 100644
index 0000000..bce2af7
--- /dev/null
+++ b/core/lib/libtomcrypt/src/encauth/ocb/ocb_encrypt.c
@@ -0,0 +1,99 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+
+/** 
+   @file ocb_encrypt.c
+   OCB implementation, encrypt data, by Tom St Denis
+*/
+#include "tomcrypt.h"
+
+#ifdef LTC_OCB_MODE
+
+/**
+   Encrypt a block of data with OCB.
+   @param ocb     The OCB state
+   @param pt      The plaintext (length of the block size of the block cipher)
+   @param ct      [out] The ciphertext (same size as the pt)
+   @return CRYPT_OK if successful
+*/
+int ocb_encrypt(ocb_state *ocb, const unsigned char *pt, unsigned char *ct)
+{
+   unsigned char Z[MAXBLOCKSIZE], tmp[MAXBLOCKSIZE];
+   int err, x;
+
+   LTC_ARGCHK(ocb != NULL);
+   LTC_ARGCHK(pt  != NULL);
+   LTC_ARGCHK(ct  != NULL);
+   if ((err = cipher_is_valid(ocb->cipher)) != CRYPT_OK) {
+      return err;
+   }
+   if (ocb->block_len != cipher_descriptor[ocb->cipher].block_length) {
+      return CRYPT_INVALID_ARG;
+   }
+
+   /* compute checksum */
+   for (x = 0; x < ocb->block_len; x++) {
+       ocb->checksum[x] ^= pt[x];
+   }
+
+   /* Get Z[i] value */
+   ocb_shift_xor(ocb, Z);
+
+   /* xor pt in, encrypt, xor Z out */
+   for (x = 0; x < ocb->block_len; x++) {
+       tmp[x] = pt[x] ^ Z[x];
+   }
+   if ((err = cipher_descriptor[ocb->cipher].ecb_encrypt(tmp, ct, &ocb->key)) != CRYPT_OK) {
+      return err;
+   }
+   for (x = 0; x < ocb->block_len; x++) {
+       ct[x] ^= Z[x];
+   }
+
+#ifdef LTC_CLEAN_STACK
+   zeromem(Z, sizeof(Z));
+   zeromem(tmp, sizeof(tmp));
+#endif
+   return CRYPT_OK;
+}
+
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/encauth/ocb/ocb_encrypt.c,v $ */
+/* $Revision: 1.7 $ */
+/* $Date: 2007/05/12 14:32:35 $ */
diff --git a/core/lib/libtomcrypt/src/encauth/ocb/ocb_encrypt_authenticate_memory.c b/core/lib/libtomcrypt/src/encauth/ocb/ocb_encrypt_authenticate_memory.c
new file mode 100644
index 0000000..da44945
--- /dev/null
+++ b/core/lib/libtomcrypt/src/encauth/ocb/ocb_encrypt_authenticate_memory.c
@@ -0,0 +1,111 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+
+/** 
+  @file ocb_encrypt_authenticate_memory.c
+  OCB implementation, encrypt block of memory, by Tom St Denis
+*/
+#include "tomcrypt.h"
+
+#ifdef LTC_OCB_MODE
+
+/**
+   Encrypt and generate an authentication code for a buffer of memory
+   @param cipher     The index of the cipher desired
+   @param key        The secret key
+   @param keylen     The length of the secret key (octets)
+   @param nonce      The session nonce (length of the block ciphers block size)
+   @param pt         The plaintext
+   @param ptlen      The length of the plaintext (octets)
+   @param ct         [out] The ciphertext
+   @param tag        [out] The authentication tag
+   @param taglen     [in/out] The max size and resulting size of the authentication tag
+   @return CRYPT_OK if successful
+*/
+int ocb_encrypt_authenticate_memory(int cipher,
+    const unsigned char *key,    unsigned long keylen,
+    const unsigned char *nonce,  
+    const unsigned char *pt,     unsigned long ptlen,
+          unsigned char *ct,
+          unsigned char *tag,    unsigned long *taglen)
+{
+   int err;
+   ocb_state *ocb;
+
+   LTC_ARGCHK(key    != NULL);
+   LTC_ARGCHK(nonce  != NULL);
+   LTC_ARGCHK(pt     != NULL);
+   LTC_ARGCHK(ct     != NULL);
+   LTC_ARGCHK(tag    != NULL);
+   LTC_ARGCHK(taglen != NULL);
+
+   /* allocate ram */
+   ocb = XMALLOC(sizeof(ocb_state));
+   if (ocb == NULL) {
+      return CRYPT_MEM;
+   }
+
+   if ((err = ocb_init(ocb, cipher, key, keylen, nonce)) != CRYPT_OK) {
+      goto LBL_ERR;
+   }
+
+   while (ptlen > (unsigned long)ocb->block_len) {
+        if ((err = ocb_encrypt(ocb, pt, ct)) != CRYPT_OK) {
+           goto LBL_ERR;
+        }
+        ptlen   -= ocb->block_len;
+        pt      += ocb->block_len;
+        ct      += ocb->block_len;
+   }
+
+   err = ocb_done_encrypt(ocb, pt, ptlen, ct, tag, taglen);
+LBL_ERR:
+#ifdef LTC_CLEAN_STACK
+   zeromem(ocb, sizeof(ocb_state));
+#endif
+
+   XFREE(ocb);
+
+   return err;
+}
+
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/encauth/ocb/ocb_encrypt_authenticate_memory.c,v $ */
+/* $Revision: 1.6 $ */
+/* $Date: 2007/05/12 14:32:35 $ */
diff --git a/core/lib/libtomcrypt/src/encauth/ocb/ocb_init.c b/core/lib/libtomcrypt/src/encauth/ocb/ocb_init.c
new file mode 100644
index 0000000..b656e6a
--- /dev/null
+++ b/core/lib/libtomcrypt/src/encauth/ocb/ocb_init.c
@@ -0,0 +1,168 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+
+/**
+   @file ocb_init.c
+   OCB implementation, initialize state, by Tom St Denis
+*/
+#include "tomcrypt.h"
+
+#ifdef LTC_OCB_MODE
+
+static const struct {
+    int           len;
+    unsigned char poly_div[MAXBLOCKSIZE], 
+                  poly_mul[MAXBLOCKSIZE];
+} polys[] = {
+{
+    8,
+    { 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0D },
+    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1B }
+}, {
+    16, 
+    { 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+      0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x43 },
+    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+      0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x87 }
+}
+};
+
+/**
+  Initialize an OCB context.
+  @param ocb     [out] The destination of the OCB state
+  @param cipher  The index of the desired cipher
+  @param key     The secret key
+  @param keylen  The length of the secret key (octets)
+  @param nonce   The session nonce (length of the block size of the cipher)
+  @return CRYPT_OK if successful
+*/
+int ocb_init(ocb_state *ocb, int cipher, 
+             const unsigned char *key, unsigned long keylen, const unsigned char *nonce)
+{
+   int poly, x, y, m, err;
+
+   LTC_ARGCHK(ocb   != NULL);
+   LTC_ARGCHK(key   != NULL);
+   LTC_ARGCHK(nonce != NULL);
+
+   /* valid cipher? */
+   if ((err = cipher_is_valid(cipher)) != CRYPT_OK) {
+      return err;
+   }
+
+   /* determine which polys to use */
+   ocb->block_len = cipher_descriptor[cipher].block_length;
+   x = (int)(sizeof(polys)/sizeof(polys[0]));
+   for (poly = 0; poly < x; poly++) {
+       if (polys[poly].len == ocb->block_len) { 
+          break;
+       }
+   }
+   if (poly == x) {
+      return CRYPT_INVALID_ARG; /* block_len not found in polys */
+   }
+   if (polys[poly].len != ocb->block_len) {
+      return CRYPT_INVALID_ARG;
+   }   
+
+   /* schedule the key */
+   if ((err = cipher_descriptor[cipher].setup(key, keylen, 0, &ocb->key)) != CRYPT_OK) {
+      return err;
+   }
+ 
+   /* find L = E[0] */
+   zeromem(ocb->L, ocb->block_len);
+   if ((err = cipher_descriptor[cipher].ecb_encrypt(ocb->L, ocb->L, &ocb->key)) != CRYPT_OK) {
+      return err;
+   }
+
+   /* find R = E[N xor L] */
+   for (x = 0; x < ocb->block_len; x++) {
+       ocb->R[x] = ocb->L[x] ^ nonce[x];
+   }
+   if ((err = cipher_descriptor[cipher].ecb_encrypt(ocb->R, ocb->R, &ocb->key)) != CRYPT_OK) {
+      return err;
+   }
+
+   /* find Ls[i] = L << i for i == 0..31 */
+   XMEMCPY(ocb->Ls[0], ocb->L, ocb->block_len);
+   for (x = 1; x < 32; x++) {
+       m = ocb->Ls[x-1][0] >> 7;
+       for (y = 0; y < ocb->block_len-1; y++) {
+           ocb->Ls[x][y] = ((ocb->Ls[x-1][y] << 1) | (ocb->Ls[x-1][y+1] >> 7)) & 255;
+       }
+       ocb->Ls[x][ocb->block_len-1] = (ocb->Ls[x-1][ocb->block_len-1] << 1) & 255;
+
+       if (m == 1) {
+          for (y = 0; y < ocb->block_len; y++) {
+              ocb->Ls[x][y] ^= polys[poly].poly_mul[y];
+          }
+       }
+    }
+
+    /* find Lr = L / x */
+    m = ocb->L[ocb->block_len-1] & 1;
+
+    /* shift right */
+    for (x = ocb->block_len - 1; x > 0; x--) {
+        ocb->Lr[x] = ((ocb->L[x] >> 1) | (ocb->L[x-1] << 7)) & 255;
+    }
+    ocb->Lr[0] = ocb->L[0] >> 1;
+
+    if (m == 1) {
+       for (x = 0; x < ocb->block_len; x++) {
+           ocb->Lr[x] ^= polys[poly].poly_div[x];
+       }
+    }
+
+    /* set Li, checksum */
+    zeromem(ocb->Li,       ocb->block_len);
+    zeromem(ocb->checksum, ocb->block_len);
+
+    /* set other params */
+    ocb->block_index = 1;
+    ocb->cipher      = cipher;
+
+    return CRYPT_OK;
+}
+
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/encauth/ocb/ocb_init.c,v $ */
+/* $Revision: 1.7 $ */
+/* $Date: 2007/05/12 14:32:35 $ */
diff --git a/core/lib/libtomcrypt/src/encauth/ocb/ocb_ntz.c b/core/lib/libtomcrypt/src/encauth/ocb/ocb_ntz.c
new file mode 100644
index 0000000..8012d01
--- /dev/null
+++ b/core/lib/libtomcrypt/src/encauth/ocb/ocb_ntz.c
@@ -0,0 +1,69 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+
+/**
+   @file ocb_ntz.c
+   OCB implementation, internal function, by Tom St Denis
+*/
+
+#include "tomcrypt.h"
+
+#ifdef LTC_OCB_MODE
+
+/**
+   Returns the number of leading zero bits [from lsb up]
+   @param x  The 32-bit value to observe
+   @return   The number of bits [from the lsb up] that are zero
+*/
+int ocb_ntz(unsigned long x)
+{
+   int c;
+   x &= 0xFFFFFFFFUL;
+   c = 0;
+   while ((x & 1) == 0) {
+      ++c;
+      x >>= 1;
+   }
+   return c;
+}
+
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/encauth/ocb/ocb_ntz.c,v $ */
+/* $Revision: 1.6 $ */
+/* $Date: 2007/05/12 14:32:35 $ */
diff --git a/core/lib/libtomcrypt/src/encauth/ocb/ocb_shift_xor.c b/core/lib/libtomcrypt/src/encauth/ocb/ocb_shift_xor.c
new file mode 100644
index 0000000..e5c5436
--- /dev/null
+++ b/core/lib/libtomcrypt/src/encauth/ocb/ocb_shift_xor.c
@@ -0,0 +1,66 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+
+/** 
+   @file ocb_shift_xor.c
+   OCB implementation, internal function, by Tom St Denis
+*/
+#include "tomcrypt.h"
+
+#ifdef LTC_OCB_MODE
+
+/**
+   Compute the shift/xor for OCB (internal function)
+   @param ocb  The OCB state 
+   @param Z    The destination of the shift
+*/
+void ocb_shift_xor(ocb_state *ocb, unsigned char *Z)
+{
+   int x, y;
+   y = ocb_ntz(ocb->block_index++);
+   for (x = 0; x < ocb->block_len; x++) {
+       ocb->Li[x] ^= ocb->Ls[y][x];
+       Z[x]        = ocb->Li[x] ^ ocb->R[x];
+   }
+}
+
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/encauth/ocb/ocb_shift_xor.c,v $ */
+/* $Revision: 1.6 $ */
+/* $Date: 2007/05/12 14:32:35 $ */
diff --git a/core/lib/libtomcrypt/src/encauth/ocb/s_ocb_done.c b/core/lib/libtomcrypt/src/encauth/ocb/s_ocb_done.c
new file mode 100644
index 0000000..50d4af6
--- /dev/null
+++ b/core/lib/libtomcrypt/src/encauth/ocb/s_ocb_done.c
@@ -0,0 +1,175 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+
+/** 
+   @file s_ocb_done.c
+   OCB implementation, internal helper, by Tom St Denis
+*/
+#include "tomcrypt.h"
+
+#ifdef LTC_OCB_MODE
+
+/* Since the last block is encrypted in CTR mode the same code can
+ * be used to finish a decrypt or encrypt stream.  The only difference
+ * is we XOR the final ciphertext into the checksum so we have to xor it
+ * before we CTR [decrypt] or after [encrypt]
+ *
+ * the names pt/ptlen/ct really just mean in/inlen/out but this is the way I wrote it... 
+ */
+
+/**
+   Shared code to finish an OCB stream
+   @param ocb    The OCB state
+   @param pt     The remaining plaintext [or input]
+   @param ptlen  The length of the input (octets)
+   @param ct     [out] The output buffer
+   @param tag    [out] The destination for the authentication tag
+   @param taglen [in/out] The max size and resulting size of the authentication tag
+   @param mode   The mode we are terminating, 0==encrypt, 1==decrypt
+   @return       CRYPT_OK if successful
+*/
+int s_ocb_done(ocb_state *ocb, const unsigned char *pt, unsigned long ptlen,
+               unsigned char *ct, unsigned char *tag, unsigned long *taglen, int mode)
+
+{
+   unsigned char *Z, *Y, *X;
+   int err, x;
+
+   LTC_ARGCHK(ocb    != NULL);
+   LTC_ARGCHK(pt     != NULL);
+   LTC_ARGCHK(ct     != NULL);
+   LTC_ARGCHK(tag    != NULL);
+   LTC_ARGCHK(taglen != NULL);
+   if ((err = cipher_is_valid(ocb->cipher)) != CRYPT_OK) {
+      return err;
+   }
+   if (ocb->block_len != cipher_descriptor[ocb->cipher].block_length ||
+       (int)ptlen > ocb->block_len || (int)ptlen < 0) {
+      return CRYPT_INVALID_ARG;
+   }
+
+   /* allocate ram */
+   Z = XMALLOC(MAXBLOCKSIZE);
+   Y = XMALLOC(MAXBLOCKSIZE);
+   X = XMALLOC(MAXBLOCKSIZE);
+   if (X == NULL || Y == NULL || Z == NULL) {
+      if (X != NULL) {
+         XFREE(X);
+      }
+      if (Y != NULL) {
+         XFREE(Y);
+      }
+      if (Z != NULL) {
+         XFREE(Z);
+      }
+      return CRYPT_MEM;
+   }
+
+   /* compute X[m] = len(pt[m]) XOR Lr XOR Z[m] */
+   ocb_shift_xor(ocb, X); 
+   XMEMCPY(Z, X, ocb->block_len);
+
+   X[ocb->block_len-1] ^= (ptlen*8)&255;
+   X[ocb->block_len-2] ^= ((ptlen*8)>>8)&255;
+   for (x = 0; x < ocb->block_len; x++) {
+       X[x] ^= ocb->Lr[x]; 
+   }
+
+   /* Y[m] = E(X[m])) */
+   if ((err = cipher_descriptor[ocb->cipher].ecb_encrypt(X, Y, &ocb->key)) != CRYPT_OK) {
+      goto error;
+   }
+
+   if (mode == 1) {
+      /* decrypt mode, so let's xor it first */
+      /* xor C[m] into checksum */
+      for (x = 0; x < (int)ptlen; x++) {
+         ocb->checksum[x] ^= ct[x];
+      }  
+   }
+
+   /* C[m] = P[m] xor Y[m] */
+   for (x = 0; x < (int)ptlen; x++) {
+       ct[x] = pt[x] ^ Y[x];
+   }
+
+   if (mode == 0) {
+      /* encrypt mode */    
+      /* xor C[m] into checksum */
+      for (x = 0; x < (int)ptlen; x++) {
+          ocb->checksum[x] ^= ct[x];
+      }
+   }
+
+   /* xor Y[m] and Z[m] into checksum */
+   for (x = 0; x < ocb->block_len; x++) {
+       ocb->checksum[x] ^= Y[x] ^ Z[x];
+   }
+   
+   /* encrypt checksum, er... tag!! */
+   if ((err = cipher_descriptor[ocb->cipher].ecb_encrypt(ocb->checksum, X, &ocb->key)) != CRYPT_OK) {
+      goto error;
+   }
+   cipher_descriptor[ocb->cipher].done(&ocb->key);
+
+   /* now store it */
+   for (x = 0; x < ocb->block_len && x < (int)*taglen; x++) {
+       tag[x] = X[x];
+   }
+   *taglen = x;
+
+#ifdef LTC_CLEAN_STACK
+   zeromem(X, MAXBLOCKSIZE);
+   zeromem(Y, MAXBLOCKSIZE);
+   zeromem(Z, MAXBLOCKSIZE);
+   zeromem(ocb, sizeof(*ocb));
+#endif
+error:   
+   XFREE(X);
+   XFREE(Y);
+   XFREE(Z);
+
+   return err;
+}
+
+#endif
+
+
+/* $Source: /cvs/libtom/libtomcrypt/src/encauth/ocb/s_ocb_done.c,v $ */
+/* $Revision: 1.8 $ */
+/* $Date: 2007/05/12 14:32:35 $ */
diff --git a/core/lib/libtomcrypt/src/encauth/sub.mk b/core/lib/libtomcrypt/src/encauth/sub.mk
new file mode 100644
index 0000000..1434122
--- /dev/null
+++ b/core/lib/libtomcrypt/src/encauth/sub.mk
@@ -0,0 +1,2 @@
+subdirs-$(CFG_CRYPTO_CCM) += ccm
+subdirs-$(CFG_CRYPTO_GCM) += gcm
diff --git a/core/lib/libtomcrypt/src/hashes/helper/hash_file.c b/core/lib/libtomcrypt/src/hashes/helper/hash_file.c
new file mode 100644
index 0000000..9d058c6
--- /dev/null
+++ b/core/lib/libtomcrypt/src/hashes/helper/hash_file.c
@@ -0,0 +1,82 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+#ifndef LTC_NO_FILE
+/** 
+  @file hash_file.c
+  Hash a file, Tom St Denis
+*/
+
+/**
+  @param hash   The index of the hash desired
+  @param fname  The name of the file you wish to hash
+  @param out    [out] The destination of the digest
+  @param outlen [in/out] The max size and resulting size of the message digest
+  @result CRYPT_OK if successful
+*/
+int hash_file(int hash, const char *fname, unsigned char *out, unsigned long *outlen)
+{
+    FILE *in;
+    int err;
+    LTC_ARGCHK(fname  != NULL);
+    LTC_ARGCHK(out    != NULL);
+    LTC_ARGCHK(outlen != NULL);
+
+    if ((err = hash_is_valid(hash)) != CRYPT_OK) {
+        return err;
+    }
+
+    in = fopen(fname, "rb");
+    if (in == NULL) { 
+       return CRYPT_FILE_NOTFOUND;
+    }
+
+    err = hash_filehandle(hash, in, out, outlen);
+    if (fclose(in) != 0) {
+       return CRYPT_ERROR;
+    }
+
+    return err;
+}
+#endif /* #ifndef LTC_NO_FILE */
+
+
+/* $Source: /cvs/libtom/libtomcrypt/src/hashes/helper/hash_file.c,v $ */
+/* $Revision: 1.5 $ */
+/* $Date: 2006/12/28 01:27:23 $ */
diff --git a/core/lib/libtomcrypt/src/hashes/helper/hash_filehandle.c b/core/lib/libtomcrypt/src/hashes/helper/hash_filehandle.c
new file mode 100644
index 0000000..b283846
--- /dev/null
+++ b/core/lib/libtomcrypt/src/hashes/helper/hash_filehandle.c
@@ -0,0 +1,96 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+#ifndef LTC_NO_FILE
+/**
+   @file hash_filehandle.c
+   Hash open files, Tom St Denis
+*/
+
+/** 
+  Hash data from an open file handle.  
+  @param hash   The index of the hash you want to use
+  @param in     The FILE* handle of the file you want to hash
+  @param out    [out] The destination of the digest
+  @param outlen [in/out] The max size and resulting size of the digest
+  @result CRYPT_OK if successful   
+*/
+int hash_filehandle(int hash, FILE *in, unsigned char *out, unsigned long *outlen)
+{
+    hash_state md;
+    unsigned char buf[512];
+    size_t x;
+    int err;
+
+    LTC_ARGCHK(out    != NULL);
+    LTC_ARGCHK(outlen != NULL);
+    LTC_ARGCHK(in     != NULL);
+
+    if ((err = hash_is_valid(hash)) != CRYPT_OK) {
+        return err;
+    }
+
+    if (*outlen < hash_descriptor[hash].hashsize) {
+       *outlen = hash_descriptor[hash].hashsize;
+       return CRYPT_BUFFER_OVERFLOW;
+    }
+    if ((err = hash_descriptor[hash].init(&md)) != CRYPT_OK) {
+       return err;
+    }
+
+    *outlen = hash_descriptor[hash].hashsize;
+    do {
+        x = fread(buf, 1, sizeof(buf), in);
+        if ((err = hash_descriptor[hash].process(&md, buf, x)) != CRYPT_OK) {
+           return err;
+        }
+    } while (x == sizeof(buf));
+    err = hash_descriptor[hash].done(&md, out);
+
+#ifdef LTC_CLEAN_STACK
+    zeromem(buf, sizeof(buf));
+#endif
+    return err;
+}
+#endif /* #ifndef LTC_NO_FILE */
+
+
+/* $Source: /cvs/libtom/libtomcrypt/src/hashes/helper/hash_filehandle.c,v $ */
+/* $Revision: 1.6 $ */
+/* $Date: 2006/12/28 01:27:23 $ */
diff --git a/core/lib/libtomcrypt/src/hashes/helper/hash_memory.c b/core/lib/libtomcrypt/src/hashes/helper/hash_memory.c
new file mode 100644
index 0000000..2e8dce0
--- /dev/null
+++ b/core/lib/libtomcrypt/src/hashes/helper/hash_memory.c
@@ -0,0 +1,96 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file hash_memory.c
+  Hash memory helper, Tom St Denis
+*/
+
+/**
+  Hash a block of memory and store the digest.
+  @param hash   The index of the hash you wish to use
+  @param in     The data you wish to hash
+  @param inlen  The length of the data to hash (octets)
+  @param out    [out] Where to store the digest
+  @param outlen [in/out] Max size and resulting size of the digest
+  @return CRYPT_OK if successful
+*/
+int hash_memory(int hash, const unsigned char *in, unsigned long inlen, unsigned char *out, unsigned long *outlen)
+{
+    hash_state *md;
+    int err;
+
+    LTC_ARGCHK(in     != NULL);
+    LTC_ARGCHK(out    != NULL);
+    LTC_ARGCHK(outlen != NULL);
+
+    if ((err = hash_is_valid(hash)) != CRYPT_OK) {
+        return err;
+    }
+
+    if (*outlen < hash_descriptor[hash]->hashsize) {
+       *outlen = hash_descriptor[hash]->hashsize;
+       return CRYPT_BUFFER_OVERFLOW;
+    }
+
+    md = XMALLOC(sizeof(hash_state));
+    if (md == NULL) {
+       return CRYPT_MEM;
+    }
+
+    if ((err = hash_descriptor[hash]->init(md)) != CRYPT_OK) {
+       goto LBL_ERR;
+    }
+    if ((err = hash_descriptor[hash]->process(md, in, inlen)) != CRYPT_OK) {
+       goto LBL_ERR;
+    }
+    err = hash_descriptor[hash]->done(md, out);
+    *outlen = hash_descriptor[hash]->hashsize;
+LBL_ERR:
+#ifdef LTC_CLEAN_STACK
+    zeromem(md, sizeof(hash_state));
+#endif
+    XFREE(md);
+
+    return err;
+}
+
+/* $Source: /cvs/libtom/libtomcrypt/src/hashes/helper/hash_memory.c,v $ */
+/* $Revision: 1.6 $ */
+/* $Date: 2006/12/28 01:27:23 $ */
diff --git a/core/lib/libtomcrypt/src/hashes/helper/hash_memory_multi.c b/core/lib/libtomcrypt/src/hashes/helper/hash_memory_multi.c
new file mode 100644
index 0000000..4488b8f
--- /dev/null
+++ b/core/lib/libtomcrypt/src/hashes/helper/hash_memory_multi.c
@@ -0,0 +1,114 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+#include <stdarg.h>
+/**
+  @file hash_memory_multi.c
+  Hash (multiple buffers) memory helper, Tom St Denis
+*/
+
+/**
+  Hash multiple (non-adjacent) blocks of memory at once.  
+  @param hash   The index of the hash you wish to use
+  @param out    [out] Where to store the digest
+  @param outlen [in/out] Max size and resulting size of the digest
+  @param in     The data you wish to hash
+  @param inlen  The length of the data to hash (octets)
+  @param ...    tuples of (data,len) pairs to hash, terminated with a (NULL,x) (x=don't care)
+  @return CRYPT_OK if successful
+*/  
+int hash_memory_multi(int hash, unsigned char *out, unsigned long *outlen,
+                      const unsigned char *in, unsigned long inlen, ...)
+{
+    hash_state          *md;
+    int                  err;
+    va_list              args;
+    const unsigned char *curptr;
+    unsigned long        curlen;
+
+    LTC_ARGCHK(in     != NULL);
+    LTC_ARGCHK(out    != NULL);
+    LTC_ARGCHK(outlen != NULL);
+
+    if ((err = hash_is_valid(hash)) != CRYPT_OK) {
+        return err;
+    }
+
+    if (*outlen < hash_descriptor[hash]->hashsize) {
+       *outlen = hash_descriptor[hash]->hashsize;
+       return CRYPT_BUFFER_OVERFLOW;
+    }
+
+    md = XMALLOC(sizeof(hash_state));
+    if (md == NULL) {
+       return CRYPT_MEM;
+    }
+
+    if ((err = hash_descriptor[hash]->init(md)) != CRYPT_OK) {
+       goto LBL_ERR;
+    }
+
+    va_start(args, inlen);
+    curptr = in; 
+    curlen = inlen;
+    for (;;) {
+       /* process buf */
+       if ((err = hash_descriptor[hash]->process(md, curptr, curlen)) != CRYPT_OK) {
+          goto LBL_ERR;
+       }
+       /* step to next */
+       curptr = va_arg(args, const unsigned char*);
+       if (curptr == NULL) {
+          break;
+       }
+       curlen = va_arg(args, unsigned long);
+    }
+    err = hash_descriptor[hash]->done(md, out);
+    *outlen = hash_descriptor[hash]->hashsize;
+LBL_ERR:
+#ifdef LTC_CLEAN_STACK
+    zeromem(md, sizeof(hash_state));
+#endif
+    XFREE(md);
+    va_end(args);
+    return err;
+}
+
+/* $Source: /cvs/libtom/libtomcrypt/src/hashes/helper/hash_memory_multi.c,v $ */
+/* $Revision: 1.6 $ */
+/* $Date: 2006/12/28 01:27:23 $ */
diff --git a/core/lib/libtomcrypt/src/hashes/helper/sub.mk b/core/lib/libtomcrypt/src/hashes/helper/sub.mk
new file mode 100644
index 0000000..8e0a971
--- /dev/null
+++ b/core/lib/libtomcrypt/src/hashes/helper/sub.mk
@@ -0,0 +1,6 @@
+cflags-y += -Wno-unused-parameter
+
+# srcs-y += hash_file.c
+# srcs-y += hash_filehandle.c
+srcs-y += hash_memory.c
+srcs-y += hash_memory_multi.c
diff --git a/core/lib/libtomcrypt/src/hashes/md5.c b/core/lib/libtomcrypt/src/hashes/md5.c
new file mode 100644
index 0000000..de6de16
--- /dev/null
+++ b/core/lib/libtomcrypt/src/hashes/md5.c
@@ -0,0 +1,395 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+
+/**
+  @file md5.c
+  LTC_MD5 hash function by Tom St Denis 
+*/
+
+#ifdef LTC_MD5
+
+const struct ltc_hash_descriptor md5_desc =
+{
+    "md5",
+    3,
+    16,
+    64,
+
+    /* OID */
+   { 1, 2, 840, 113549, 2, 5,  },
+   6,
+
+    &md5_init,
+    &md5_process,
+    &md5_done,
+    &md5_test,
+    NULL
+};
+
+#define F(x,y,z)  (z ^ (x & (y ^ z)))
+#define G(x,y,z)  (y ^ (z & (y ^ x)))
+#define H(x,y,z)  (x^y^z)
+#define I(x,y,z)  (y^(x|(~z)))
+
+#ifdef LTC_SMALL_CODE
+
+#define FF(a,b,c,d,M,s,t) \
+    a = (a + F(b,c,d) + M + t); a = ROL(a, s) + b;
+
+#define GG(a,b,c,d,M,s,t) \
+    a = (a + G(b,c,d) + M + t); a = ROL(a, s) + b;
+
+#define HH(a,b,c,d,M,s,t) \
+    a = (a + H(b,c,d) + M + t); a = ROL(a, s) + b;
+
+#define II(a,b,c,d,M,s,t) \
+    a = (a + I(b,c,d) + M + t); a = ROL(a, s) + b;
+
+static const unsigned char Worder[64] = {
+   0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,
+   1,6,11,0,5,10,15,4,9,14,3,8,13,2,7,12,
+   5,8,11,14,1,4,7,10,13,0,3,6,9,12,15,2,
+   0,7,14,5,12,3,10,1,8,15,6,13,4,11,2,9
+};
+
+static const unsigned char Rorder[64] = {
+   7,12,17,22,7,12,17,22,7,12,17,22,7,12,17,22,
+   5,9,14,20,5,9,14,20,5,9,14,20,5,9,14,20,
+   4,11,16,23,4,11,16,23,4,11,16,23,4,11,16,23,
+   6,10,15,21,6,10,15,21,6,10,15,21,6,10,15,21
+};
+
+static const ulong32 Korder[64] = {
+0xd76aa478UL, 0xe8c7b756UL, 0x242070dbUL, 0xc1bdceeeUL, 0xf57c0fafUL, 0x4787c62aUL, 0xa8304613UL, 0xfd469501UL,
+0x698098d8UL, 0x8b44f7afUL, 0xffff5bb1UL, 0x895cd7beUL, 0x6b901122UL, 0xfd987193UL, 0xa679438eUL, 0x49b40821UL,
+0xf61e2562UL, 0xc040b340UL, 0x265e5a51UL, 0xe9b6c7aaUL, 0xd62f105dUL, 0x02441453UL, 0xd8a1e681UL, 0xe7d3fbc8UL,
+0x21e1cde6UL, 0xc33707d6UL, 0xf4d50d87UL, 0x455a14edUL, 0xa9e3e905UL, 0xfcefa3f8UL, 0x676f02d9UL, 0x8d2a4c8aUL,
+0xfffa3942UL, 0x8771f681UL, 0x6d9d6122UL, 0xfde5380cUL, 0xa4beea44UL, 0x4bdecfa9UL, 0xf6bb4b60UL, 0xbebfbc70UL,
+0x289b7ec6UL, 0xeaa127faUL, 0xd4ef3085UL, 0x04881d05UL, 0xd9d4d039UL, 0xe6db99e5UL, 0x1fa27cf8UL, 0xc4ac5665UL,
+0xf4292244UL, 0x432aff97UL, 0xab9423a7UL, 0xfc93a039UL, 0x655b59c3UL, 0x8f0ccc92UL, 0xffeff47dUL, 0x85845dd1UL,
+0x6fa87e4fUL, 0xfe2ce6e0UL, 0xa3014314UL, 0x4e0811a1UL, 0xf7537e82UL, 0xbd3af235UL, 0x2ad7d2bbUL, 0xeb86d391UL
+};
+
+#else
+
+#define FF(a,b,c,d,M,s,t) \
+    a = (a + F(b,c,d) + M + t); a = ROLc(a, s) + b;
+
+#define GG(a,b,c,d,M,s,t) \
+    a = (a + G(b,c,d) + M + t); a = ROLc(a, s) + b;
+
+#define HH(a,b,c,d,M,s,t) \
+    a = (a + H(b,c,d) + M + t); a = ROLc(a, s) + b;
+
+#define II(a,b,c,d,M,s,t) \
+    a = (a + I(b,c,d) + M + t); a = ROLc(a, s) + b;
+
+
+#endif   
+
+#ifdef LTC_CLEAN_STACK
+static int _md5_compress(hash_state *md, unsigned char *buf)
+#else
+static int  md5_compress(hash_state *md, unsigned char *buf)
+#endif
+{
+    ulong32 i, W[16], a, b, c, d;
+#ifdef LTC_SMALL_CODE
+    ulong32 t;
+#endif
+
+    /* copy the state into 512-bits into W[0..15] */
+    for (i = 0; i < 16; i++) {
+        LOAD32L(W[i], buf + (4*i));
+    }
+ 
+    /* copy state */
+    a = md->md5.state[0];
+    b = md->md5.state[1];
+    c = md->md5.state[2];
+    d = md->md5.state[3];
+
+#ifdef LTC_SMALL_CODE
+    for (i = 0; i < 16; ++i) {
+        FF(a,b,c,d,W[Worder[i]],Rorder[i],Korder[i]);
+        t = d; d = c; c = b; b = a; a = t;
+    }
+
+    for (; i < 32; ++i) {
+        GG(a,b,c,d,W[Worder[i]],Rorder[i],Korder[i]);
+        t = d; d = c; c = b; b = a; a = t;
+    }
+
+    for (; i < 48; ++i) {
+        HH(a,b,c,d,W[Worder[i]],Rorder[i],Korder[i]);
+        t = d; d = c; c = b; b = a; a = t;
+    }
+
+    for (; i < 64; ++i) {
+        II(a,b,c,d,W[Worder[i]],Rorder[i],Korder[i]);
+        t = d; d = c; c = b; b = a; a = t;
+    }
+
+#else
+    FF(a,b,c,d,W[0],7,0xd76aa478UL)
+    FF(d,a,b,c,W[1],12,0xe8c7b756UL)
+    FF(c,d,a,b,W[2],17,0x242070dbUL)
+    FF(b,c,d,a,W[3],22,0xc1bdceeeUL)
+    FF(a,b,c,d,W[4],7,0xf57c0fafUL)
+    FF(d,a,b,c,W[5],12,0x4787c62aUL)
+    FF(c,d,a,b,W[6],17,0xa8304613UL)
+    FF(b,c,d,a,W[7],22,0xfd469501UL)
+    FF(a,b,c,d,W[8],7,0x698098d8UL)
+    FF(d,a,b,c,W[9],12,0x8b44f7afUL)
+    FF(c,d,a,b,W[10],17,0xffff5bb1UL)
+    FF(b,c,d,a,W[11],22,0x895cd7beUL)
+    FF(a,b,c,d,W[12],7,0x6b901122UL)
+    FF(d,a,b,c,W[13],12,0xfd987193UL)
+    FF(c,d,a,b,W[14],17,0xa679438eUL)
+    FF(b,c,d,a,W[15],22,0x49b40821UL)
+    GG(a,b,c,d,W[1],5,0xf61e2562UL)
+    GG(d,a,b,c,W[6],9,0xc040b340UL)
+    GG(c,d,a,b,W[11],14,0x265e5a51UL)
+    GG(b,c,d,a,W[0],20,0xe9b6c7aaUL)
+    GG(a,b,c,d,W[5],5,0xd62f105dUL)
+    GG(d,a,b,c,W[10],9,0x02441453UL)
+    GG(c,d,a,b,W[15],14,0xd8a1e681UL)
+    GG(b,c,d,a,W[4],20,0xe7d3fbc8UL)
+    GG(a,b,c,d,W[9],5,0x21e1cde6UL)
+    GG(d,a,b,c,W[14],9,0xc33707d6UL)
+    GG(c,d,a,b,W[3],14,0xf4d50d87UL)
+    GG(b,c,d,a,W[8],20,0x455a14edUL)
+    GG(a,b,c,d,W[13],5,0xa9e3e905UL)
+    GG(d,a,b,c,W[2],9,0xfcefa3f8UL)
+    GG(c,d,a,b,W[7],14,0x676f02d9UL)
+    GG(b,c,d,a,W[12],20,0x8d2a4c8aUL)
+    HH(a,b,c,d,W[5],4,0xfffa3942UL)
+    HH(d,a,b,c,W[8],11,0x8771f681UL)
+    HH(c,d,a,b,W[11],16,0x6d9d6122UL)
+    HH(b,c,d,a,W[14],23,0xfde5380cUL)
+    HH(a,b,c,d,W[1],4,0xa4beea44UL)
+    HH(d,a,b,c,W[4],11,0x4bdecfa9UL)
+    HH(c,d,a,b,W[7],16,0xf6bb4b60UL)
+    HH(b,c,d,a,W[10],23,0xbebfbc70UL)
+    HH(a,b,c,d,W[13],4,0x289b7ec6UL)
+    HH(d,a,b,c,W[0],11,0xeaa127faUL)
+    HH(c,d,a,b,W[3],16,0xd4ef3085UL)
+    HH(b,c,d,a,W[6],23,0x04881d05UL)
+    HH(a,b,c,d,W[9],4,0xd9d4d039UL)
+    HH(d,a,b,c,W[12],11,0xe6db99e5UL)
+    HH(c,d,a,b,W[15],16,0x1fa27cf8UL)
+    HH(b,c,d,a,W[2],23,0xc4ac5665UL)
+    II(a,b,c,d,W[0],6,0xf4292244UL)
+    II(d,a,b,c,W[7],10,0x432aff97UL)
+    II(c,d,a,b,W[14],15,0xab9423a7UL)
+    II(b,c,d,a,W[5],21,0xfc93a039UL)
+    II(a,b,c,d,W[12],6,0x655b59c3UL)
+    II(d,a,b,c,W[3],10,0x8f0ccc92UL)
+    II(c,d,a,b,W[10],15,0xffeff47dUL)
+    II(b,c,d,a,W[1],21,0x85845dd1UL)
+    II(a,b,c,d,W[8],6,0x6fa87e4fUL)
+    II(d,a,b,c,W[15],10,0xfe2ce6e0UL)
+    II(c,d,a,b,W[6],15,0xa3014314UL)
+    II(b,c,d,a,W[13],21,0x4e0811a1UL)
+    II(a,b,c,d,W[4],6,0xf7537e82UL)
+    II(d,a,b,c,W[11],10,0xbd3af235UL)
+    II(c,d,a,b,W[2],15,0x2ad7d2bbUL)
+    II(b,c,d,a,W[9],21,0xeb86d391UL)
+#endif
+
+    md->md5.state[0] = md->md5.state[0] + a;
+    md->md5.state[1] = md->md5.state[1] + b;
+    md->md5.state[2] = md->md5.state[2] + c;
+    md->md5.state[3] = md->md5.state[3] + d;
+
+    return CRYPT_OK;
+}
+
+#ifdef LTC_CLEAN_STACK
+static int md5_compress(hash_state *md, unsigned char *buf)
+{
+   int err;
+   err = _md5_compress(md, buf);
+   burn_stack(sizeof(ulong32) * 21);
+   return err;
+}
+#endif
+
+/**
+   Initialize the hash state
+   @param md   The hash state you wish to initialize
+   @return CRYPT_OK if successful
+*/
+int md5_init(hash_state * md)
+{
+   LTC_ARGCHK(md != NULL);
+   md->md5.state[0] = 0x67452301UL;
+   md->md5.state[1] = 0xefcdab89UL;
+   md->md5.state[2] = 0x98badcfeUL;
+   md->md5.state[3] = 0x10325476UL;
+   md->md5.curlen = 0;
+   md->md5.length = 0;
+   return CRYPT_OK;
+}
+
+/**
+   Process a block of memory though the hash
+   @param md     The hash state
+   @param in     The data to hash
+   @param inlen  The length of the data (octets)
+   @return CRYPT_OK if successful
+*/
+HASH_PROCESS(md5_process, md5_compress, md5, 64)
+
+/**
+   Terminate the hash to get the digest
+   @param md  The hash state
+   @param out [out] The destination of the hash (16 bytes)
+   @return CRYPT_OK if successful
+*/
+int md5_done(hash_state * md, unsigned char *out)
+{
+    int i;
+
+    LTC_ARGCHK(md  != NULL);
+    LTC_ARGCHK(out != NULL);
+
+    if (md->md5.curlen >= sizeof(md->md5.buf)) {
+       return CRYPT_INVALID_ARG;
+    }
+
+
+    /* increase the length of the message */
+    md->md5.length += md->md5.curlen * 8;
+
+    /* append the '1' bit */
+    md->md5.buf[md->md5.curlen++] = (unsigned char)0x80;
+
+    /* if the length is currently above 56 bytes we append zeros
+     * then compress.  Then we can fall back to padding zeros and length
+     * encoding like normal.
+     */
+    if (md->md5.curlen > 56) {
+        while (md->md5.curlen < 64) {
+            md->md5.buf[md->md5.curlen++] = (unsigned char)0;
+        }
+        md5_compress(md, md->md5.buf);
+        md->md5.curlen = 0;
+    }
+
+    /* pad upto 56 bytes of zeroes */
+    while (md->md5.curlen < 56) {
+        md->md5.buf[md->md5.curlen++] = (unsigned char)0;
+    }
+
+    /* store length */
+    STORE64L(md->md5.length, md->md5.buf+56);
+    md5_compress(md, md->md5.buf);
+
+    /* copy output */
+    for (i = 0; i < 4; i++) {
+        STORE32L(md->md5.state[i], out+(4*i));
+    }
+#ifdef LTC_CLEAN_STACK
+    zeromem(md, sizeof(hash_state));
+#endif
+    return CRYPT_OK;
+}
+
+/**
+  Self-test the hash
+  @return CRYPT_OK if successful, CRYPT_NOP if self-tests have been disabled
+*/  
+int  md5_test(void)
+{
+ #ifndef LTC_TEST
+    return CRYPT_NOP;
+ #else    
+  static const struct {
+      const char *msg;
+      unsigned char hash[16];
+  } tests[] = {
+    { "",
+      { 0xd4, 0x1d, 0x8c, 0xd9, 0x8f, 0x00, 0xb2, 0x04, 
+        0xe9, 0x80, 0x09, 0x98, 0xec, 0xf8, 0x42, 0x7e } },
+    { "a",
+      {0x0c, 0xc1, 0x75, 0xb9, 0xc0, 0xf1, 0xb6, 0xa8, 
+       0x31, 0xc3, 0x99, 0xe2, 0x69, 0x77, 0x26, 0x61 } },
+    { "abc",
+      { 0x90, 0x01, 0x50, 0x98, 0x3c, 0xd2, 0x4f, 0xb0, 
+        0xd6, 0x96, 0x3f, 0x7d, 0x28, 0xe1, 0x7f, 0x72 } },
+    { "message digest", 
+      { 0xf9, 0x6b, 0x69, 0x7d, 0x7c, 0xb7, 0x93, 0x8d, 
+        0x52, 0x5a, 0x2f, 0x31, 0xaa, 0xf1, 0x61, 0xd0 } }, 
+    { "abcdefghijklmnopqrstuvwxyz",
+      { 0xc3, 0xfc, 0xd3, 0xd7, 0x61, 0x92, 0xe4, 0x00, 
+        0x7d, 0xfb, 0x49, 0x6c, 0xca, 0x67, 0xe1, 0x3b } },
+    { "ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz0123456789",
+      { 0xd1, 0x74, 0xab, 0x98, 0xd2, 0x77, 0xd9, 0xf5, 
+        0xa5, 0x61, 0x1c, 0x2c, 0x9f, 0x41, 0x9d, 0x9f } },
+    { "12345678901234567890123456789012345678901234567890123456789012345678901234567890",
+      { 0x57, 0xed, 0xf4, 0xa2, 0x2b, 0xe3, 0xc9, 0x55, 
+        0xac, 0x49, 0xda, 0x2e, 0x21, 0x07, 0xb6, 0x7a } }, 
+    { NULL, { 0 } }
+  };
+
+  int i;
+  unsigned char tmp[16];
+  hash_state md;
+
+  for (i = 0; tests[i].msg != NULL; i++) {
+      md5_init(&md);
+      md5_process(&md, (unsigned char *)tests[i].msg, (unsigned long)strlen(tests[i].msg));
+      md5_done(&md, tmp);
+      if (XMEMCMP(tmp, tests[i].hash, 16) != 0) {
+         return CRYPT_FAIL_TESTVECTOR;
+      }
+  }
+  return CRYPT_OK;
+ #endif
+}
+
+#endif
+
+
+
+/* $Source: /cvs/libtom/libtomcrypt/src/hashes/md5.c,v $ */
+/* $Revision: 1.10 $ */
+/* $Date: 2007/05/12 14:25:28 $ */
diff --git a/core/lib/libtomcrypt/src/hashes/sha1.c b/core/lib/libtomcrypt/src/hashes/sha1.c
new file mode 100644
index 0000000..d82d4d0
--- /dev/null
+++ b/core/lib/libtomcrypt/src/hashes/sha1.c
@@ -0,0 +1,315 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file sha1.c
+  LTC_SHA1 code by Tom St Denis 
+*/
+
+
+#ifdef LTC_SHA1
+
+const struct ltc_hash_descriptor sha1_desc =
+{
+    "sha1",
+    2,
+    20,
+    64,
+
+    /* OID */
+   { 1, 3, 14, 3, 2, 26,  },
+   6,
+
+    &sha1_init,
+    &sha1_process,
+    &sha1_done,
+    &sha1_test,
+    NULL
+};
+
+#define F0(x,y,z)  (z ^ (x & (y ^ z)))
+#define F1(x,y,z)  (x ^ y ^ z)
+#define F2(x,y,z)  ((x & y) | (z & (x | y)))
+#define F3(x,y,z)  (x ^ y ^ z)
+
+#ifdef LTC_CLEAN_STACK
+static int _sha1_compress(hash_state *md, unsigned char *buf)
+#else
+static int  sha1_compress(hash_state *md, unsigned char *buf)
+#endif
+{
+    ulong32 a,b,c,d,e,W[80],i;
+#ifdef LTC_SMALL_CODE
+    ulong32 t;
+#endif
+
+    /* copy the state into 512-bits into W[0..15] */
+    for (i = 0; i < 16; i++) {
+        LOAD32H(W[i], buf + (4*i));
+    }
+
+    /* copy state */
+    a = md->sha1.state[0];
+    b = md->sha1.state[1];
+    c = md->sha1.state[2];
+    d = md->sha1.state[3];
+    e = md->sha1.state[4];
+
+    /* expand it */
+    for (i = 16; i < 80; i++) {
+        W[i] = ROL(W[i-3] ^ W[i-8] ^ W[i-14] ^ W[i-16], 1); 
+    }
+
+    /* compress */
+    /* round one */
+    #define FF0(a,b,c,d,e,i) e = (ROLc(a, 5) + F0(b,c,d) + e + W[i] + 0x5a827999UL); b = ROLc(b, 30);
+    #define FF1(a,b,c,d,e,i) e = (ROLc(a, 5) + F1(b,c,d) + e + W[i] + 0x6ed9eba1UL); b = ROLc(b, 30);
+    #define FF2(a,b,c,d,e,i) e = (ROLc(a, 5) + F2(b,c,d) + e + W[i] + 0x8f1bbcdcUL); b = ROLc(b, 30);
+    #define FF3(a,b,c,d,e,i) e = (ROLc(a, 5) + F3(b,c,d) + e + W[i] + 0xca62c1d6UL); b = ROLc(b, 30);
+ 
+#ifdef LTC_SMALL_CODE
+ 
+    for (i = 0; i < 20; ) {
+       FF0(a,b,c,d,e,i++); t = e; e = d; d = c; c = b; b = a; a = t;
+    }
+
+    for (; i < 40; ) {
+       FF1(a,b,c,d,e,i++); t = e; e = d; d = c; c = b; b = a; a = t;
+    }
+
+    for (; i < 60; ) {
+       FF2(a,b,c,d,e,i++); t = e; e = d; d = c; c = b; b = a; a = t;
+    }
+
+    for (; i < 80; ) {
+       FF3(a,b,c,d,e,i++); t = e; e = d; d = c; c = b; b = a; a = t;
+    }
+
+#else
+
+    for (i = 0; i < 20; ) {
+       FF0(a,b,c,d,e,i++);
+       FF0(e,a,b,c,d,i++);
+       FF0(d,e,a,b,c,i++);
+       FF0(c,d,e,a,b,i++);
+       FF0(b,c,d,e,a,i++);
+    }
+
+    /* round two */
+    for (; i < 40; )  { 
+       FF1(a,b,c,d,e,i++);
+       FF1(e,a,b,c,d,i++);
+       FF1(d,e,a,b,c,i++);
+       FF1(c,d,e,a,b,i++);
+       FF1(b,c,d,e,a,i++);
+    }
+
+    /* round three */
+    for (; i < 60; )  { 
+       FF2(a,b,c,d,e,i++);
+       FF2(e,a,b,c,d,i++);
+       FF2(d,e,a,b,c,i++);
+       FF2(c,d,e,a,b,i++);
+       FF2(b,c,d,e,a,i++);
+    }
+
+    /* round four */
+    for (; i < 80; )  { 
+       FF3(a,b,c,d,e,i++);
+       FF3(e,a,b,c,d,i++);
+       FF3(d,e,a,b,c,i++);
+       FF3(c,d,e,a,b,i++);
+       FF3(b,c,d,e,a,i++);
+    }
+#endif
+
+    #undef FF0
+    #undef FF1
+    #undef FF2
+    #undef FF3
+
+    /* store */
+    md->sha1.state[0] = md->sha1.state[0] + a;
+    md->sha1.state[1] = md->sha1.state[1] + b;
+    md->sha1.state[2] = md->sha1.state[2] + c;
+    md->sha1.state[3] = md->sha1.state[3] + d;
+    md->sha1.state[4] = md->sha1.state[4] + e;
+
+    return CRYPT_OK;
+}
+
+#ifdef LTC_CLEAN_STACK
+static int sha1_compress(hash_state *md, unsigned char *buf)
+{
+   int err;
+   err = _sha1_compress(md, buf);
+   burn_stack(sizeof(ulong32) * 87);
+   return err;
+}
+#endif
+
+/**
+   Initialize the hash state
+   @param md   The hash state you wish to initialize
+   @return CRYPT_OK if successful
+*/
+int sha1_init(hash_state * md)
+{
+   LTC_ARGCHK(md != NULL);
+   md->sha1.state[0] = 0x67452301UL;
+   md->sha1.state[1] = 0xefcdab89UL;
+   md->sha1.state[2] = 0x98badcfeUL;
+   md->sha1.state[3] = 0x10325476UL;
+   md->sha1.state[4] = 0xc3d2e1f0UL;
+   md->sha1.curlen = 0;
+   md->sha1.length = 0;
+   return CRYPT_OK;
+}
+
+/**
+   Process a block of memory though the hash
+   @param md     The hash state
+   @param in     The data to hash
+   @param inlen  The length of the data (octets)
+   @return CRYPT_OK if successful
+*/
+HASH_PROCESS(sha1_process, sha1_compress, sha1, 64)
+
+/**
+   Terminate the hash to get the digest
+   @param md  The hash state
+   @param out [out] The destination of the hash (20 bytes)
+   @return CRYPT_OK if successful
+*/
+int sha1_done(hash_state * md, unsigned char *out)
+{
+    int i;
+
+    LTC_ARGCHK(md  != NULL);
+    LTC_ARGCHK(out != NULL);
+
+    if (md->sha1.curlen >= sizeof(md->sha1.buf)) {
+       return CRYPT_INVALID_ARG;
+    }
+
+    /* increase the length of the message */
+    md->sha1.length += md->sha1.curlen * 8;
+
+    /* append the '1' bit */
+    md->sha1.buf[md->sha1.curlen++] = (unsigned char)0x80;
+
+    /* if the length is currently above 56 bytes we append zeros
+     * then compress.  Then we can fall back to padding zeros and length
+     * encoding like normal.
+     */
+    if (md->sha1.curlen > 56) {
+        while (md->sha1.curlen < 64) {
+            md->sha1.buf[md->sha1.curlen++] = (unsigned char)0;
+        }
+        sha1_compress(md, md->sha1.buf);
+        md->sha1.curlen = 0;
+    }
+
+    /* pad upto 56 bytes of zeroes */
+    while (md->sha1.curlen < 56) {
+        md->sha1.buf[md->sha1.curlen++] = (unsigned char)0;
+    }
+
+    /* store length */
+    STORE64H(md->sha1.length, md->sha1.buf+56);
+    sha1_compress(md, md->sha1.buf);
+
+    /* copy output */
+    for (i = 0; i < 5; i++) {
+        STORE32H(md->sha1.state[i], out+(4*i));
+    }
+#ifdef LTC_CLEAN_STACK
+    zeromem(md, sizeof(hash_state));
+#endif
+    return CRYPT_OK;
+}
+
+/**
+  Self-test the hash
+  @return CRYPT_OK if successful, CRYPT_NOP if self-tests have been disabled
+*/  
+int  sha1_test(void)
+{
+ #ifndef LTC_TEST
+    return CRYPT_NOP;
+ #else    
+  static const struct {
+      const char *msg;
+      unsigned char hash[20];
+  } tests[] = {
+    { "abc",
+      { 0xa9, 0x99, 0x3e, 0x36, 0x47, 0x06, 0x81, 0x6a,
+        0xba, 0x3e, 0x25, 0x71, 0x78, 0x50, 0xc2, 0x6c,
+        0x9c, 0xd0, 0xd8, 0x9d }
+    },
+    { "abcdbcdecdefdefgefghfghighijhijkijkljklmklmnlmnomnopnopq",
+      { 0x84, 0x98, 0x3E, 0x44, 0x1C, 0x3B, 0xD2, 0x6E,
+        0xBA, 0xAE, 0x4A, 0xA1, 0xF9, 0x51, 0x29, 0xE5,
+        0xE5, 0x46, 0x70, 0xF1 }
+    }
+  };
+
+  int i;
+  unsigned char tmp[20];
+  hash_state md;
+
+  for (i = 0; i < (int)(sizeof(tests) / sizeof(tests[0]));  i++) {
+      sha1_init(&md);
+      sha1_process(&md, (unsigned char*)tests[i].msg, (unsigned long)strlen(tests[i].msg));
+      sha1_done(&md, tmp);
+      if (XMEMCMP(tmp, tests[i].hash, 20) != 0) {
+         return CRYPT_FAIL_TESTVECTOR;
+      }
+  }
+  return CRYPT_OK;
+  #endif
+}
+
+#endif
+
+
+
+/* $Source: /cvs/libtom/libtomcrypt/src/hashes/sha1.c,v $ */
+/* $Revision: 1.10 $ */
+/* $Date: 2007/05/12 14:25:28 $ */
diff --git a/core/lib/libtomcrypt/src/hashes/sha1_armv8a_ce.c b/core/lib/libtomcrypt/src/hashes/sha1_armv8a_ce.c
new file mode 100644
index 0000000..e19aa4a
--- /dev/null
+++ b/core/lib/libtomcrypt/src/hashes/sha1_armv8a_ce.c
@@ -0,0 +1,207 @@
+/*
+ * Copyright (c) 2015, Linaro Limited
+ * All rights reserved.
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+#include "tomcrypt_arm_neon.h"
+
+#if defined(LTC_SHA1_ARM32_CE) || defined(LTC_SHA1_ARM64_CE)
+
+const struct ltc_hash_descriptor sha1_desc =
+{
+    "sha1",
+    2,
+    20,
+    64,
+
+    /* OID */
+   { 1, 3, 14, 3, 2, 26,  },
+   6,
+
+    &sha1_init,
+    &sha1_process,
+    &sha1_done,
+    &sha1_test,
+    NULL
+};
+
+
+/* Implemented in assembly */
+void sha1_ce_transform(ulong32 *state, unsigned char *src, int blocks);
+
+static int sha1_compress_nblocks(hash_state *md, unsigned char *buf, int blocks)
+{
+   struct tomcrypt_arm_neon_state state;
+
+   /* sha1_ce_transform() assumes this */
+   COMPILE_TIME_ASSERT(sizeof(md->sha1.state[0]) == 4);
+
+   tomcrypt_arm_neon_enable(&state);
+   sha1_ce_transform(md->sha1.state, buf, blocks);
+   tomcrypt_arm_neon_disable(&state);
+   return CRYPT_OK;
+}
+
+static int sha1_compress(hash_state *md, unsigned char *buf)
+{
+   return sha1_compress_nblocks(md, buf, 1);
+}
+
+/**
+   Process a block of memory though the hash
+   @param md     The hash state
+   @param in     The data to hash
+   @param inlen  The length of the data (octets)
+   @return CRYPT_OK if successful
+*/
+HASH_PROCESS_NBLOCKS(sha1_process, sha1_compress_nblocks, sha1, 64)
+
+/**
+   Initialize the hash state
+   @param md   The hash state you wish to initialize
+   @return CRYPT_OK if successful
+*/
+int sha1_init(hash_state * md)
+{
+   LTC_ARGCHK(md != NULL);
+   md->sha1.state[0] = 0x67452301UL;
+   md->sha1.state[1] = 0xefcdab89UL;
+   md->sha1.state[2] = 0x98badcfeUL;
+   md->sha1.state[3] = 0x10325476UL;
+   md->sha1.state[4] = 0xc3d2e1f0UL;
+   md->sha1.curlen = 0;
+   md->sha1.length = 0;
+   return CRYPT_OK;
+}
+
+/**
+   Terminate the hash to get the digest
+   @param md  The hash state
+   @param out [out] The destination of the hash (20 bytes)
+   @return CRYPT_OK if successful
+*/
+int sha1_done(hash_state * md, unsigned char *out)
+{
+    int i;
+
+    LTC_ARGCHK(md  != NULL);
+    LTC_ARGCHK(out != NULL);
+
+    if (md->sha1.curlen >= sizeof(md->sha1.buf)) {
+       return CRYPT_INVALID_ARG;
+    }
+
+    /* increase the length of the message */
+    md->sha1.length += md->sha1.curlen * 8;
+
+    /* append the '1' bit */
+    md->sha1.buf[md->sha1.curlen++] = (unsigned char)0x80;
+
+    /* if the length is currently above 56 bytes we append zeros
+     * then compress.  Then we can fall back to padding zeros and length
+     * encoding like normal.
+     */
+    if (md->sha1.curlen > 56) {
+        while (md->sha1.curlen < 64) {
+            md->sha1.buf[md->sha1.curlen++] = (unsigned char)0;
+        }
+        sha1_compress(md, md->sha1.buf);
+        md->sha1.curlen = 0;
+    }
+
+    /* pad upto 56 bytes of zeroes */
+    while (md->sha1.curlen < 56) {
+        md->sha1.buf[md->sha1.curlen++] = (unsigned char)0;
+    }
+
+    /* store length */
+    STORE64H(md->sha1.length, md->sha1.buf+56);
+    sha1_compress(md, md->sha1.buf);
+
+    /* copy output */
+    for (i = 0; i < 5; i++) {
+        STORE32H(md->sha1.state[i], out+(4*i));
+    }
+#ifdef LTC_CLEAN_STACK
+    zeromem(md, sizeof(hash_state));
+#endif
+    return CRYPT_OK;
+}
+
+/**
+  Self-test the hash
+  @return CRYPT_OK if successful, CRYPT_NOP if self-tests have been disabled
+*/
+int  sha1_test(void)
+{
+ #ifndef LTC_TEST
+    return CRYPT_NOP;
+ #else
+  static const struct {
+      const char *msg;
+      unsigned char hash[20];
+  } tests[] = {
+    { "abc",
+      { 0xa9, 0x99, 0x3e, 0x36, 0x47, 0x06, 0x81, 0x6a,
+        0xba, 0x3e, 0x25, 0x71, 0x78, 0x50, 0xc2, 0x6c,
+        0x9c, 0xd0, 0xd8, 0x9d }
+    },
+    { "abcdbcdecdefdefgefghfghighijhijkijkljklmklmnlmnomnopnopq",
+      { 0x84, 0x98, 0x3E, 0x44, 0x1C, 0x3B, 0xD2, 0x6E,
+        0xBA, 0xAE, 0x4A, 0xA1, 0xF9, 0x51, 0x29, 0xE5,
+        0xE5, 0x46, 0x70, 0xF1 }
+    }
+  };
+
+  int i;
+  unsigned char tmp[20];
+  hash_state md;
+
+  for (i = 0; i < (int)(sizeof(tests) / sizeof(tests[0]));  i++) {
+      sha1_init(&md);
+      sha1_process(&md, (unsigned char*)tests[i].msg, (unsigned long)strlen(tests[i].msg));
+      sha1_done(&md, tmp);
+      if (XMEMCMP(tmp, tests[i].hash, 20) != 0) {
+         return CRYPT_FAIL_TESTVECTOR;
+      }
+  }
+  return CRYPT_OK;
+  #endif
+}
+
+#endif
diff --git a/core/lib/libtomcrypt/src/hashes/sha1_armv8a_ce_a32.S b/core/lib/libtomcrypt/src/hashes/sha1_armv8a_ce_a32.S
new file mode 100644
index 0000000..9ed81b3
--- /dev/null
+++ b/core/lib/libtomcrypt/src/hashes/sha1_armv8a_ce_a32.S
@@ -0,0 +1,135 @@
+/*
+ * Copyright (c) 2014-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.
+ */
+
+/* SHA-1 secure hash using ARMv8 Crypto Extensions */
+
+	.text
+	.fpu		crypto-neon-fp-armv8
+
+	k0		.req	q0
+	k1		.req	q1
+	k2		.req	q2
+	k3		.req	q3
+
+	ta0		.req	q4
+	ta1		.req	q5
+	tb0		.req	q5
+	tb1		.req	q4
+
+	dga		.req	q6
+	dgb		.req	q7
+	dgbs		.req	s28
+
+	dg0		.req	q12
+
+	dg1a0		.req	q13
+	dg1a1		.req	q14
+	dg1b0		.req	q14
+	dg1b1		.req	q13
+
+	.macro		add_only, op, ev, rc, s0, dg1
+	.ifnb		\s0
+	vadd.u32	tb\ev, q\s0, \rc
+	.endif
+	sha1h.32	dg1b\ev, dg0
+	.ifb		\dg1
+	sha1\op\().32	dg0, dg1a\ev, ta\ev
+	.else
+	sha1\op\().32	dg0, \dg1, ta\ev
+	.endif
+	.endm
+
+	.macro		add_update, op, ev, rc, s0, s1, s2, s3, dg1
+	sha1su0.32	q\s0, q\s1, q\s2
+	add_only	\op, \ev, \rc, \s1, \dg1
+	sha1su1.32	q\s0, q\s3
+	.endm
+
+	.align		4
+.Lsha1_rcon:
+	.word		0x5a827999, 0x5a827999, 0x5a827999, 0x5a827999
+	.word		0x6ed9eba1, 0x6ed9eba1, 0x6ed9eba1, 0x6ed9eba1
+	.word		0x8f1bbcdc, 0x8f1bbcdc, 0x8f1bbcdc, 0x8f1bbcdc
+	.word		0xca62c1d6, 0xca62c1d6, 0xca62c1d6, 0xca62c1d6
+
+	.global		sha1_ce_transform
+	.type		sha1_ce_transform, %function
+sha1_ce_transform:
+	/* load round constants */
+	adr		r3, .Lsha1_rcon
+	vld1.32		{k0-k1}, [r3]!
+	vld1.32		{k2-k3}, [r3]
+
+	/* load state */
+	vld1.32		{dga}, [r0]
+	vldr		dgbs, [r0, #16]
+
+0:	/* load input */
+	vld1.8		{q8-q9}, [r1]!
+	vrev32.8	q8, q8
+	vrev32.8	q9, q9
+	vld1.8		{q10-q11}, [r1]!
+	vrev32.8	q10, q10
+	vrev32.8	q11, q11
+	subs		r2, r2, #1
+
+	vadd.u32	ta0, q8, k0
+	vmov		dg0, dga
+
+	add_update	c, 0, k0,  8,  9, 10, 11, dgb
+	add_update	c, 1, k0,  9, 10, 11,  8
+	add_update	c, 0, k0, 10, 11,  8,  9
+	add_update	c, 1, k0, 11,  8,  9, 10
+	add_update	c, 0, k1,  8,  9, 10, 11
+
+	add_update	p, 1, k1,  9, 10, 11,  8
+	add_update	p, 0, k1, 10, 11,  8,  9
+	add_update	p, 1, k1, 11,  8,  9, 10
+	add_update	p, 0, k1,  8,  9, 10, 11
+	add_update	p, 1, k2,  9, 10, 11,  8
+
+	add_update	m, 0, k2, 10, 11,  8,  9
+	add_update	m, 1, k2, 11,  8,  9, 10
+	add_update	m, 0, k2,  8,  9, 10, 11
+	add_update	m, 1, k2,  9, 10, 11,  8
+	add_update	m, 0, k3, 10, 11,  8,  9
+
+	add_update	p, 1, k3, 11,  8,  9, 10
+	add_only	p, 0, k3,  9
+	add_only	p, 1, k3, 10
+	add_only	p, 0, k3, 11
+	add_only	p, 1
+
+	/* update state */
+	vadd.u32	dga, dga, dg0
+	vadd.u32	dgb, dgb, dg1a0
+	bne		0b
+
+	/* store new state */
+	vst1.32		{dga}, [r0]
+	vstr		dgbs, [r0, #16]
+	bx		lr
diff --git a/core/lib/libtomcrypt/src/hashes/sha1_armv8a_ce_a64.S b/core/lib/libtomcrypt/src/hashes/sha1_armv8a_ce_a64.S
new file mode 100644
index 0000000..76091ad
--- /dev/null
+++ b/core/lib/libtomcrypt/src/hashes/sha1_armv8a_ce_a64.S
@@ -0,0 +1,158 @@
+/*
+ * Copyright (c) 2014-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.
+ */
+
+ /*
+  * SHA-1 secure hash using ARMv8 Crypto Extensions
+  *
+  * Copyright (C) 2014 Linaro Ltd <ard.biesheuvel@linaro.org>
+  */
+
+
+#define ENTRY(func) \
+	.global func ; \
+	.type func , %function ; \
+	func :
+
+#define ENDPROC(func) \
+	.size func , .-func
+
+	.text
+	.arch		armv8-a+crypto
+
+	k0		.req	v0
+	k1		.req	v1
+	k2		.req	v2
+	k3		.req	v3
+
+	t0		.req	v4
+	t1		.req	v5
+
+	dga		.req	q6
+	dgav		.req	v6
+	dgb		.req	s7
+	dgbv		.req	v7
+
+	dg0q		.req	q12
+	dg0s		.req	s12
+	dg0v		.req	v12
+	dg1s		.req	s13
+	dg1v		.req	v13
+	dg2s		.req	s14
+
+	.macro		add_only, op, ev, rc, s0, dg1
+	.ifc		\ev, ev
+	add		t1.4s, v\s0\().4s, \rc\().4s
+	sha1h		dg2s, dg0s
+	.ifnb		\dg1
+	sha1\op		dg0q, \dg1, t0.4s
+	.else
+	sha1\op		dg0q, dg1s, t0.4s
+	.endif
+	.else
+	.ifnb		\s0
+	add		t0.4s, v\s0\().4s, \rc\().4s
+	.endif
+	sha1h		dg1s, dg0s
+	sha1\op		dg0q, dg2s, t1.4s
+	.endif
+	.endm
+
+	.macro		add_update, op, ev, rc, s0, s1, s2, s3, dg1
+	sha1su0		v\s0\().4s, v\s1\().4s, v\s2\().4s
+	add_only	\op, \ev, \rc, \s1, \dg1
+	sha1su1		v\s0\().4s, v\s3\().4s
+	.endm
+
+	/*
+	 * The SHA1 round constants
+	 */
+	.align		4
+.Lsha1_rcon:
+	.word		0x5a827999, 0x6ed9eba1, 0x8f1bbcdc, 0xca62c1d6
+
+	/*
+	 * void sha1_ce_transform(u32 state[5], u8 const *src, int blocks)
+	 */
+ENTRY(sha1_ce_transform)
+	/* load round constants */
+	adr		x6, .Lsha1_rcon
+	ld1r		{k0.4s}, [x6], #4
+	ld1r		{k1.4s}, [x6], #4
+	ld1r		{k2.4s}, [x6], #4
+	ld1r		{k3.4s}, [x6]
+
+	/* load state */
+	ld1		{dgav.4s}, [x0]
+	ldr		dgb, [x0, #16]
+
+	/* load input */
+0:	ld1		{v8.16b-v11.16b}, [x1], #64
+	sub		w2, w2, #1
+
+	rev32		v8.16b, v8.16b
+	rev32		v9.16b, v9.16b
+	rev32		v10.16b, v10.16b
+	rev32		v11.16b, v11.16b
+
+1:	add		t0.4s, v8.4s, k0.4s
+	mov		dg0v.16b, dgav.16b
+
+	add_update	c, ev, k0,  8,  9, 10, 11, dgb
+	add_update	c, od, k0,  9, 10, 11,  8
+	add_update	c, ev, k0, 10, 11,  8,  9
+	add_update	c, od, k0, 11,  8,  9, 10
+	add_update	c, ev, k1,  8,  9, 10, 11
+
+	add_update	p, od, k1,  9, 10, 11,  8
+	add_update	p, ev, k1, 10, 11,  8,  9
+	add_update	p, od, k1, 11,  8,  9, 10
+	add_update	p, ev, k1,  8,  9, 10, 11
+	add_update	p, od, k2,  9, 10, 11,  8
+
+	add_update	m, ev, k2, 10, 11,  8,  9
+	add_update	m, od, k2, 11,  8,  9, 10
+	add_update	m, ev, k2,  8,  9, 10, 11
+	add_update	m, od, k2,  9, 10, 11,  8
+	add_update	m, ev, k3, 10, 11,  8,  9
+
+	add_update	p, od, k3, 11,  8,  9, 10
+	add_only	p, ev, k3,  9
+	add_only	p, od, k3, 10
+	add_only	p, ev, k3, 11
+	add_only	p, od
+
+	/* update state */
+	add		dgbv.2s, dgbv.2s, dg1v.2s
+	add		dgav.4s, dgav.4s, dg0v.4s
+
+	cbnz		w2, 0b
+
+	/* store new state */
+3:	st1		{dgav.4s}, [x0]
+	str		dgb, [x0, #16]
+	ret
+ENDPROC(sha1_ce_transform)
diff --git a/core/lib/libtomcrypt/src/hashes/sha2/sha224.c b/core/lib/libtomcrypt/src/hashes/sha2/sha224.c
new file mode 100644
index 0000000..6055655
--- /dev/null
+++ b/core/lib/libtomcrypt/src/hashes/sha2/sha224.c
@@ -0,0 +1,157 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+/**
+   @param sha224.c
+   LTC_SHA-224 new NIST standard based off of LTC_SHA-256 truncated to 224 bits (Tom St Denis)
+*/
+
+#include "tomcrypt.h"
+
+#if defined(LTC_SHA224) && defined(LTC_SHA256)
+
+const struct ltc_hash_descriptor sha224_desc =
+{
+    "sha224",
+    10,
+    28,
+    64,
+
+    /* OID */
+   { 2, 16, 840, 1, 101, 3, 4, 2, 4,  },
+   9,
+
+    &sha224_init,
+    &sha256_process,
+    &sha224_done,
+    &sha224_test,
+    NULL
+};
+
+/* init the sha256 er... sha224 state ;-) */
+/**
+   Initialize the hash state
+   @param md   The hash state you wish to initialize
+   @return CRYPT_OK if successful
+*/
+int sha224_init(hash_state * md)
+{
+    LTC_ARGCHK(md != NULL);
+
+    md->sha256.curlen = 0;
+    md->sha256.length = 0;
+    md->sha256.state[0] = 0xc1059ed8UL;
+    md->sha256.state[1] = 0x367cd507UL;
+    md->sha256.state[2] = 0x3070dd17UL;
+    md->sha256.state[3] = 0xf70e5939UL;
+    md->sha256.state[4] = 0xffc00b31UL;
+    md->sha256.state[5] = 0x68581511UL;
+    md->sha256.state[6] = 0x64f98fa7UL;
+    md->sha256.state[7] = 0xbefa4fa4UL;
+    return CRYPT_OK;
+}
+
+/**
+   Terminate the hash to get the digest
+   @param md  The hash state
+   @param out [out] The destination of the hash (28 bytes)
+   @return CRYPT_OK if successful
+*/
+int sha224_done(hash_state * md, unsigned char *out)
+{
+    unsigned char buf[32];
+    int err;
+
+    LTC_ARGCHK(md  != NULL);
+    LTC_ARGCHK(out != NULL);
+
+    err = sha256_done(md, buf);
+    XMEMCPY(out, buf, 28);
+#ifdef LTC_CLEAN_STACK
+    zeromem(buf, sizeof(buf));
+#endif 
+    return err;
+}
+
+/**
+  Self-test the hash
+  @return CRYPT_OK if successful, CRYPT_NOP if self-tests have been disabled
+*/  
+int  sha224_test(void)
+{
+ #ifndef LTC_TEST
+    return CRYPT_NOP;
+ #else    
+  static const struct {
+      const char *msg;
+      unsigned char hash[28];
+  } tests[] = {
+    { "abc",
+      { 0x23, 0x09, 0x7d, 0x22, 0x34, 0x05, 0xd8,
+        0x22, 0x86, 0x42, 0xa4, 0x77, 0xbd, 0xa2,
+        0x55, 0xb3, 0x2a, 0xad, 0xbc, 0xe4, 0xbd,
+        0xa0, 0xb3, 0xf7, 0xe3, 0x6c, 0x9d, 0xa7 }
+    },
+    { "abcdbcdecdefdefgefghfghighijhijkijkljklmklmnlmnomnopnopq",
+      { 0x75, 0x38, 0x8b, 0x16, 0x51, 0x27, 0x76,
+        0xcc, 0x5d, 0xba, 0x5d, 0xa1, 0xfd, 0x89,
+        0x01, 0x50, 0xb0, 0xc6, 0x45, 0x5c, 0xb4,
+        0xf5, 0x8b, 0x19, 0x52, 0x52, 0x25, 0x25 }
+    },
+  };
+
+  int i;
+  unsigned char tmp[28];
+  hash_state md;
+
+  for (i = 0; i < (int)(sizeof(tests) / sizeof(tests[0])); i++) {
+      sha224_init(&md);
+      sha224_process(&md, (unsigned char*)tests[i].msg, (unsigned long)strlen(tests[i].msg));
+      sha224_done(&md, tmp);
+      if (XMEMCMP(tmp, tests[i].hash, 28) != 0) {
+         return CRYPT_FAIL_TESTVECTOR;
+      }
+  }
+  return CRYPT_OK;
+ #endif
+}
+
+#endif /* defined(LTC_SHA224) && defined(LTC_SHA256) */
+
+/* $Source: /cvs/libtom/libtomcrypt/src/hashes/sha2/sha224.c,v $ */
+/* $Revision: 1.10 $ */
+/* $Date: 2007/05/12 14:25:28 $ */
diff --git a/core/lib/libtomcrypt/src/hashes/sha2/sha256.c b/core/lib/libtomcrypt/src/hashes/sha2/sha256.c
new file mode 100644
index 0000000..e1e827d
--- /dev/null
+++ b/core/lib/libtomcrypt/src/hashes/sha2/sha256.c
@@ -0,0 +1,363 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file sha256.c
+  LTC_SHA256 by Tom St Denis 
+*/
+
+#ifdef LTC_SHA256 
+
+const struct ltc_hash_descriptor sha256_desc =
+{
+    "sha256",
+    0,
+    32,
+    64,
+
+    /* OID */
+   { 2, 16, 840, 1, 101, 3, 4, 2, 1,  },
+   9,
+    
+    &sha256_init,
+    &sha256_process,
+    &sha256_done,
+    &sha256_test,
+    NULL
+};
+
+#ifdef LTC_SMALL_CODE
+/* the K array */
+static const ulong32 K[64] = {
+    0x428a2f98UL, 0x71374491UL, 0xb5c0fbcfUL, 0xe9b5dba5UL, 0x3956c25bUL,
+    0x59f111f1UL, 0x923f82a4UL, 0xab1c5ed5UL, 0xd807aa98UL, 0x12835b01UL,
+    0x243185beUL, 0x550c7dc3UL, 0x72be5d74UL, 0x80deb1feUL, 0x9bdc06a7UL,
+    0xc19bf174UL, 0xe49b69c1UL, 0xefbe4786UL, 0x0fc19dc6UL, 0x240ca1ccUL,
+    0x2de92c6fUL, 0x4a7484aaUL, 0x5cb0a9dcUL, 0x76f988daUL, 0x983e5152UL,
+    0xa831c66dUL, 0xb00327c8UL, 0xbf597fc7UL, 0xc6e00bf3UL, 0xd5a79147UL,
+    0x06ca6351UL, 0x14292967UL, 0x27b70a85UL, 0x2e1b2138UL, 0x4d2c6dfcUL,
+    0x53380d13UL, 0x650a7354UL, 0x766a0abbUL, 0x81c2c92eUL, 0x92722c85UL,
+    0xa2bfe8a1UL, 0xa81a664bUL, 0xc24b8b70UL, 0xc76c51a3UL, 0xd192e819UL,
+    0xd6990624UL, 0xf40e3585UL, 0x106aa070UL, 0x19a4c116UL, 0x1e376c08UL,
+    0x2748774cUL, 0x34b0bcb5UL, 0x391c0cb3UL, 0x4ed8aa4aUL, 0x5b9cca4fUL,
+    0x682e6ff3UL, 0x748f82eeUL, 0x78a5636fUL, 0x84c87814UL, 0x8cc70208UL,
+    0x90befffaUL, 0xa4506cebUL, 0xbef9a3f7UL, 0xc67178f2UL
+};
+#endif
+
+/* Various logical functions */
+#define Ch(x,y,z)       (z ^ (x & (y ^ z)))
+#define Maj(x,y,z)      (((x | y) & z) | (x & y)) 
+#define S(x, n)         RORc((x),(n))
+#define R(x, n)         (((x)&0xFFFFFFFFUL)>>(n))
+#define Sigma0(x)       (S(x, 2) ^ S(x, 13) ^ S(x, 22))
+#define Sigma1(x)       (S(x, 6) ^ S(x, 11) ^ S(x, 25))
+#define Gamma0(x)       (S(x, 7) ^ S(x, 18) ^ R(x, 3))
+#define Gamma1(x)       (S(x, 17) ^ S(x, 19) ^ R(x, 10))
+
+/* compress 512-bits */
+#ifdef LTC_CLEAN_STACK
+static int _sha256_compress(hash_state * md, unsigned char *buf)
+#else
+static int  sha256_compress(hash_state * md, unsigned char *buf)
+#endif
+{
+    ulong32 S[8], W[64], t0, t1;
+#ifdef LTC_SMALL_CODE
+    ulong32 t;
+#endif
+    int i;
+
+    /* copy state into S */
+    for (i = 0; i < 8; i++) {
+        S[i] = md->sha256.state[i];
+    }
+
+    /* copy the state into 512-bits into W[0..15] */
+    for (i = 0; i < 16; i++) {
+        LOAD32H(W[i], buf + (4*i));
+    }
+
+    /* fill W[16..63] */
+    for (i = 16; i < 64; i++) {
+        W[i] = Gamma1(W[i - 2]) + W[i - 7] + Gamma0(W[i - 15]) + W[i - 16];
+    }        
+
+    /* Compress */
+#ifdef LTC_SMALL_CODE   
+#define RND(a,b,c,d,e,f,g,h,i)                         \
+     t0 = h + Sigma1(e) + Ch(e, f, g) + K[i] + W[i];   \
+     t1 = Sigma0(a) + Maj(a, b, c);                    \
+     d += t0;                                          \
+     h  = t0 + t1;
+
+     for (i = 0; i < 64; ++i) {
+         RND(S[0],S[1],S[2],S[3],S[4],S[5],S[6],S[7],i);
+         t = S[7]; S[7] = S[6]; S[6] = S[5]; S[5] = S[4]; 
+         S[4] = S[3]; S[3] = S[2]; S[2] = S[1]; S[1] = S[0]; S[0] = t;
+     }  
+#else 
+#define RND(a,b,c,d,e,f,g,h,i,ki)                    \
+     t0 = h + Sigma1(e) + Ch(e, f, g) + ki + W[i];   \
+     t1 = Sigma0(a) + Maj(a, b, c);                  \
+     d += t0;                                        \
+     h  = t0 + t1;
+
+    RND(S[0],S[1],S[2],S[3],S[4],S[5],S[6],S[7],0,0x428a2f98);
+    RND(S[7],S[0],S[1],S[2],S[3],S[4],S[5],S[6],1,0x71374491);
+    RND(S[6],S[7],S[0],S[1],S[2],S[3],S[4],S[5],2,0xb5c0fbcf);
+    RND(S[5],S[6],S[7],S[0],S[1],S[2],S[3],S[4],3,0xe9b5dba5);
+    RND(S[4],S[5],S[6],S[7],S[0],S[1],S[2],S[3],4,0x3956c25b);
+    RND(S[3],S[4],S[5],S[6],S[7],S[0],S[1],S[2],5,0x59f111f1);
+    RND(S[2],S[3],S[4],S[5],S[6],S[7],S[0],S[1],6,0x923f82a4);
+    RND(S[1],S[2],S[3],S[4],S[5],S[6],S[7],S[0],7,0xab1c5ed5);
+    RND(S[0],S[1],S[2],S[3],S[4],S[5],S[6],S[7],8,0xd807aa98);
+    RND(S[7],S[0],S[1],S[2],S[3],S[4],S[5],S[6],9,0x12835b01);
+    RND(S[6],S[7],S[0],S[1],S[2],S[3],S[4],S[5],10,0x243185be);
+    RND(S[5],S[6],S[7],S[0],S[1],S[2],S[3],S[4],11,0x550c7dc3);
+    RND(S[4],S[5],S[6],S[7],S[0],S[1],S[2],S[3],12,0x72be5d74);
+    RND(S[3],S[4],S[5],S[6],S[7],S[0],S[1],S[2],13,0x80deb1fe);
+    RND(S[2],S[3],S[4],S[5],S[6],S[7],S[0],S[1],14,0x9bdc06a7);
+    RND(S[1],S[2],S[3],S[4],S[5],S[6],S[7],S[0],15,0xc19bf174);
+    RND(S[0],S[1],S[2],S[3],S[4],S[5],S[6],S[7],16,0xe49b69c1);
+    RND(S[7],S[0],S[1],S[2],S[3],S[4],S[5],S[6],17,0xefbe4786);
+    RND(S[6],S[7],S[0],S[1],S[2],S[3],S[4],S[5],18,0x0fc19dc6);
+    RND(S[5],S[6],S[7],S[0],S[1],S[2],S[3],S[4],19,0x240ca1cc);
+    RND(S[4],S[5],S[6],S[7],S[0],S[1],S[2],S[3],20,0x2de92c6f);
+    RND(S[3],S[4],S[5],S[6],S[7],S[0],S[1],S[2],21,0x4a7484aa);
+    RND(S[2],S[3],S[4],S[5],S[6],S[7],S[0],S[1],22,0x5cb0a9dc);
+    RND(S[1],S[2],S[3],S[4],S[5],S[6],S[7],S[0],23,0x76f988da);
+    RND(S[0],S[1],S[2],S[3],S[4],S[5],S[6],S[7],24,0x983e5152);
+    RND(S[7],S[0],S[1],S[2],S[3],S[4],S[5],S[6],25,0xa831c66d);
+    RND(S[6],S[7],S[0],S[1],S[2],S[3],S[4],S[5],26,0xb00327c8);
+    RND(S[5],S[6],S[7],S[0],S[1],S[2],S[3],S[4],27,0xbf597fc7);
+    RND(S[4],S[5],S[6],S[7],S[0],S[1],S[2],S[3],28,0xc6e00bf3);
+    RND(S[3],S[4],S[5],S[6],S[7],S[0],S[1],S[2],29,0xd5a79147);
+    RND(S[2],S[3],S[4],S[5],S[6],S[7],S[0],S[1],30,0x06ca6351);
+    RND(S[1],S[2],S[3],S[4],S[5],S[6],S[7],S[0],31,0x14292967);
+    RND(S[0],S[1],S[2],S[3],S[4],S[5],S[6],S[7],32,0x27b70a85);
+    RND(S[7],S[0],S[1],S[2],S[3],S[4],S[5],S[6],33,0x2e1b2138);
+    RND(S[6],S[7],S[0],S[1],S[2],S[3],S[4],S[5],34,0x4d2c6dfc);
+    RND(S[5],S[6],S[7],S[0],S[1],S[2],S[3],S[4],35,0x53380d13);
+    RND(S[4],S[5],S[6],S[7],S[0],S[1],S[2],S[3],36,0x650a7354);
+    RND(S[3],S[4],S[5],S[6],S[7],S[0],S[1],S[2],37,0x766a0abb);
+    RND(S[2],S[3],S[4],S[5],S[6],S[7],S[0],S[1],38,0x81c2c92e);
+    RND(S[1],S[2],S[3],S[4],S[5],S[6],S[7],S[0],39,0x92722c85);
+    RND(S[0],S[1],S[2],S[3],S[4],S[5],S[6],S[7],40,0xa2bfe8a1);
+    RND(S[7],S[0],S[1],S[2],S[3],S[4],S[5],S[6],41,0xa81a664b);
+    RND(S[6],S[7],S[0],S[1],S[2],S[3],S[4],S[5],42,0xc24b8b70);
+    RND(S[5],S[6],S[7],S[0],S[1],S[2],S[3],S[4],43,0xc76c51a3);
+    RND(S[4],S[5],S[6],S[7],S[0],S[1],S[2],S[3],44,0xd192e819);
+    RND(S[3],S[4],S[5],S[6],S[7],S[0],S[1],S[2],45,0xd6990624);
+    RND(S[2],S[3],S[4],S[5],S[6],S[7],S[0],S[1],46,0xf40e3585);
+    RND(S[1],S[2],S[3],S[4],S[5],S[6],S[7],S[0],47,0x106aa070);
+    RND(S[0],S[1],S[2],S[3],S[4],S[5],S[6],S[7],48,0x19a4c116);
+    RND(S[7],S[0],S[1],S[2],S[3],S[4],S[5],S[6],49,0x1e376c08);
+    RND(S[6],S[7],S[0],S[1],S[2],S[3],S[4],S[5],50,0x2748774c);
+    RND(S[5],S[6],S[7],S[0],S[1],S[2],S[3],S[4],51,0x34b0bcb5);
+    RND(S[4],S[5],S[6],S[7],S[0],S[1],S[2],S[3],52,0x391c0cb3);
+    RND(S[3],S[4],S[5],S[6],S[7],S[0],S[1],S[2],53,0x4ed8aa4a);
+    RND(S[2],S[3],S[4],S[5],S[6],S[7],S[0],S[1],54,0x5b9cca4f);
+    RND(S[1],S[2],S[3],S[4],S[5],S[6],S[7],S[0],55,0x682e6ff3);
+    RND(S[0],S[1],S[2],S[3],S[4],S[5],S[6],S[7],56,0x748f82ee);
+    RND(S[7],S[0],S[1],S[2],S[3],S[4],S[5],S[6],57,0x78a5636f);
+    RND(S[6],S[7],S[0],S[1],S[2],S[3],S[4],S[5],58,0x84c87814);
+    RND(S[5],S[6],S[7],S[0],S[1],S[2],S[3],S[4],59,0x8cc70208);
+    RND(S[4],S[5],S[6],S[7],S[0],S[1],S[2],S[3],60,0x90befffa);
+    RND(S[3],S[4],S[5],S[6],S[7],S[0],S[1],S[2],61,0xa4506ceb);
+    RND(S[2],S[3],S[4],S[5],S[6],S[7],S[0],S[1],62,0xbef9a3f7);
+    RND(S[1],S[2],S[3],S[4],S[5],S[6],S[7],S[0],63,0xc67178f2);
+
+#undef RND     
+    
+#endif     
+
+    /* feedback */
+    for (i = 0; i < 8; i++) {
+        md->sha256.state[i] = md->sha256.state[i] + S[i];
+    }
+    return CRYPT_OK;
+}
+
+#ifdef LTC_CLEAN_STACK
+static int sha256_compress(hash_state * md, unsigned char *buf)
+{
+    int err;
+    err = _sha256_compress(md, buf);
+    burn_stack(sizeof(ulong32) * 74);
+    return err;
+}
+#endif
+
+/**
+   Initialize the hash state
+   @param md   The hash state you wish to initialize
+   @return CRYPT_OK if successful
+*/
+int sha256_init(hash_state * md)
+{
+    LTC_ARGCHK(md != NULL);
+
+    md->sha256.curlen = 0;
+    md->sha256.length = 0;
+    md->sha256.state[0] = 0x6A09E667UL;
+    md->sha256.state[1] = 0xBB67AE85UL;
+    md->sha256.state[2] = 0x3C6EF372UL;
+    md->sha256.state[3] = 0xA54FF53AUL;
+    md->sha256.state[4] = 0x510E527FUL;
+    md->sha256.state[5] = 0x9B05688CUL;
+    md->sha256.state[6] = 0x1F83D9ABUL;
+    md->sha256.state[7] = 0x5BE0CD19UL;
+    return CRYPT_OK;
+}
+
+/**
+   Process a block of memory though the hash
+   @param md     The hash state
+   @param in     The data to hash
+   @param inlen  The length of the data (octets)
+   @return CRYPT_OK if successful
+*/
+HASH_PROCESS(sha256_process, sha256_compress, sha256, 64)
+
+/**
+   Terminate the hash to get the digest
+   @param md  The hash state
+   @param out [out] The destination of the hash (32 bytes)
+   @return CRYPT_OK if successful
+*/
+int sha256_done(hash_state * md, unsigned char *out)
+{
+    int i;
+
+    LTC_ARGCHK(md  != NULL);
+    LTC_ARGCHK(out != NULL);
+
+    if (md->sha256.curlen >= sizeof(md->sha256.buf)) {
+       return CRYPT_INVALID_ARG;
+    }
+
+
+    /* increase the length of the message */
+    md->sha256.length += md->sha256.curlen * 8;
+
+    /* append the '1' bit */
+    md->sha256.buf[md->sha256.curlen++] = (unsigned char)0x80;
+
+    /* if the length is currently above 56 bytes we append zeros
+     * then compress.  Then we can fall back to padding zeros and length
+     * encoding like normal.
+     */
+    if (md->sha256.curlen > 56) {
+        while (md->sha256.curlen < 64) {
+            md->sha256.buf[md->sha256.curlen++] = (unsigned char)0;
+        }
+        sha256_compress(md, md->sha256.buf);
+        md->sha256.curlen = 0;
+    }
+
+    /* pad upto 56 bytes of zeroes */
+    while (md->sha256.curlen < 56) {
+        md->sha256.buf[md->sha256.curlen++] = (unsigned char)0;
+    }
+
+    /* store length */
+    STORE64H(md->sha256.length, md->sha256.buf+56);
+    sha256_compress(md, md->sha256.buf);
+
+    /* copy output */
+    for (i = 0; i < 8; i++) {
+        STORE32H(md->sha256.state[i], out+(4*i));
+    }
+#ifdef LTC_CLEAN_STACK
+    zeromem(md, sizeof(hash_state));
+#endif
+    return CRYPT_OK;
+}
+
+/**
+  Self-test the hash
+  @return CRYPT_OK if successful, CRYPT_NOP if self-tests have been disabled
+*/  
+int  sha256_test(void)
+{
+ #ifndef LTC_TEST
+    return CRYPT_NOP;
+ #else    
+  static const struct {
+      const char *msg;
+      unsigned char hash[32];
+  } tests[] = {
+    { "abc",
+      { 0xba, 0x78, 0x16, 0xbf, 0x8f, 0x01, 0xcf, 0xea,
+        0x41, 0x41, 0x40, 0xde, 0x5d, 0xae, 0x22, 0x23,
+        0xb0, 0x03, 0x61, 0xa3, 0x96, 0x17, 0x7a, 0x9c,
+        0xb4, 0x10, 0xff, 0x61, 0xf2, 0x00, 0x15, 0xad }
+    },
+    { "abcdbcdecdefdefgefghfghighijhijkijkljklmklmnlmnomnopnopq",
+      { 0x24, 0x8d, 0x6a, 0x61, 0xd2, 0x06, 0x38, 0xb8, 
+        0xe5, 0xc0, 0x26, 0x93, 0x0c, 0x3e, 0x60, 0x39,
+        0xa3, 0x3c, 0xe4, 0x59, 0x64, 0xff, 0x21, 0x67, 
+        0xf6, 0xec, 0xed, 0xd4, 0x19, 0xdb, 0x06, 0xc1 }
+    },
+  };
+
+  int i;
+  unsigned char tmp[32];
+  hash_state md;
+
+  for (i = 0; i < (int)(sizeof(tests) / sizeof(tests[0])); i++) {
+      sha256_init(&md);
+      sha256_process(&md, (unsigned char*)tests[i].msg, (unsigned long)strlen(tests[i].msg));
+      sha256_done(&md, tmp);
+      if (XMEMCMP(tmp, tests[i].hash, 32) != 0) {
+         return CRYPT_FAIL_TESTVECTOR;
+      }
+  }
+  return CRYPT_OK;
+ #endif
+}
+
+#endif
+
+
+
+/* $Source: /cvs/libtom/libtomcrypt/src/hashes/sha2/sha256.c,v $ */
+/* $Revision: 1.11 $ */
+/* $Date: 2007/05/12 14:25:28 $ */
diff --git a/core/lib/libtomcrypt/src/hashes/sha2/sha256_armv8a_ce.c b/core/lib/libtomcrypt/src/hashes/sha2/sha256_armv8a_ce.c
new file mode 100644
index 0000000..0ba73e7
--- /dev/null
+++ b/core/lib/libtomcrypt/src/hashes/sha2/sha256_armv8a_ce.c
@@ -0,0 +1,216 @@
+/*
+ * Copyright (c) 2015, Linaro Limited
+ * All rights reserved.
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+#include "tomcrypt_arm_neon.h"
+
+/**
+  @file sha256_arm32_ce.c
+  LTC_SHA256_ARM32_CE
+*/
+
+#if defined(LTC_SHA256_ARM32_CE) || defined(LTC_SHA256_ARM64_CE)
+
+const struct ltc_hash_descriptor sha256_desc =
+{
+    "sha256",
+    0,
+    32,
+    64,
+
+    /* OID */
+   { 2, 16, 840, 1, 101, 3, 4, 2, 1,  },
+   9,
+
+    &sha256_init,
+    &sha256_process,
+    &sha256_done,
+    &sha256_test,
+    NULL
+};
+
+
+/* Implemented in assembly */
+int sha256_ce_transform(ulong32 *state, unsigned char *buf, int blocks);
+
+static int sha256_compress_nblocks(hash_state *md, unsigned char *buf, int blocks)
+{
+    struct tomcrypt_arm_neon_state state;
+
+    tomcrypt_arm_neon_enable(&state);
+    sha256_ce_transform(md->sha256.state, buf, blocks);
+    tomcrypt_arm_neon_disable(&state);
+    return CRYPT_OK;
+}
+
+static int sha256_compress(hash_state *md, unsigned char *buf)
+{
+   return sha256_compress_nblocks(md, buf, 1);
+}
+
+/**
+   Initialize the hash state
+   @param md   The hash state you wish to initialize
+   @return CRYPT_OK if successful
+*/
+int sha256_init(hash_state * md)
+{
+    LTC_ARGCHK(md != NULL);
+
+    md->sha256.curlen = 0;
+    md->sha256.length = 0;
+    md->sha256.state[0] = 0x6A09E667UL;
+    md->sha256.state[1] = 0xBB67AE85UL;
+    md->sha256.state[2] = 0x3C6EF372UL;
+    md->sha256.state[3] = 0xA54FF53AUL;
+    md->sha256.state[4] = 0x510E527FUL;
+    md->sha256.state[5] = 0x9B05688CUL;
+    md->sha256.state[6] = 0x1F83D9ABUL;
+    md->sha256.state[7] = 0x5BE0CD19UL;
+    return CRYPT_OK;
+}
+
+/**
+   Process a block of memory though the hash
+   @param md     The hash state
+   @param in     The data to hash
+   @param inlen  The length of the data (octets)
+   @return CRYPT_OK if successful
+*/
+HASH_PROCESS_NBLOCKS(sha256_process, sha256_compress_nblocks, sha256, 64)
+
+/**
+   Terminate the hash to get the digest
+   @param md  The hash state
+   @param out [out] The destination of the hash (32 bytes)
+   @return CRYPT_OK if successful
+*/
+int sha256_done(hash_state * md, unsigned char *out)
+{
+    int i;
+
+    LTC_ARGCHK(md  != NULL);
+    LTC_ARGCHK(out != NULL);
+
+    if (md->sha256.curlen >= sizeof(md->sha256.buf)) {
+       return CRYPT_INVALID_ARG;
+    }
+
+
+    /* increase the length of the message */
+    md->sha256.length += md->sha256.curlen * 8;
+
+    /* append the '1' bit */
+    md->sha256.buf[md->sha256.curlen++] = (unsigned char)0x80;
+
+    /* if the length is currently above 56 bytes we append zeros
+     * then compress.  Then we can fall back to padding zeros and length
+     * encoding like normal.
+     */
+    if (md->sha256.curlen > 56) {
+        while (md->sha256.curlen < 64) {
+            md->sha256.buf[md->sha256.curlen++] = (unsigned char)0;
+        }
+        sha256_compress(md, md->sha256.buf);
+        md->sha256.curlen = 0;
+    }
+
+    /* pad upto 56 bytes of zeroes */
+    while (md->sha256.curlen < 56) {
+        md->sha256.buf[md->sha256.curlen++] = (unsigned char)0;
+    }
+
+    /* store length */
+    STORE64H(md->sha256.length, md->sha256.buf+56);
+    sha256_compress(md, md->sha256.buf);
+
+    /* copy output */
+    for (i = 0; i < 8; i++) {
+        STORE32H(md->sha256.state[i], out+(4*i));
+    }
+#ifdef LTC_CLEAN_STACK
+    zeromem(md, sizeof(hash_state));
+#endif
+    return CRYPT_OK;
+}
+
+/**
+  Self-test the hash
+  @return CRYPT_OK if successful, CRYPT_NOP if self-tests have been disabled
+*/  
+int  sha256_test(void)
+{
+ #ifndef LTC_TEST
+    return CRYPT_NOP;
+ #else    
+  static const struct {
+      const char *msg;
+      unsigned char hash[32];
+  } tests[] = {
+    { "abc",
+      { 0xba, 0x78, 0x16, 0xbf, 0x8f, 0x01, 0xcf, 0xea,
+        0x41, 0x41, 0x40, 0xde, 0x5d, 0xae, 0x22, 0x23,
+        0xb0, 0x03, 0x61, 0xa3, 0x96, 0x17, 0x7a, 0x9c,
+        0xb4, 0x10, 0xff, 0x61, 0xf2, 0x00, 0x15, 0xad }
+    },
+    { "abcdbcdecdefdefgefghfghighijhijkijkljklmklmnlmnomnopnopq",
+      { 0x24, 0x8d, 0x6a, 0x61, 0xd2, 0x06, 0x38, 0xb8, 
+        0xe5, 0xc0, 0x26, 0x93, 0x0c, 0x3e, 0x60, 0x39,
+        0xa3, 0x3c, 0xe4, 0x59, 0x64, 0xff, 0x21, 0x67, 
+        0xf6, 0xec, 0xed, 0xd4, 0x19, 0xdb, 0x06, 0xc1 }
+    },
+  };
+
+  int i;
+  unsigned char tmp[32];
+  hash_state md;
+
+  for (i = 0; i < (int)(sizeof(tests) / sizeof(tests[0])); i++) {
+      sha256_init(&md);
+      sha256_process(&md, (unsigned char*)tests[i].msg, (unsigned long)strlen(tests[i].msg));
+      sha256_done(&md, tmp);
+      if (XMEMCMP(tmp, tests[i].hash, 32) != 0) {
+         return CRYPT_FAIL_TESTVECTOR;
+      }
+  }
+  return CRYPT_OK;
+ #endif
+}
+
+#endif
diff --git a/core/lib/libtomcrypt/src/hashes/sha2/sha256_armv8a_ce_a32.S b/core/lib/libtomcrypt/src/hashes/sha2/sha256_armv8a_ce_a32.S
new file mode 100644
index 0000000..26568fd
--- /dev/null
+++ b/core/lib/libtomcrypt/src/hashes/sha2/sha256_armv8a_ce_a32.S
@@ -0,0 +1,133 @@
+/*
+ * Copyright (c) 2014-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.
+ */
+
+/* SHA-256 secure hash using ARMv8 Crypto Extensions */
+
+	.text
+	.fpu		crypto-neon-fp-armv8
+
+	k0		.req	q7
+	k1		.req	q8
+
+	ta0		.req	q9
+	ta1		.req	q10
+	tb0		.req	q10
+	tb1		.req	q9
+
+	dga		.req	q11
+	dgb		.req	q12
+
+	dg0		.req	q13
+	dg1		.req	q14
+	dg2		.req	q15
+
+	.macro		add_only, ev, s0
+	vmov		dg2, dg0
+	.ifnb		\s0
+	vld1.32		{k\ev}, [r3]!
+	.endif
+	sha256h.32	dg0, dg1, tb\ev
+	sha256h2.32	dg1, dg2, tb\ev
+	.ifnb		\s0
+	vadd.u32	ta\ev, q\s0, k\ev
+	.endif
+	.endm
+
+	.macro		add_update, ev, s0, s1, s2, s3
+	sha256su0.32	q\s0, q\s1
+	add_only	\ev, \s1
+	sha256su1.32	q\s0, q\s2, q\s3
+	.endm
+
+	.align		6
+.Lsha256_rcon:
+	.word		0x428a2f98, 0x71374491, 0xb5c0fbcf, 0xe9b5dba5
+	.word		0x3956c25b, 0x59f111f1, 0x923f82a4, 0xab1c5ed5
+	.word		0xd807aa98, 0x12835b01, 0x243185be, 0x550c7dc3
+	.word		0x72be5d74, 0x80deb1fe, 0x9bdc06a7, 0xc19bf174
+	.word		0xe49b69c1, 0xefbe4786, 0x0fc19dc6, 0x240ca1cc
+	.word		0x2de92c6f, 0x4a7484aa, 0x5cb0a9dc, 0x76f988da
+	.word		0x983e5152, 0xa831c66d, 0xb00327c8, 0xbf597fc7
+	.word		0xc6e00bf3, 0xd5a79147, 0x06ca6351, 0x14292967
+	.word		0x27b70a85, 0x2e1b2138, 0x4d2c6dfc, 0x53380d13
+	.word		0x650a7354, 0x766a0abb, 0x81c2c92e, 0x92722c85
+	.word		0xa2bfe8a1, 0xa81a664b, 0xc24b8b70, 0xc76c51a3
+	.word		0xd192e819, 0xd6990624, 0xf40e3585, 0x106aa070
+	.word		0x19a4c116, 0x1e376c08, 0x2748774c, 0x34b0bcb5
+	.word		0x391c0cb3, 0x4ed8aa4a, 0x5b9cca4f, 0x682e6ff3
+	.word		0x748f82ee, 0x78a5636f, 0x84c87814, 0x8cc70208
+	.word		0x90befffa, 0xa4506ceb, 0xbef9a3f7, 0xc67178f2
+
+	.global		sha256_ce_transform
+	.type		sha256_ce_transform, %function
+sha256_ce_transform:
+	/* load state */
+	vld1.8		{dga-dgb}, [r0]
+
+	/* load input */
+0:	vld1.8		{q0-q1}, [r1]!
+	vrev32.8	q0, q0
+	vrev32.8	q1, q1
+	vld1.8		{q2-q3}, [r1]!
+	vrev32.8	q2, q2
+	vrev32.8	q3, q3
+	subs            r2, r2, #1
+
+	/* load round constants */
+	adr		r3, .Lsha256_rcon
+	vld1.32		{k0}, [r3]!
+
+	vadd.u32	ta0, q0, k0
+	vmov		dg0, dga
+	vmov		dg1, dgb
+
+	add_update	1, 0, 1, 2, 3
+	add_update	0, 1, 2, 3, 0
+	add_update	1, 2, 3, 0, 1
+	add_update	0, 3, 0, 1, 2
+	add_update	1, 0, 1, 2, 3
+	add_update	0, 1, 2, 3, 0
+	add_update	1, 2, 3, 0, 1
+	add_update	0, 3, 0, 1, 2
+	add_update	1, 0, 1, 2, 3
+	add_update	0, 1, 2, 3, 0
+	add_update	1, 2, 3, 0, 1
+	add_update	0, 3, 0, 1, 2
+
+	add_only	1, 1
+	add_only	0, 2
+	add_only	1, 3
+	add_only	0
+
+	/* update state */
+	vadd.u32	dga, dga, dg0
+	vadd.u32	dgb, dgb, dg1
+	bne		0b
+
+	/* store new state */
+	vst1.8		{dga-dgb}, [r0]
+	bx		lr
diff --git a/core/lib/libtomcrypt/src/hashes/sha2/sha256_armv8a_ce_a64.S b/core/lib/libtomcrypt/src/hashes/sha2/sha256_armv8a_ce_a64.S
new file mode 100644
index 0000000..506525e
--- /dev/null
+++ b/core/lib/libtomcrypt/src/hashes/sha2/sha256_armv8a_ce_a64.S
@@ -0,0 +1,166 @@
+/*
+ * 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.
+ */
+
+/*
+ * Core SHA-224/SHA-256 transform using v8 Crypto Extensions
+ *
+ * Copyright (C) 2014 Linaro Ltd <ard.biesheuvel@linaro.org>
+ */
+
+
+#define ENTRY(func) \
+	.global func ; \
+	.type func , %function ; \
+	func :
+
+#define ENDPROC(func) \
+	.size func , .-func
+
+	.text
+	.arch		armv8-a+crypto
+
+	dga		.req	q20
+	dgav		.req	v20
+	dgb		.req	q21
+	dgbv		.req	v21
+
+	t0		.req	v22
+	t1		.req	v23
+
+	dg0q		.req	q24
+	dg0v		.req	v24
+	dg1q		.req	q25
+	dg1v		.req	v25
+	dg2q		.req	q26
+	dg2v		.req	v26
+
+	.macro		add_only, ev, rc, s0
+	mov		dg2v.16b, dg0v.16b
+	.ifeq		\ev
+	add		t1.4s, v\s0\().4s, \rc\().4s
+	sha256h		dg0q, dg1q, t0.4s
+	sha256h2	dg1q, dg2q, t0.4s
+	.else
+	.ifnb		\s0
+	add		t0.4s, v\s0\().4s, \rc\().4s
+	.endif
+	sha256h		dg0q, dg1q, t1.4s
+	sha256h2	dg1q, dg2q, t1.4s
+	.endif
+	.endm
+
+	.macro		add_update, ev, rc, s0, s1, s2, s3
+	sha256su0	v\s0\().4s, v\s1\().4s
+	add_only	\ev, \rc, \s1
+	sha256su1	v\s0\().4s, v\s2\().4s, v\s3\().4s
+	.endm
+
+	/*
+	 * The SHA-256 round constants
+	 */
+	.align		4
+.Lsha2_rcon:
+	.word		0x428a2f98, 0x71374491, 0xb5c0fbcf, 0xe9b5dba5
+	.word		0x3956c25b, 0x59f111f1, 0x923f82a4, 0xab1c5ed5
+	.word		0xd807aa98, 0x12835b01, 0x243185be, 0x550c7dc3
+	.word		0x72be5d74, 0x80deb1fe, 0x9bdc06a7, 0xc19bf174
+	.word		0xe49b69c1, 0xefbe4786, 0x0fc19dc6, 0x240ca1cc
+	.word		0x2de92c6f, 0x4a7484aa, 0x5cb0a9dc, 0x76f988da
+	.word		0x983e5152, 0xa831c66d, 0xb00327c8, 0xbf597fc7
+	.word		0xc6e00bf3, 0xd5a79147, 0x06ca6351, 0x14292967
+	.word		0x27b70a85, 0x2e1b2138, 0x4d2c6dfc, 0x53380d13
+	.word		0x650a7354, 0x766a0abb, 0x81c2c92e, 0x92722c85
+	.word		0xa2bfe8a1, 0xa81a664b, 0xc24b8b70, 0xc76c51a3
+	.word		0xd192e819, 0xd6990624, 0xf40e3585, 0x106aa070
+	.word		0x19a4c116, 0x1e376c08, 0x2748774c, 0x34b0bcb5
+	.word		0x391c0cb3, 0x4ed8aa4a, 0x5b9cca4f, 0x682e6ff3
+	.word		0x748f82ee, 0x78a5636f, 0x84c87814, 0x8cc70208
+	.word		0x90befffa, 0xa4506ceb, 0xbef9a3f7, 0xc67178f2
+
+	/*
+	 * void sha2_ce_transform(struct sha256_ce_state *sst, u8 const *src,
+	 *			  int blocks)
+	 */
+ENTRY(sha256_ce_transform)
+	/* load round constants */
+	adr		x8, .Lsha2_rcon
+	ld1		{ v0.4s- v3.4s}, [x8], #64
+	ld1		{ v4.4s- v7.4s}, [x8], #64
+	ld1		{ v8.4s-v11.4s}, [x8], #64
+	ld1		{v12.4s-v15.4s}, [x8]
+
+	/* load state */
+	mov		x9, x0
+	ld1		{dgav.4s}, [x9], #16
+	ld1             {dgbv.4s}, [x9]
+
+	/* load input */
+0:	ld1		{v16.16b-v19.16b}, [x1], #64
+	sub		w2, w2, #1
+
+	rev32		v16.16b, v16.16b
+	rev32		v17.16b, v17.16b
+	rev32		v18.16b, v18.16b
+	rev32		v19.16b, v19.16b
+
+1:	add		t0.4s, v16.4s, v0.4s
+	mov		dg0v.16b, dgav.16b
+	mov		dg1v.16b, dgbv.16b
+
+	add_update	0,  v1, 16, 17, 18, 19
+	add_update	1,  v2, 17, 18, 19, 16
+	add_update	0,  v3, 18, 19, 16, 17
+	add_update	1,  v4, 19, 16, 17, 18
+
+	add_update	0,  v5, 16, 17, 18, 19
+	add_update	1,  v6, 17, 18, 19, 16
+	add_update	0,  v7, 18, 19, 16, 17
+	add_update	1,  v8, 19, 16, 17, 18
+
+	add_update	0,  v9, 16, 17, 18, 19
+	add_update	1, v10, 17, 18, 19, 16
+	add_update	0, v11, 18, 19, 16, 17
+	add_update	1, v12, 19, 16, 17, 18
+
+	add_only	0, v13, 17
+	add_only	1, v14, 18
+	add_only	0, v15, 19
+	add_only	1
+
+	/* update state */
+	add		dgav.4s, dgav.4s, dg0v.4s
+	add		dgbv.4s, dgbv.4s, dg1v.4s
+
+	/* handled all input blocks? */
+	cbnz		w2, 0b
+
+	/* store new state */
+3:	mov		x9, x0
+	st1		{dgav.16b}, [x9], #16
+	st1		{dgbv.16b}, [x9]
+	ret
+ENDPROC(sha256_ce_transform)
diff --git a/core/lib/libtomcrypt/src/hashes/sha2/sha384.c b/core/lib/libtomcrypt/src/hashes/sha2/sha384.c
new file mode 100644
index 0000000..c491f70
--- /dev/null
+++ b/core/lib/libtomcrypt/src/hashes/sha2/sha384.c
@@ -0,0 +1,165 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+/**
+   @param sha384.c
+   LTC_SHA384 hash included in sha512.c, Tom St Denis
+*/
+
+#include "tomcrypt.h"
+
+#if defined(LTC_SHA384) && defined(LTC_SHA512)
+
+const struct ltc_hash_descriptor sha384_desc =
+{
+    "sha384",
+    4,
+    48,
+    128,
+
+    /* OID */
+   { 2, 16, 840, 1, 101, 3, 4, 2, 2,  },
+   9,
+
+    &sha384_init,
+    &sha512_process,
+    &sha384_done,
+    &sha384_test,
+    NULL
+};
+
+/**
+   Initialize the hash state
+   @param md   The hash state you wish to initialize
+   @return CRYPT_OK if successful
+*/
+int sha384_init(hash_state * md)
+{
+    LTC_ARGCHK(md != NULL);
+
+    md->sha512.curlen = 0;
+    md->sha512.length = 0;
+    md->sha512.state[0] = CONST64(0xcbbb9d5dc1059ed8);
+    md->sha512.state[1] = CONST64(0x629a292a367cd507);
+    md->sha512.state[2] = CONST64(0x9159015a3070dd17);
+    md->sha512.state[3] = CONST64(0x152fecd8f70e5939);
+    md->sha512.state[4] = CONST64(0x67332667ffc00b31);
+    md->sha512.state[5] = CONST64(0x8eb44a8768581511);
+    md->sha512.state[6] = CONST64(0xdb0c2e0d64f98fa7);
+    md->sha512.state[7] = CONST64(0x47b5481dbefa4fa4);
+    return CRYPT_OK;
+}
+
+/**
+   Terminate the hash to get the digest
+   @param md  The hash state
+   @param out [out] The destination of the hash (48 bytes)
+   @return CRYPT_OK if successful
+*/
+int sha384_done(hash_state * md, unsigned char *out)
+{
+   unsigned char buf[64];
+
+   LTC_ARGCHK(md  != NULL);
+   LTC_ARGCHK(out != NULL);
+
+    if (md->sha512.curlen >= sizeof(md->sha512.buf)) {
+       return CRYPT_INVALID_ARG;
+    }
+
+   sha512_done(md, buf);
+   XMEMCPY(out, buf, 48);
+#ifdef LTC_CLEAN_STACK
+   zeromem(buf, sizeof(buf));
+#endif
+   return CRYPT_OK;
+}
+
+/**
+  Self-test the hash
+  @return CRYPT_OK if successful, CRYPT_NOP if self-tests have been disabled
+*/  
+int  sha384_test(void)
+{
+ #ifndef LTC_TEST
+    return CRYPT_NOP;
+ #else    
+  static const struct {
+      const char *msg;
+      unsigned char hash[48];
+  } tests[] = {
+    { "abc",
+      { 0xcb, 0x00, 0x75, 0x3f, 0x45, 0xa3, 0x5e, 0x8b,
+        0xb5, 0xa0, 0x3d, 0x69, 0x9a, 0xc6, 0x50, 0x07,
+        0x27, 0x2c, 0x32, 0xab, 0x0e, 0xde, 0xd1, 0x63,
+        0x1a, 0x8b, 0x60, 0x5a, 0x43, 0xff, 0x5b, 0xed,
+        0x80, 0x86, 0x07, 0x2b, 0xa1, 0xe7, 0xcc, 0x23,
+        0x58, 0xba, 0xec, 0xa1, 0x34, 0xc8, 0x25, 0xa7 }
+    },
+    { "abcdefghbcdefghicdefghijdefghijkefghijklfghijklmghijklmnhijklmnoijklmnopjklmnopqklmnopqrlmnopqrsmnopqrstnopqrstu",
+      { 0x09, 0x33, 0x0c, 0x33, 0xf7, 0x11, 0x47, 0xe8,
+        0x3d, 0x19, 0x2f, 0xc7, 0x82, 0xcd, 0x1b, 0x47,
+        0x53, 0x11, 0x1b, 0x17, 0x3b, 0x3b, 0x05, 0xd2,
+        0x2f, 0xa0, 0x80, 0x86, 0xe3, 0xb0, 0xf7, 0x12,
+        0xfc, 0xc7, 0xc7, 0x1a, 0x55, 0x7e, 0x2d, 0xb9,
+        0x66, 0xc3, 0xe9, 0xfa, 0x91, 0x74, 0x60, 0x39 }
+    },
+  };
+
+  int i;
+  unsigned char tmp[48];
+  hash_state md;
+
+  for (i = 0; i < (int)(sizeof(tests) / sizeof(tests[0])); i++) {
+      sha384_init(&md);
+      sha384_process(&md, (unsigned char*)tests[i].msg, (unsigned long)strlen(tests[i].msg));
+      sha384_done(&md, tmp);
+      if (XMEMCMP(tmp, tests[i].hash, 48) != 0) {
+         return CRYPT_FAIL_TESTVECTOR;
+      }
+  }
+  return CRYPT_OK;
+ #endif
+}
+
+#endif /* defined(LTC_SHA384) && defined(LTC_SHA512) */
+
+
+
+/* $Source: /cvs/libtom/libtomcrypt/src/hashes/sha2/sha384.c,v $ */
+/* $Revision: 1.10 $ */
+/* $Date: 2007/05/12 14:25:28 $ */
diff --git a/core/lib/libtomcrypt/src/hashes/sha2/sha512.c b/core/lib/libtomcrypt/src/hashes/sha2/sha512.c
new file mode 100644
index 0000000..1fd2323
--- /dev/null
+++ b/core/lib/libtomcrypt/src/hashes/sha2/sha512.c
@@ -0,0 +1,342 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+   @param sha512.c
+   LTC_SHA512 by Tom St Denis 
+*/
+
+#ifdef LTC_SHA512
+
+const struct ltc_hash_descriptor sha512_desc =
+{
+    "sha512",
+    5,
+    64,
+    128,
+
+    /* OID */
+   { 2, 16, 840, 1, 101, 3, 4, 2, 3,  },
+   9,
+
+    &sha512_init,
+    &sha512_process,
+    &sha512_done,
+    &sha512_test,
+    NULL
+};
+
+/* the K array */
+static const ulong64 K[80] = {
+CONST64(0x428a2f98d728ae22), CONST64(0x7137449123ef65cd), 
+CONST64(0xb5c0fbcfec4d3b2f), CONST64(0xe9b5dba58189dbbc),
+CONST64(0x3956c25bf348b538), CONST64(0x59f111f1b605d019), 
+CONST64(0x923f82a4af194f9b), CONST64(0xab1c5ed5da6d8118),
+CONST64(0xd807aa98a3030242), CONST64(0x12835b0145706fbe), 
+CONST64(0x243185be4ee4b28c), CONST64(0x550c7dc3d5ffb4e2),
+CONST64(0x72be5d74f27b896f), CONST64(0x80deb1fe3b1696b1), 
+CONST64(0x9bdc06a725c71235), CONST64(0xc19bf174cf692694),
+CONST64(0xe49b69c19ef14ad2), CONST64(0xefbe4786384f25e3), 
+CONST64(0x0fc19dc68b8cd5b5), CONST64(0x240ca1cc77ac9c65),
+CONST64(0x2de92c6f592b0275), CONST64(0x4a7484aa6ea6e483), 
+CONST64(0x5cb0a9dcbd41fbd4), CONST64(0x76f988da831153b5),
+CONST64(0x983e5152ee66dfab), CONST64(0xa831c66d2db43210), 
+CONST64(0xb00327c898fb213f), CONST64(0xbf597fc7beef0ee4),
+CONST64(0xc6e00bf33da88fc2), CONST64(0xd5a79147930aa725), 
+CONST64(0x06ca6351e003826f), CONST64(0x142929670a0e6e70),
+CONST64(0x27b70a8546d22ffc), CONST64(0x2e1b21385c26c926), 
+CONST64(0x4d2c6dfc5ac42aed), CONST64(0x53380d139d95b3df),
+CONST64(0x650a73548baf63de), CONST64(0x766a0abb3c77b2a8), 
+CONST64(0x81c2c92e47edaee6), CONST64(0x92722c851482353b),
+CONST64(0xa2bfe8a14cf10364), CONST64(0xa81a664bbc423001),
+CONST64(0xc24b8b70d0f89791), CONST64(0xc76c51a30654be30),
+CONST64(0xd192e819d6ef5218), CONST64(0xd69906245565a910), 
+CONST64(0xf40e35855771202a), CONST64(0x106aa07032bbd1b8),
+CONST64(0x19a4c116b8d2d0c8), CONST64(0x1e376c085141ab53), 
+CONST64(0x2748774cdf8eeb99), CONST64(0x34b0bcb5e19b48a8),
+CONST64(0x391c0cb3c5c95a63), CONST64(0x4ed8aa4ae3418acb), 
+CONST64(0x5b9cca4f7763e373), CONST64(0x682e6ff3d6b2b8a3),
+CONST64(0x748f82ee5defb2fc), CONST64(0x78a5636f43172f60), 
+CONST64(0x84c87814a1f0ab72), CONST64(0x8cc702081a6439ec),
+CONST64(0x90befffa23631e28), CONST64(0xa4506cebde82bde9), 
+CONST64(0xbef9a3f7b2c67915), CONST64(0xc67178f2e372532b),
+CONST64(0xca273eceea26619c), CONST64(0xd186b8c721c0c207), 
+CONST64(0xeada7dd6cde0eb1e), CONST64(0xf57d4f7fee6ed178),
+CONST64(0x06f067aa72176fba), CONST64(0x0a637dc5a2c898a6), 
+CONST64(0x113f9804bef90dae), CONST64(0x1b710b35131c471b),
+CONST64(0x28db77f523047d84), CONST64(0x32caab7b40c72493), 
+CONST64(0x3c9ebe0a15c9bebc), CONST64(0x431d67c49c100d4c),
+CONST64(0x4cc5d4becb3e42b6), CONST64(0x597f299cfc657e2a), 
+CONST64(0x5fcb6fab3ad6faec), CONST64(0x6c44198c4a475817)
+};
+
+/* Various logical functions */
+#define Ch(x,y,z)       (z ^ (x & (y ^ z)))
+#define Maj(x,y,z)      (((x | y) & z) | (x & y)) 
+#define S(x, n)         ROR64c(x, n)
+#define R(x, n)         (((x)&CONST64(0xFFFFFFFFFFFFFFFF))>>((ulong64)n))
+#define Sigma0(x)       (S(x, 28) ^ S(x, 34) ^ S(x, 39))
+#define Sigma1(x)       (S(x, 14) ^ S(x, 18) ^ S(x, 41))
+#define Gamma0(x)       (S(x, 1) ^ S(x, 8) ^ R(x, 7))
+#define Gamma1(x)       (S(x, 19) ^ S(x, 61) ^ R(x, 6))
+
+/* compress 1024-bits */
+#ifdef LTC_CLEAN_STACK
+static int _sha512_compress(hash_state * md, unsigned char *buf)
+#else
+static int  sha512_compress(hash_state * md, unsigned char *buf)
+#endif
+{
+    ulong64 S[8], W[80], t0, t1;
+    int i;
+
+    /* copy state into S */
+    for (i = 0; i < 8; i++) {
+        S[i] = md->sha512.state[i];
+    }
+
+    /* copy the state into 1024-bits into W[0..15] */
+    for (i = 0; i < 16; i++) {
+        LOAD64H(W[i], buf + (8*i));
+    }
+
+    /* fill W[16..79] */
+    for (i = 16; i < 80; i++) {
+        W[i] = Gamma1(W[i - 2]) + W[i - 7] + Gamma0(W[i - 15]) + W[i - 16];
+    }        
+
+    /* Compress */
+#ifdef LTC_SMALL_CODE
+    for (i = 0; i < 80; i++) {
+        t0 = S[7] + Sigma1(S[4]) + Ch(S[4], S[5], S[6]) + K[i] + W[i];
+        t1 = Sigma0(S[0]) + Maj(S[0], S[1], S[2]);
+        S[7] = S[6];
+        S[6] = S[5];
+        S[5] = S[4];
+        S[4] = S[3] + t0;
+        S[3] = S[2];
+        S[2] = S[1];
+        S[1] = S[0];
+        S[0] = t0 + t1;
+    }
+#else
+#define RND(a,b,c,d,e,f,g,h,i)                    \
+     t0 = h + Sigma1(e) + Ch(e, f, g) + K[i] + W[i];   \
+     t1 = Sigma0(a) + Maj(a, b, c);                  \
+     d += t0;                                        \
+     h  = t0 + t1;
+
+     for (i = 0; i < 80; i += 8) {
+         RND(S[0],S[1],S[2],S[3],S[4],S[5],S[6],S[7],i+0);
+         RND(S[7],S[0],S[1],S[2],S[3],S[4],S[5],S[6],i+1);
+         RND(S[6],S[7],S[0],S[1],S[2],S[3],S[4],S[5],i+2);
+         RND(S[5],S[6],S[7],S[0],S[1],S[2],S[3],S[4],i+3);
+         RND(S[4],S[5],S[6],S[7],S[0],S[1],S[2],S[3],i+4);
+         RND(S[3],S[4],S[5],S[6],S[7],S[0],S[1],S[2],i+5);
+         RND(S[2],S[3],S[4],S[5],S[6],S[7],S[0],S[1],i+6);
+         RND(S[1],S[2],S[3],S[4],S[5],S[6],S[7],S[0],i+7);
+     }
+#endif     
+
+
+    /* feedback */
+    for (i = 0; i < 8; i++) {
+        md->sha512.state[i] = md->sha512.state[i] + S[i];
+    }
+
+    return CRYPT_OK;
+}
+
+/* compress 1024-bits */
+#ifdef LTC_CLEAN_STACK
+static int sha512_compress(hash_state * md, unsigned char *buf)
+{
+    int err;
+    err = _sha512_compress(md, buf);
+    burn_stack(sizeof(ulong64) * 90 + sizeof(int));
+    return err;
+}
+#endif
+
+/**
+   Initialize the hash state
+   @param md   The hash state you wish to initialize
+   @return CRYPT_OK if successful
+*/
+int sha512_init(hash_state * md)
+{
+    LTC_ARGCHK(md != NULL);
+    md->sha512.curlen = 0;
+    md->sha512.length = 0;
+    md->sha512.state[0] = CONST64(0x6a09e667f3bcc908);
+    md->sha512.state[1] = CONST64(0xbb67ae8584caa73b);
+    md->sha512.state[2] = CONST64(0x3c6ef372fe94f82b);
+    md->sha512.state[3] = CONST64(0xa54ff53a5f1d36f1);
+    md->sha512.state[4] = CONST64(0x510e527fade682d1);
+    md->sha512.state[5] = CONST64(0x9b05688c2b3e6c1f);
+    md->sha512.state[6] = CONST64(0x1f83d9abfb41bd6b);
+    md->sha512.state[7] = CONST64(0x5be0cd19137e2179);
+    return CRYPT_OK;
+}
+
+/**
+   Process a block of memory though the hash
+   @param md     The hash state
+   @param in     The data to hash
+   @param inlen  The length of the data (octets)
+   @return CRYPT_OK if successful
+*/
+HASH_PROCESS(sha512_process, sha512_compress, sha512, 128)
+
+/**
+   Terminate the hash to get the digest
+   @param md  The hash state
+   @param out [out] The destination of the hash (64 bytes)
+   @return CRYPT_OK if successful
+*/
+int sha512_done(hash_state * md, unsigned char *out)
+{
+    int i;
+
+    LTC_ARGCHK(md  != NULL);
+    LTC_ARGCHK(out != NULL);
+
+    if (md->sha512.curlen >= sizeof(md->sha512.buf)) {
+       return CRYPT_INVALID_ARG;
+    }
+
+    /* increase the length of the message */
+    md->sha512.length += md->sha512.curlen * CONST64(8);
+
+    /* append the '1' bit */
+    md->sha512.buf[md->sha512.curlen++] = (unsigned char)0x80;
+
+    /* if the length is currently above 112 bytes we append zeros
+     * then compress.  Then we can fall back to padding zeros and length
+     * encoding like normal.
+     */
+    if (md->sha512.curlen > 112) {
+        while (md->sha512.curlen < 128) {
+            md->sha512.buf[md->sha512.curlen++] = (unsigned char)0;
+        }
+        sha512_compress(md, md->sha512.buf);
+        md->sha512.curlen = 0;
+    }
+
+    /* pad upto 120 bytes of zeroes 
+     * note: that from 112 to 120 is the 64 MSB of the length.  We assume that you won't hash
+     * > 2^64 bits of data... :-)
+     */
+    while (md->sha512.curlen < 120) {
+        md->sha512.buf[md->sha512.curlen++] = (unsigned char)0;
+    }
+
+    /* store length */
+    STORE64H(md->sha512.length, md->sha512.buf+120);
+    sha512_compress(md, md->sha512.buf);
+
+    /* copy output */
+    for (i = 0; i < 8; i++) {
+        STORE64H(md->sha512.state[i], out+(8*i));
+    }
+#ifdef LTC_CLEAN_STACK
+    zeromem(md, sizeof(hash_state));
+#endif
+    return CRYPT_OK;
+}
+
+/**
+  Self-test the hash
+  @return CRYPT_OK if successful, CRYPT_NOP if self-tests have been disabled
+*/  
+int  sha512_test(void)
+{
+ #ifndef LTC_TEST
+    return CRYPT_NOP;
+ #else    
+  static const struct {
+      const char *msg;
+      unsigned char hash[64];
+  } tests[] = {
+    { "abc",
+     { 0xdd, 0xaf, 0x35, 0xa1, 0x93, 0x61, 0x7a, 0xba,
+       0xcc, 0x41, 0x73, 0x49, 0xae, 0x20, 0x41, 0x31,
+       0x12, 0xe6, 0xfa, 0x4e, 0x89, 0xa9, 0x7e, 0xa2,
+       0x0a, 0x9e, 0xee, 0xe6, 0x4b, 0x55, 0xd3, 0x9a,
+       0x21, 0x92, 0x99, 0x2a, 0x27, 0x4f, 0xc1, 0xa8,
+       0x36, 0xba, 0x3c, 0x23, 0xa3, 0xfe, 0xeb, 0xbd,
+       0x45, 0x4d, 0x44, 0x23, 0x64, 0x3c, 0xe8, 0x0e,
+       0x2a, 0x9a, 0xc9, 0x4f, 0xa5, 0x4c, 0xa4, 0x9f }
+    },
+    { "abcdefghbcdefghicdefghijdefghijkefghijklfghijklmghijklmnhijklmnoijklmnopjklmnopqklmnopqrlmnopqrsmnopqrstnopqrstu",
+     { 0x8e, 0x95, 0x9b, 0x75, 0xda, 0xe3, 0x13, 0xda,
+       0x8c, 0xf4, 0xf7, 0x28, 0x14, 0xfc, 0x14, 0x3f,
+       0x8f, 0x77, 0x79, 0xc6, 0xeb, 0x9f, 0x7f, 0xa1,
+       0x72, 0x99, 0xae, 0xad, 0xb6, 0x88, 0x90, 0x18,
+       0x50, 0x1d, 0x28, 0x9e, 0x49, 0x00, 0xf7, 0xe4,
+       0x33, 0x1b, 0x99, 0xde, 0xc4, 0xb5, 0x43, 0x3a,
+       0xc7, 0xd3, 0x29, 0xee, 0xb6, 0xdd, 0x26, 0x54,
+       0x5e, 0x96, 0xe5, 0x5b, 0x87, 0x4b, 0xe9, 0x09 }
+    },
+  };
+
+  int i;
+  unsigned char tmp[64];
+  hash_state md;
+
+  for (i = 0; i < (int)(sizeof(tests) / sizeof(tests[0])); i++) {
+      sha512_init(&md);
+      sha512_process(&md, (unsigned char *)tests[i].msg, (unsigned long)strlen(tests[i].msg));
+      sha512_done(&md, tmp);
+      if (XMEMCMP(tmp, tests[i].hash, 64) != 0) {
+         return CRYPT_FAIL_TESTVECTOR;
+      }
+  }
+  return CRYPT_OK;
+  #endif
+}
+
+#endif
+
+
+
+
+/* $Source: /cvs/libtom/libtomcrypt/src/hashes/sha2/sha512.c,v $ */
+/* $Revision: 1.10 $ */
+/* $Date: 2007/05/12 14:25:28 $ */
diff --git a/core/lib/libtomcrypt/src/hashes/sha2/sub.mk b/core/lib/libtomcrypt/src/hashes/sha2/sub.mk
new file mode 100644
index 0000000..e6ff9bf
--- /dev/null
+++ b/core/lib/libtomcrypt/src/hashes/sha2/sub.mk
@@ -0,0 +1,17 @@
+srcs-$(CFG_CRYPTO_SHA224) += sha224.c
+
+# SHA-224 needs SHA-256
+SHA256 := $(call cfg-one-enabled, CFG_CRYPTO_SHA224 CFG_CRYPTO_SHA256)
+ifeq ($(SHA256),y)
+SHA256_CE := $(call cfg-one-enabled, CFG_CRYPTO_SHA256_ARM32_CE CFG_CRYPTO_SHA256_ARM64_CE)
+ifeq ($(SHA256_CE),y)
+srcs-y += sha256_armv8a_ce.c
+srcs-$(CFG_CRYPTO_SHA256_ARM32_CE) += sha256_armv8a_ce_a32.S
+srcs-$(CFG_CRYPTO_SHA256_ARM64_CE) += sha256_armv8a_ce_a64.S
+else
+srcs-y += sha256.c
+endif
+endif
+
+srcs-$(CFG_CRYPTO_SHA384) += sha384.c
+srcs-$(CFG_CRYPTO_SHA512) += sha512.c
diff --git a/core/lib/libtomcrypt/src/hashes/sub.mk b/core/lib/libtomcrypt/src/hashes/sub.mk
new file mode 100644
index 0000000..7e897f7
--- /dev/null
+++ b/core/lib/libtomcrypt/src/hashes/sub.mk
@@ -0,0 +1,15 @@
+srcs-$(CFG_CRYPTO_MD5) += md5.c
+
+ifeq ($(CFG_CRYPTO_SHA1),y)
+SHA1_CE := $(call cfg-one-enabled, CFG_CRYPTO_SHA1_ARM32_CE CFG_CRYPTO_SHA1_ARM64_CE)
+ifeq ($(SHA1_CE),y)
+srcs-y += sha1_armv8a_ce.c
+srcs-$(CFG_CRYPTO_SHA1_ARM32_CE) += sha1_armv8a_ce_a32.S
+srcs-$(CFG_CRYPTO_SHA1_ARM64_CE) += sha1_armv8a_ce_a64.S
+else
+srcs-y += sha1.c
+endif
+endif
+
+subdirs-y += helper
+subdirs-y += sha2
diff --git a/core/lib/libtomcrypt/src/mac/hmac/hmac_done.c b/core/lib/libtomcrypt/src/mac/hmac/hmac_done.c
new file mode 100644
index 0000000..5b9e738
--- /dev/null
+++ b/core/lib/libtomcrypt/src/mac/hmac/hmac_done.c
@@ -0,0 +1,135 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file hmac_done.c
+  LTC_HMAC support, terminate stream, Tom St Denis/Dobes Vandermeer
+*/
+
+#ifdef LTC_HMAC
+
+#define LTC_HMAC_BLOCKSIZE hash_descriptor[hash]->blocksize
+
+/**
+   Terminate an LTC_HMAC session
+   @param hmac    The LTC_HMAC state
+   @param out     [out] The destination of the LTC_HMAC authentication tag
+   @param outlen  [in/out]  The max size and resulting size of the LTC_HMAC authentication tag
+   @return CRYPT_OK if successful
+*/
+int hmac_done(hmac_state *hmac, unsigned char *out, unsigned long *outlen)
+{
+    unsigned char *buf, *isha;
+    unsigned long hashsize, i;
+    int hash, err;
+
+    LTC_ARGCHK(hmac  != NULL);
+    LTC_ARGCHK(out   != NULL);
+
+    /* test hash */
+    hash = hmac->hash;
+    if((err = hash_is_valid(hash)) != CRYPT_OK) {
+        return err;
+    }
+
+    /* get the hash message digest size */
+    hashsize = hash_descriptor[hash]->hashsize;
+
+    /* allocate buffers */
+    buf  = XMALLOC(LTC_HMAC_BLOCKSIZE);
+    isha = XMALLOC(hashsize);
+    if (buf == NULL || isha == NULL) { 
+       if (buf != NULL) {
+          XFREE(buf);
+       } 
+       if (isha != NULL) {
+          XFREE(isha);
+       }  
+       return CRYPT_MEM;
+    }
+
+    /* Get the hash of the first LTC_HMAC vector plus the data */
+    if ((err = hash_descriptor[hash]->done(&hmac->md, isha)) != CRYPT_OK) {
+       goto LBL_ERR;
+    }
+
+    /* Create the second LTC_HMAC vector vector for step (3) */
+    for(i=0; i < LTC_HMAC_BLOCKSIZE; i++) {
+        buf[i] = hmac->key[i] ^ 0x5C;
+    }
+
+    /* Now calculate the "outer" hash for step (5), (6), and (7) */
+    if ((err = hash_descriptor[hash]->init(&hmac->md)) != CRYPT_OK) {
+       goto LBL_ERR;
+    }
+    if ((err = hash_descriptor[hash]->process(&hmac->md, buf, LTC_HMAC_BLOCKSIZE)) != CRYPT_OK) {
+       goto LBL_ERR;
+    }
+    if ((err = hash_descriptor[hash]->process(&hmac->md, isha, hashsize)) != CRYPT_OK) {
+       goto LBL_ERR;
+    }
+    if ((err = hash_descriptor[hash]->done(&hmac->md, buf)) != CRYPT_OK) {
+       goto LBL_ERR;
+    }
+
+    /* copy to output  */
+    for (i = 0; i < hashsize && i < *outlen; i++) {
+        out[i] = buf[i];
+    }
+    *outlen = i;
+
+    err = CRYPT_OK;
+LBL_ERR:
+#ifdef LTC_CLEAN_STACK
+    zeromem(isha, hashsize);
+    zeromem(buf,  hashsize);
+    zeromem(hmac, sizeof(*hmac));
+#endif
+
+    XFREE(isha);
+    XFREE(buf);
+
+    return err;
+}
+
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/mac/hmac/hmac_done.c,v $ */
+/* $Revision: 1.7 $ */
+/* $Date: 2007/05/12 14:37:41 $ */
diff --git a/core/lib/libtomcrypt/src/mac/hmac/hmac_file.c b/core/lib/libtomcrypt/src/mac/hmac/hmac_file.c
new file mode 100644
index 0000000..03cc140
--- /dev/null
+++ b/core/lib/libtomcrypt/src/mac/hmac/hmac_file.c
@@ -0,0 +1,120 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file hmac_file.c
+  LTC_HMAC support, process a file, Tom St Denis/Dobes Vandermeer
+*/
+
+#ifdef LTC_HMAC
+
+/**
+  LTC_HMAC a file
+  @param hash     The index of the hash you wish to use
+  @param fname    The name of the file you wish to LTC_HMAC
+  @param key      The secret key
+  @param keylen   The length of the secret key
+  @param out      [out] The LTC_HMAC authentication tag
+  @param outlen   [in/out]  The max size and resulting size of the authentication tag
+  @return CRYPT_OK if successful, CRYPT_NOP if file support has been disabled
+*/
+int hmac_file(int hash, const char *fname, 
+              const unsigned char *key, unsigned long keylen, 
+                    unsigned char *out, unsigned long *outlen)
+{
+#ifdef LTC_NO_FILE
+    return CRYPT_NOP;
+#else
+   hmac_state hmac;
+   FILE *in;
+   unsigned char buf[512];
+   size_t x;
+   int err;
+
+   LTC_ARGCHK(fname  != NULL);
+   LTC_ARGCHK(key    != NULL);
+   LTC_ARGCHK(out    != NULL);
+   LTC_ARGCHK(outlen != NULL);
+   
+   if((err = hash_is_valid(hash)) != CRYPT_OK) {
+       return err;
+   }
+
+   if ((err = hmac_init(&hmac, hash, key, keylen)) != CRYPT_OK) {
+       return err;
+   }
+
+   in = fopen(fname, "rb");
+   if (in == NULL) {
+      return CRYPT_FILE_NOTFOUND;
+   }
+
+   /* process the file contents */
+   do {
+      x = fread(buf, 1, sizeof(buf), in);
+      if ((err = hmac_process(&hmac, buf, (unsigned long)x)) != CRYPT_OK) {
+         /* we don't trap this error since we're already returning an error! */
+         fclose(in);
+         return err;
+      }
+   } while (x == sizeof(buf));
+
+   if (fclose(in) != 0) {
+      return CRYPT_ERROR;
+   }
+
+   /* get final hmac */
+   if ((err = hmac_done(&hmac, out, outlen)) != CRYPT_OK) {
+      return err;
+   }
+
+#ifdef LTC_CLEAN_STACK
+   /* clear memory */
+   zeromem(buf, sizeof(buf));
+#endif   
+   return CRYPT_OK;
+#endif
+}
+
+#endif
+
+
+/* $Source: /cvs/libtom/libtomcrypt/src/mac/hmac/hmac_file.c,v $ */
+/* $Revision: 1.7 $ */
+/* $Date: 2007/05/12 14:37:41 $ */
diff --git a/core/lib/libtomcrypt/src/mac/hmac/hmac_init.c b/core/lib/libtomcrypt/src/mac/hmac/hmac_init.c
new file mode 100644
index 0000000..abc5bf3
--- /dev/null
+++ b/core/lib/libtomcrypt/src/mac/hmac/hmac_init.c
@@ -0,0 +1,134 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file hmac_init.c
+  LTC_HMAC support, initialize state, Tom St Denis/Dobes Vandermeer 
+*/
+
+#ifdef LTC_HMAC
+
+#define LTC_HMAC_BLOCKSIZE hash_descriptor[hash]->blocksize
+
+/**
+   Initialize an LTC_HMAC context.
+   @param hmac     The LTC_HMAC state 
+   @param hash     The index of the hash you want to use 
+   @param key      The secret key
+   @param keylen   The length of the secret key (octets)
+   @return CRYPT_OK if successful
+*/
+int hmac_init(hmac_state *hmac, int hash, const unsigned char *key, unsigned long keylen)
+{
+    unsigned char *buf;
+    unsigned long hashsize;
+    unsigned long i, z;
+    int err;
+
+    LTC_ARGCHK(hmac != NULL);
+    LTC_ARGCHK(key  != NULL);
+
+    /* valid hash? */
+    if ((err = hash_is_valid(hash)) != CRYPT_OK) {
+        return err;
+    }
+    hmac->hash = hash;
+    hashsize   = hash_descriptor[hash]->hashsize;
+
+    /* valid key length? */
+    if (keylen == 0) {
+        return CRYPT_INVALID_KEYSIZE;
+    }
+
+    /* allocate ram for buf */
+    buf = XMALLOC(LTC_HMAC_BLOCKSIZE);
+    if (buf == NULL) {
+       return CRYPT_MEM;
+    }
+
+    /* check hash blocks fits */
+    if (sizeof(hmac->key) < LTC_HMAC_BLOCKSIZE) {
+        err = CRYPT_BUFFER_OVERFLOW;
+        goto LBL_ERR;
+    }
+
+    /* (1) make sure we have a large enough key */
+    if(keylen > LTC_HMAC_BLOCKSIZE) {
+        z = LTC_HMAC_BLOCKSIZE;
+        if ((err = hash_memory(hash, key, keylen, hmac->key, &z)) != CRYPT_OK) {
+           goto LBL_ERR;
+        }
+        keylen = hashsize;
+    } else {
+        XMEMCPY(hmac->key, key, (size_t)keylen);
+    }
+
+    if(keylen < LTC_HMAC_BLOCKSIZE) {
+       zeromem((hmac->key) + keylen, (size_t)(LTC_HMAC_BLOCKSIZE - keylen));
+    }
+
+    /* Create the initial vector for step (3) */
+    for(i=0; i < LTC_HMAC_BLOCKSIZE;   i++) {
+       buf[i] = hmac->key[i] ^ 0x36;
+    }
+
+    /* Pre-pend that to the hash data */
+    if ((err = hash_descriptor[hash]->init(&hmac->md)) != CRYPT_OK) {
+       goto LBL_ERR;
+    }
+
+    if ((err = hash_descriptor[hash]->process(&hmac->md, buf, LTC_HMAC_BLOCKSIZE)) != CRYPT_OK) {
+       goto LBL_ERR;
+    }
+    goto done;
+LBL_ERR:
+done:
+#ifdef LTC_CLEAN_STACK
+   zeromem(buf, LTC_HMAC_BLOCKSIZE);
+#endif
+ 
+   XFREE(buf);
+   return err;    
+}
+
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/mac/hmac/hmac_init.c,v $ */
+/* $Revision: 1.7 $ */
+/* $Date: 2007/05/12 14:37:41 $ */
diff --git a/core/lib/libtomcrypt/src/mac/hmac/hmac_memory.c b/core/lib/libtomcrypt/src/mac/hmac/hmac_memory.c
new file mode 100644
index 0000000..1d2e1ef
--- /dev/null
+++ b/core/lib/libtomcrypt/src/mac/hmac/hmac_memory.c
@@ -0,0 +1,115 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file hmac_memory.c
+  LTC_HMAC support, process a block of memory, Tom St Denis/Dobes Vandermeer
+*/
+
+#ifdef LTC_HMAC
+
+/**
+   LTC_HMAC a block of memory to produce the authentication tag
+   @param hash      The index of the hash to use 
+   @param key       The secret key 
+   @param keylen    The length of the secret key (octets)
+   @param in        The data to LTC_HMAC
+   @param inlen     The length of the data to LTC_HMAC (octets)
+   @param out       [out] Destination of the authentication tag
+   @param outlen    [in/out] Max size and resulting size of authentication tag
+   @return CRYPT_OK if successful
+*/
+int hmac_memory(int hash, 
+                const unsigned char *key,  unsigned long keylen,
+                const unsigned char *in,   unsigned long inlen, 
+                      unsigned char *out,  unsigned long *outlen)
+{
+    hmac_state *hmac;
+    int         err;
+
+    LTC_ARGCHK(key    != NULL);
+    LTC_ARGCHK(in     != NULL);
+    LTC_ARGCHK(out    != NULL); 
+    LTC_ARGCHK(outlen != NULL);
+
+    /* make sure hash descriptor is valid */
+    if ((err = hash_is_valid(hash)) != CRYPT_OK) {
+       return err;
+    }
+
+    /* is there a descriptor? */
+    if (hash_descriptor[hash]->hmac_block != NULL) {
+        return hash_descriptor[hash]->hmac_block(key, keylen, in, inlen, out, outlen);
+    }
+
+    /* nope, so call the hmac functions */
+    /* allocate ram for hmac state */
+    hmac = XMALLOC(sizeof(hmac_state));
+    if (hmac == NULL) {
+       return CRYPT_MEM;
+    }
+
+    if ((err = hmac_init(hmac, hash, key, keylen)) != CRYPT_OK) {
+       goto LBL_ERR;
+    }
+
+    if ((err = hmac_process(hmac, in, inlen)) != CRYPT_OK) {
+       goto LBL_ERR;
+    }
+
+    if ((err = hmac_done(hmac, out, outlen)) != CRYPT_OK) {
+       goto LBL_ERR;
+    }
+
+   err = CRYPT_OK;
+LBL_ERR:
+#ifdef LTC_CLEAN_STACK
+   zeromem(hmac, sizeof(hmac_state));
+#endif
+
+   XFREE(hmac);
+   return err;   
+}
+
+#endif
+
+
+/* $Source: /cvs/libtom/libtomcrypt/src/mac/hmac/hmac_memory.c,v $ */
+/* $Revision: 1.8 $ */
+/* $Date: 2007/05/12 14:37:41 $ */
diff --git a/core/lib/libtomcrypt/src/mac/hmac/hmac_memory_multi.c b/core/lib/libtomcrypt/src/mac/hmac/hmac_memory_multi.c
new file mode 100644
index 0000000..95d7d1e
--- /dev/null
+++ b/core/lib/libtomcrypt/src/mac/hmac/hmac_memory_multi.c
@@ -0,0 +1,119 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+#include <stdarg.h>
+
+/**
+  @file hmac_memory_multi.c
+  LTC_HMAC support, process multiple blocks of memory, Tom St Denis/Dobes Vandermeer
+*/
+
+#ifdef LTC_HMAC
+
+/**
+   LTC_HMAC multiple blocks of memory to produce the authentication tag
+   @param hash      The index of the hash to use 
+   @param key       The secret key 
+   @param keylen    The length of the secret key (octets)
+   @param out       [out] Destination of the authentication tag
+   @param outlen    [in/out] Max size and resulting size of authentication tag
+   @param in        The data to LTC_HMAC
+   @param inlen     The length of the data to LTC_HMAC (octets)
+   @param ...       tuples of (data,len) pairs to LTC_HMAC, terminated with a (NULL,x) (x=don't care)
+   @return CRYPT_OK if successful
+*/
+int hmac_memory_multi(int hash, 
+                const unsigned char *key,  unsigned long keylen,
+                      unsigned char *out,  unsigned long *outlen,
+                const unsigned char *in,   unsigned long inlen, ...)
+
+{
+    hmac_state          *hmac;
+    int                  err;
+    va_list              args;
+    const unsigned char *curptr;
+    unsigned long        curlen;
+
+    LTC_ARGCHK(key    != NULL);
+    LTC_ARGCHK(in     != NULL);
+    LTC_ARGCHK(out    != NULL); 
+    LTC_ARGCHK(outlen != NULL);
+
+    /* allocate ram for hmac state */
+    hmac = XMALLOC(sizeof(hmac_state));
+    if (hmac == NULL) {
+       return CRYPT_MEM;
+    }
+
+    if ((err = hmac_init(hmac, hash, key, keylen)) != CRYPT_OK) {
+       goto LBL_ERR;
+    }
+
+    va_start(args, inlen);
+    curptr = in; 
+    curlen = inlen;
+    for (;;) {
+       /* process buf */
+       if ((err = hmac_process(hmac, curptr, curlen)) != CRYPT_OK) {
+          goto LBL_ERR;
+       }
+       /* step to next */
+       curptr = va_arg(args, const unsigned char*);
+       if (curptr == NULL) {
+          break;
+       }
+       curlen = va_arg(args, unsigned long);
+    }
+    if ((err = hmac_done(hmac, out, outlen)) != CRYPT_OK) {
+       goto LBL_ERR;
+    }
+LBL_ERR:
+#ifdef LTC_CLEAN_STACK
+   zeromem(hmac, sizeof(hmac_state));
+#endif
+   XFREE(hmac);
+   va_end(args);
+   return err;   
+}
+
+#endif
+
+
+/* $Source: /cvs/libtom/libtomcrypt/src/mac/hmac/hmac_memory_multi.c,v $ */
+/* $Revision: 1.7 $ */
+/* $Date: 2007/05/12 14:37:41 $ */
diff --git a/core/lib/libtomcrypt/src/mac/hmac/hmac_process.c b/core/lib/libtomcrypt/src/mac/hmac/hmac_process.c
new file mode 100644
index 0000000..6b6f62d
--- /dev/null
+++ b/core/lib/libtomcrypt/src/mac/hmac/hmac_process.c
@@ -0,0 +1,70 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file hmac_process.c
+  LTC_HMAC support, process data, Tom St Denis/Dobes Vandermeer
+*/
+
+#ifdef LTC_HMAC
+
+/** 
+  Process data through LTC_HMAC
+  @param hmac    The hmac state
+  @param in      The data to send through LTC_HMAC
+  @param inlen   The length of the data to LTC_HMAC (octets)
+  @return CRYPT_OK if successful
+*/
+int hmac_process(hmac_state *hmac, const unsigned char *in, unsigned long inlen)
+{
+    int err;
+    LTC_ARGCHK(hmac != NULL);
+    LTC_ARGCHK(in != NULL);
+    if ((err = hash_is_valid(hmac->hash)) != CRYPT_OK) {
+        return err;
+    }
+    return hash_descriptor[hmac->hash]->process(&hmac->md, in, inlen);
+}
+
+#endif
+
+
+/* $Source: /cvs/libtom/libtomcrypt/src/mac/hmac/hmac_process.c,v $ */
+/* $Revision: 1.7 $ */
+/* $Date: 2007/05/12 14:37:41 $ */
diff --git a/core/lib/libtomcrypt/src/mac/hmac/sub.mk b/core/lib/libtomcrypt/src/mac/hmac/sub.mk
new file mode 100644
index 0000000..e1c3627
--- /dev/null
+++ b/core/lib/libtomcrypt/src/mac/hmac/sub.mk
@@ -0,0 +1,9 @@
+cflags-y += -Wno-unused-parameter
+
+srcs-y += hmac_done.c
+# srcs-y += hmac_file.c
+srcs-y += hmac_init.c
+srcs-y += hmac_memory.c
+srcs-y += hmac_memory_multi.c
+srcs-y += hmac_process.c
+# srcs-y += hmac_test.c
diff --git a/core/lib/libtomcrypt/src/mac/omac/omac_done.c b/core/lib/libtomcrypt/src/mac/omac/omac_done.c
new file mode 100644
index 0000000..3bb62a2
--- /dev/null
+++ b/core/lib/libtomcrypt/src/mac/omac/omac_done.c
@@ -0,0 +1,113 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/** 
+  @file omac_done.c
+  LTC_OMAC1 support, terminate a stream, Tom St Denis
+*/
+
+#ifdef LTC_OMAC
+
+/**
+  Terminate an LTC_OMAC stream
+  @param omac   The LTC_OMAC state
+  @param out    [out] Destination for the authentication tag
+  @param outlen [in/out]  The max size and resulting size of the authentication tag
+  @return CRYPT_OK if successful
+*/
+int omac_done(omac_state *omac, unsigned char *out, unsigned long *outlen)
+{
+   int       err, mode;
+   unsigned  x;
+
+   LTC_ARGCHK(omac   != NULL);
+   LTC_ARGCHK(out    != NULL);
+   LTC_ARGCHK(outlen != NULL);
+   if ((err = cipher_is_valid(omac->cipher_idx)) != CRYPT_OK) {
+      return err;
+   }
+
+   if ((omac->buflen > (int)sizeof(omac->block)) || (omac->buflen < 0) ||
+       (omac->blklen > (int)sizeof(omac->block)) || (omac->buflen > omac->blklen)) {
+      return CRYPT_INVALID_ARG;
+   }
+
+   /* figure out mode */
+   if (omac->buflen != omac->blklen) {
+      /* add the 0x80 byte */
+      omac->block[omac->buflen++] = 0x80;
+
+      /* pad with 0x00 */
+      while (omac->buflen < omac->blklen) {
+         omac->block[omac->buflen++] = 0x00;
+      }
+      mode = 1;
+   } else {
+      mode = 0;
+   }
+
+   /* now xor prev + Lu[mode] */
+   for (x = 0; x < (unsigned)omac->blklen; x++) {
+       omac->block[x] ^= omac->prev[x] ^ omac->Lu[mode][x];
+   }
+
+   /* encrypt it */
+   if ((err = cipher_descriptor[omac->cipher_idx]->ecb_encrypt(omac->block, omac->block, &omac->key)) != CRYPT_OK) {
+      return err;
+   }
+   cipher_descriptor[omac->cipher_idx]->done(&omac->key);
+ 
+   /* output it */
+   for (x = 0; x < (unsigned)omac->blklen && x < *outlen; x++) {
+       out[x] = omac->block[x];
+   }
+   *outlen = x;
+
+#ifdef LTC_CLEAN_STACK
+   zeromem(omac, sizeof(*omac));
+#endif
+   return CRYPT_OK;
+}
+
+#endif
+
+
+/* $Source: /cvs/libtom/libtomcrypt/src/mac/omac/omac_done.c,v $ */
+/* $Revision: 1.9 $ */
+/* $Date: 2007/05/12 14:37:41 $ */
diff --git a/core/lib/libtomcrypt/src/mac/omac/omac_file.c b/core/lib/libtomcrypt/src/mac/omac/omac_file.c
new file mode 100644
index 0000000..569e52a
--- /dev/null
+++ b/core/lib/libtomcrypt/src/mac/omac/omac_file.c
@@ -0,0 +1,110 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/** 
+  @file omac_file.c
+  LTC_OMAC1 support, process a file, Tom St Denis
+*/
+
+#ifdef LTC_OMAC
+
+/**
+   LTC_OMAC a file
+   @param cipher   The index of the cipher desired
+   @param key      The secret key
+   @param keylen   The length of the secret key (octets)
+   @param filename The name of the file you wish to LTC_OMAC
+   @param out      [out] Where the authentication tag is to be stored
+   @param outlen   [in/out] The max size and resulting size of the authentication tag
+   @return CRYPT_OK if successful, CRYPT_NOP if file support has been disabled
+*/
+int omac_file(int cipher, 
+              const unsigned char *key, unsigned long keylen,
+              const char *filename, 
+                    unsigned char *out, unsigned long *outlen)
+{
+#ifdef LTC_NO_FILE
+   return CRYPT_NOP;
+#else
+   int err, x;
+   omac_state omac;
+   FILE *in;
+   unsigned char buf[512];
+
+   LTC_ARGCHK(key      != NULL);
+   LTC_ARGCHK(filename != NULL);
+   LTC_ARGCHK(out      != NULL);
+   LTC_ARGCHK(outlen   != NULL);
+
+   in = fopen(filename, "rb");
+   if (in == NULL) {
+      return CRYPT_FILE_NOTFOUND;
+   }
+
+   if ((err = omac_init(&omac, cipher, key, keylen)) != CRYPT_OK) {
+      fclose(in);
+      return err;
+   }
+
+   do {
+      x = fread(buf, 1, sizeof(buf), in);
+      if ((err = omac_process(&omac, buf, x)) != CRYPT_OK) {
+         fclose(in);
+         return err;
+      }
+   } while (x == sizeof(buf));
+   fclose(in);
+
+   if ((err = omac_done(&omac, out, outlen)) != CRYPT_OK) {
+      return err;
+   }
+
+#ifdef LTC_CLEAN_STACK
+   zeromem(buf, sizeof(buf));
+#endif
+
+   return CRYPT_OK;
+#endif
+}
+
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/mac/omac/omac_file.c,v $ */
+/* $Revision: 1.7 $ */
+/* $Date: 2007/05/12 14:37:41 $ */
diff --git a/core/lib/libtomcrypt/src/mac/omac/omac_init.c b/core/lib/libtomcrypt/src/mac/omac/omac_init.c
new file mode 100644
index 0000000..a5c7be0
--- /dev/null
+++ b/core/lib/libtomcrypt/src/mac/omac/omac_init.c
@@ -0,0 +1,128 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/** 
+  @file omac_init.c
+  LTC_OMAC1 support, initialize state, by Tom St Denis
+*/
+
+
+#ifdef LTC_OMAC
+
+/**
+   Initialize an LTC_OMAC state
+   @param omac    The LTC_OMAC state to initialize
+   @param cipher  The index of the desired cipher
+   @param key     The secret key
+   @param keylen  The length of the secret key (octets)
+   @return CRYPT_OK if successful
+*/
+int omac_init(omac_state *omac, int cipher, const unsigned char *key, unsigned long keylen)
+{
+   int err, x, y, mask, msb, len;
+
+   LTC_ARGCHK(omac != NULL);
+   LTC_ARGCHK(key  != NULL);
+
+   /* schedule the key */
+   if ((err = cipher_is_valid(cipher)) != CRYPT_OK) {
+      return err;
+   }
+
+#ifdef LTC_FAST
+   if (cipher_descriptor[cipher]->block_length % sizeof(LTC_FAST_TYPE)) {
+       return CRYPT_INVALID_ARG;
+   }
+#endif
+
+   /* now setup the system */
+   switch (cipher_descriptor[cipher]->block_length) {
+       case 8:  mask = 0x1B;
+                len  = 8;
+                break;
+       case 16: mask = 0x87;
+                len  = 16;
+                break;
+       default: return CRYPT_INVALID_ARG;
+   }
+
+   if ((err = cipher_descriptor[cipher]->setup(key, keylen, 0, &omac->key)) != CRYPT_OK) {
+      return err;
+   }
+
+   /* ok now we need Lu and Lu^2 [calc one from the other] */
+
+   /* first calc L which is Ek(0) */
+   zeromem(omac->Lu[0], cipher_descriptor[cipher]->block_length);
+   if ((err = cipher_descriptor[cipher]->ecb_encrypt(omac->Lu[0], omac->Lu[0], &omac->key)) != CRYPT_OK) {
+      return err;
+   }
+
+   /* now do the mults, whoopy! */
+   for (x = 0; x < 2; x++) {
+       /* if msb(L * u^(x+1)) = 0 then just shift, otherwise shift and xor constant mask */
+       msb = omac->Lu[x][0] >> 7;
+
+       /* shift left */
+       for (y = 0; y < (len - 1); y++) {
+           omac->Lu[x][y] = ((omac->Lu[x][y] << 1) | (omac->Lu[x][y+1] >> 7)) & 255;
+       }
+       omac->Lu[x][len - 1] = ((omac->Lu[x][len - 1] << 1) ^ (msb ? mask : 0)) & 255;
+ 
+       /* copy up as require */
+       if (x == 0) {
+          XMEMCPY(omac->Lu[1], omac->Lu[0], sizeof(omac->Lu[0]));
+       }
+   }
+
+   /* setup state */
+   omac->cipher_idx = cipher;
+   omac->buflen     = 0;
+   omac->blklen     = len;
+   zeromem(omac->prev,  sizeof(omac->prev));
+   zeromem(omac->block, sizeof(omac->block));
+
+   return CRYPT_OK;
+}
+
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/mac/omac/omac_init.c,v $ */
+/* $Revision: 1.12 $ */
+/* $Date: 2007/05/12 14:37:41 $ */
diff --git a/core/lib/libtomcrypt/src/mac/omac/omac_memory.c b/core/lib/libtomcrypt/src/mac/omac/omac_memory.c
new file mode 100644
index 0000000..594d4fb
--- /dev/null
+++ b/core/lib/libtomcrypt/src/mac/omac/omac_memory.c
@@ -0,0 +1,112 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/** 
+  @file omac_memory.c
+  LTC_OMAC1 support, process a block of memory, Tom St Denis
+*/
+
+#ifdef LTC_OMAC
+
+/**
+   LTC_OMAC a block of memory 
+   @param cipher    The index of the desired cipher
+   @param key       The secret key
+   @param keylen    The length of the secret key (octets)
+   @param in        The data to send through LTC_OMAC
+   @param inlen     The length of the data to send through LTC_OMAC (octets)
+   @param out       [out] The destination of the authentication tag
+   @param outlen    [in/out]  The max size and resulting size of the authentication tag (octets)
+   @return CRYPT_OK if successful
+*/
+int omac_memory(int cipher, 
+                const unsigned char *key, unsigned long keylen,
+                const unsigned char *in,  unsigned long inlen,
+                      unsigned char *out, unsigned long *outlen)
+{
+   int err;
+   omac_state *omac;
+
+   LTC_ARGCHK(key    != NULL);
+   LTC_ARGCHK(in     != NULL);
+   LTC_ARGCHK(out    != NULL);
+   LTC_ARGCHK(outlen != NULL);
+
+   /* is the cipher valid? */
+   if ((err = cipher_is_valid(cipher)) != CRYPT_OK) {
+      return err;
+   }
+
+   /* Use accelerator if found */
+   if (cipher_descriptor[cipher]->omac_memory != NULL) {
+      return cipher_descriptor[cipher]->omac_memory(key, keylen, in, inlen, out, outlen);
+   }
+
+   /* allocate ram for omac state */
+   omac = XMALLOC(sizeof(omac_state));
+   if (omac == NULL) {
+      return CRYPT_MEM;
+   }
+
+   /* omac process the message */
+   if ((err = omac_init(omac, cipher, key, keylen)) != CRYPT_OK) {
+      goto LBL_ERR;
+   }
+   if ((err = omac_process(omac, in, inlen)) != CRYPT_OK) {
+      goto LBL_ERR;
+   }
+   if ((err = omac_done(omac, out, outlen)) != CRYPT_OK) {
+      goto LBL_ERR;
+   }
+
+   err = CRYPT_OK;
+LBL_ERR:
+#ifdef LTC_CLEAN_STACK
+   zeromem(omac, sizeof(omac_state));
+#endif
+
+   XFREE(omac);
+   return err;   
+}
+
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/mac/omac/omac_memory.c,v $ */
+/* $Revision: 1.8 $ */
+/* $Date: 2007/05/12 14:37:41 $ */
diff --git a/core/lib/libtomcrypt/src/mac/omac/omac_memory_multi.c b/core/lib/libtomcrypt/src/mac/omac/omac_memory_multi.c
new file mode 100644
index 0000000..3739b8d
--- /dev/null
+++ b/core/lib/libtomcrypt/src/mac/omac/omac_memory_multi.c
@@ -0,0 +1,117 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+#include <stdarg.h>
+
+/** 
+  @file omac_memory_multi.c
+  LTC_OMAC1 support, process multiple blocks of memory, Tom St Denis
+*/
+
+#ifdef LTC_OMAC
+
+/**
+   LTC_OMAC multiple blocks of memory 
+   @param cipher    The index of the desired cipher
+   @param key       The secret key
+   @param keylen    The length of the secret key (octets)
+   @param out       [out] The destination of the authentication tag
+   @param outlen    [in/out]  The max size and resulting size of the authentication tag (octets)
+   @param in        The data to send through LTC_OMAC
+   @param inlen     The length of the data to send through LTC_OMAC (octets)
+   @param ...       tuples of (data,len) pairs to LTC_OMAC, terminated with a (NULL,x) (x=don't care)
+   @return CRYPT_OK if successful
+*/
+int omac_memory_multi(int cipher, 
+                const unsigned char *key, unsigned long keylen,
+                      unsigned char *out, unsigned long *outlen,
+                const unsigned char *in,  unsigned long inlen, ...)
+{
+   int                  err;
+   omac_state          *omac;
+   va_list              args;
+   const unsigned char *curptr;
+   unsigned long        curlen;
+
+   LTC_ARGCHK(key    != NULL);
+   LTC_ARGCHK(in     != NULL);
+   LTC_ARGCHK(out    != NULL);
+   LTC_ARGCHK(outlen != NULL);
+
+   /* allocate ram for omac state */
+   omac = XMALLOC(sizeof(omac_state));
+   if (omac == NULL) {
+      return CRYPT_MEM;
+   }
+
+   /* omac process the message */
+   if ((err = omac_init(omac, cipher, key, keylen)) != CRYPT_OK) {
+      goto LBL_ERR;
+   }
+   va_start(args, inlen);
+   curptr = in; 
+   curlen = inlen;
+   for (;;) {
+      /* process buf */
+      if ((err = omac_process(omac, curptr, curlen)) != CRYPT_OK) {
+         goto LBL_ERR;
+      }
+      /* step to next */
+      curptr = va_arg(args, const unsigned char*);
+      if (curptr == NULL) {
+         break;
+      }
+      curlen = va_arg(args, unsigned long);
+   }
+   if ((err = omac_done(omac, out, outlen)) != CRYPT_OK) {
+      goto LBL_ERR;
+   }
+LBL_ERR:
+#ifdef LTC_CLEAN_STACK
+   zeromem(omac, sizeof(omac_state));
+#endif
+   XFREE(omac);
+   va_end(args);
+   return err;   
+}
+
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/mac/omac/omac_memory_multi.c,v $ */
+/* $Revision: 1.7 $ */
+/* $Date: 2007/05/12 14:37:41 $ */
diff --git a/core/lib/libtomcrypt/src/mac/omac/omac_process.c b/core/lib/libtomcrypt/src/mac/omac/omac_process.c
new file mode 100644
index 0000000..2e1e2cc
--- /dev/null
+++ b/core/lib/libtomcrypt/src/mac/omac/omac_process.c
@@ -0,0 +1,116 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/** 
+  @file omac_process.c
+  LTC_OMAC1 support, process data, Tom St Denis
+*/
+
+
+#ifdef LTC_OMAC
+
+/** 
+   Process data through LTC_OMAC
+   @param omac     The LTC_OMAC state
+   @param in       The input data to send through LTC_OMAC
+   @param inlen    The length of the input (octets)
+   @return CRYPT_OK if successful
+*/
+int omac_process(omac_state *omac, const unsigned char *in, unsigned long inlen)
+{
+   unsigned long n, x;
+   int           err;
+
+   LTC_ARGCHK(omac  != NULL);
+   LTC_ARGCHK(in    != NULL);
+   if ((err = cipher_is_valid(omac->cipher_idx)) != CRYPT_OK) {
+      return err;
+   }
+
+   if ((omac->buflen > (int)sizeof(omac->block)) || (omac->buflen < 0) ||
+       (omac->blklen > (int)sizeof(omac->block)) || (omac->buflen > omac->blklen)) {
+      return CRYPT_INVALID_ARG;
+   }
+
+#ifdef LTC_FAST
+   unsigned long blklen = cipher_descriptor[omac->cipher_idx]->block_length;
+   if (omac->buflen == 0 && inlen > blklen) {
+      unsigned long y;
+      for (x = 0; x < (inlen - blklen); x += blklen) {
+          for (y = 0; y < blklen; y += sizeof(LTC_FAST_TYPE)) {
+              *((LTC_FAST_TYPE*)(&omac->prev[y])) ^= *((LTC_FAST_TYPE*)(&in[y]));
+          }
+          in += blklen;
+          if ((err = cipher_descriptor[omac->cipher_idx]->ecb_encrypt(omac->prev, omac->prev, &omac->key)) != CRYPT_OK) {
+             return err;
+          }
+      }
+      inlen -= x;
+    }
+#endif
+
+   while (inlen != 0) { 
+       /* ok if the block is full we xor in prev, encrypt and replace prev */
+       if (omac->buflen == omac->blklen) {
+          for (x = 0; x < (unsigned long)omac->blklen; x++) {
+              omac->block[x] ^= omac->prev[x];
+          }
+          if ((err = cipher_descriptor[omac->cipher_idx]->ecb_encrypt(omac->block, omac->prev, &omac->key)) != CRYPT_OK) {
+             return err;
+          }
+          omac->buflen = 0;
+       }
+
+       /* add bytes */
+       n = MIN(inlen, (unsigned long)(omac->blklen - omac->buflen));
+       XMEMCPY(omac->block + omac->buflen, in, n);
+       omac->buflen  += n;
+       inlen         -= n;
+       in            += n;
+   }
+
+   return CRYPT_OK;
+}
+
+#endif
+
+
+/* $Source: /cvs/libtom/libtomcrypt/src/mac/omac/omac_process.c,v $ */
+/* $Revision: 1.13 $ */
+/* $Date: 2007/05/12 14:37:41 $ */
diff --git a/core/lib/libtomcrypt/src/mac/omac/sub.mk b/core/lib/libtomcrypt/src/mac/omac/sub.mk
new file mode 100644
index 0000000..076bd0f
--- /dev/null
+++ b/core/lib/libtomcrypt/src/mac/omac/sub.mk
@@ -0,0 +1,9 @@
+cflags-y += -Wno-unused-parameter
+
+srcs-y += omac_done.c
+# srcs-y += omac_file.c
+srcs-y += omac_init.c
+srcs-y += omac_memory.c
+srcs-y += omac_memory_multi.c
+srcs-y += omac_process.c
+# srcs-y += omac_test.c
diff --git a/core/lib/libtomcrypt/src/mac/pmac/pmac_done.c b/core/lib/libtomcrypt/src/mac/pmac/pmac_done.c
new file mode 100644
index 0000000..49ce0e2
--- /dev/null
+++ b/core/lib/libtomcrypt/src/mac/pmac/pmac_done.c
@@ -0,0 +1,101 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/** 
+  @file pmac_done.c
+  PMAC implementation, terminate a session, by Tom St Denis 
+*/
+
+#ifdef LTC_PMAC
+
+int pmac_done(pmac_state *state, unsigned char *out, unsigned long *outlen)
+{
+   int err, x;
+
+   LTC_ARGCHK(state != NULL);
+   LTC_ARGCHK(out   != NULL);
+   if ((err = cipher_is_valid(state->cipher_idx)) != CRYPT_OK) {
+      return err;
+   }
+
+   if ((state->buflen > (int)sizeof(state->block)) || (state->buflen < 0) ||
+       (state->block_len > (int)sizeof(state->block)) || (state->buflen > state->block_len)) {
+      return CRYPT_INVALID_ARG;
+   }
+
+
+   /* handle padding.  If multiple xor in L/x */
+
+   if (state->buflen == state->block_len) {
+      /* xor Lr against the checksum */
+      for (x = 0; x < state->block_len; x++) {
+          state->checksum[x] ^= state->block[x] ^ state->Lr[x];
+      }
+   } else {
+      /* otherwise xor message bytes then the 0x80 byte */
+      for (x = 0; x < state->buflen; x++) {
+          state->checksum[x] ^= state->block[x];
+      }
+      state->checksum[x] ^= 0x80;
+   }
+
+   /* encrypt it */
+   if ((err = cipher_descriptor[state->cipher_idx].ecb_encrypt(state->checksum, state->checksum, &state->key)) != CRYPT_OK) {
+      return err;
+   }
+   cipher_descriptor[state->cipher_idx].done(&state->key);
+
+   /* store it */
+   for (x = 0; x < state->block_len && x < (int)*outlen; x++) {
+       out[x] = state->checksum[x];
+   }
+   *outlen = x;
+
+#ifdef LTC_CLEAN_STACK
+   zeromem(state, sizeof(*state));
+#endif
+   return CRYPT_OK;
+}
+
+#endif
+
+
+/* $Source: /cvs/libtom/libtomcrypt/src/mac/pmac/pmac_done.c,v $ */
+/* $Revision: 1.9 $ */
+/* $Date: 2006/12/28 01:27:23 $ */
diff --git a/core/lib/libtomcrypt/src/mac/pmac/pmac_file.c b/core/lib/libtomcrypt/src/mac/pmac/pmac_file.c
new file mode 100644
index 0000000..d018692
--- /dev/null
+++ b/core/lib/libtomcrypt/src/mac/pmac/pmac_file.c
@@ -0,0 +1,111 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/** 
+   @file pmac_file.c
+   PMAC implementation, process a file, by Tom St Denis 
+*/
+
+#ifdef LTC_PMAC
+
+/**
+   PMAC a file 
+   @param cipher       The index of the cipher desired
+   @param key          The secret key
+   @param keylen       The length of the secret key (octets)
+   @param filename     The name of the file to send through PMAC
+   @param out          [out] Destination for the authentication tag
+   @param outlen       [in/out] Max size and resulting size of the authentication tag
+   @return CRYPT_OK if successful, CRYPT_NOP if file support has been disabled
+*/
+int pmac_file(int cipher, 
+              const unsigned char *key, unsigned long keylen,
+              const char *filename, 
+                    unsigned char *out, unsigned long *outlen)
+{
+#ifdef LTC_NO_FILE
+   return CRYPT_NOP;
+#else
+   int err, x;
+   pmac_state pmac;
+   FILE *in;
+   unsigned char buf[512];
+
+
+   LTC_ARGCHK(key      != NULL);
+   LTC_ARGCHK(filename != NULL);
+   LTC_ARGCHK(out      != NULL);
+   LTC_ARGCHK(outlen   != NULL);
+
+   in = fopen(filename, "rb");
+   if (in == NULL) {
+      return CRYPT_FILE_NOTFOUND;
+   }
+
+   if ((err = pmac_init(&pmac, cipher, key, keylen)) != CRYPT_OK) {
+      fclose(in);
+      return err;
+   }
+
+   do {
+      x = fread(buf, 1, sizeof(buf), in);
+      if ((err = pmac_process(&pmac, buf, x)) != CRYPT_OK) {
+         fclose(in);
+         return err;
+      }
+   } while (x == sizeof(buf));
+   fclose(in);
+
+   if ((err = pmac_done(&pmac, out, outlen)) != CRYPT_OK) {
+      return err;
+   }
+
+#ifdef LTC_CLEAN_STACK
+   zeromem(buf, sizeof(buf));
+#endif
+
+   return CRYPT_OK;
+#endif
+}
+
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/mac/pmac/pmac_file.c,v $ */
+/* $Revision: 1.6 $ */
+/* $Date: 2006/12/28 01:27:23 $ */
diff --git a/core/lib/libtomcrypt/src/mac/pmac/pmac_init.c b/core/lib/libtomcrypt/src/mac/pmac/pmac_init.c
new file mode 100644
index 0000000..34a23d1
--- /dev/null
+++ b/core/lib/libtomcrypt/src/mac/pmac/pmac_init.c
@@ -0,0 +1,177 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/** 
+   @file pmac_init.c
+   PMAC implementation, initialize state, by Tom St Denis 
+*/
+
+#ifdef LTC_PMAC
+
+static const struct {
+    int           len;
+    unsigned char poly_div[MAXBLOCKSIZE], 
+                  poly_mul[MAXBLOCKSIZE];
+} polys[] = {
+{
+    8,
+    { 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0D },
+    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1B }
+}, {
+    16, 
+    { 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+      0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x43 },
+    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+      0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x87 }
+}
+};
+
+/**
+   Initialize a PMAC state
+   @param pmac      The PMAC state to initialize
+   @param cipher    The index of the desired cipher
+   @param key       The secret key 
+   @param keylen    The length of the secret key (octets)
+   @return CRYPT_OK if successful
+*/
+int pmac_init(pmac_state *pmac, int cipher, const unsigned char *key, unsigned long keylen)
+{
+   int poly, x, y, m, err;
+   unsigned char *L;
+
+   LTC_ARGCHK(pmac  != NULL);
+   LTC_ARGCHK(key   != NULL);
+
+   /* valid cipher? */
+   if ((err = cipher_is_valid(cipher)) != CRYPT_OK) {
+      return err;
+   }
+
+   /* determine which polys to use */
+   pmac->block_len = cipher_descriptor[cipher].block_length;
+   for (poly = 0; poly < (int)(sizeof(polys)/sizeof(polys[0])); poly++) {
+       if (polys[poly].len == pmac->block_len) { 
+          break;
+       }
+   }
+   if (poly >= (int)(sizeof(polys)/sizeof(polys[0]))) {
+      return CRYPT_INVALID_ARG;
+    }
+   if (polys[poly].len != pmac->block_len) {
+      return CRYPT_INVALID_ARG;
+   }
+
+#ifdef LTC_FAST
+   if (pmac->block_len % sizeof(LTC_FAST_TYPE)) {
+      return CRYPT_INVALID_ARG;
+   }
+#endif
+
+
+   /* schedule the key */
+   if ((err = cipher_descriptor[cipher].setup(key, keylen, 0, &pmac->key)) != CRYPT_OK) {
+      return err;
+   }
+ 
+   /* allocate L */
+   L = XMALLOC(pmac->block_len);
+   if (L == NULL) {
+      return CRYPT_MEM;
+   }
+
+   /* find L = E[0] */
+   zeromem(L, pmac->block_len);
+   if ((err = cipher_descriptor[cipher].ecb_encrypt(L, L, &pmac->key)) != CRYPT_OK) {
+      goto error;
+   }
+
+   /* find Ls[i] = L << i for i == 0..31 */
+   XMEMCPY(pmac->Ls[0], L, pmac->block_len);
+   for (x = 1; x < 32; x++) {
+       m = pmac->Ls[x-1][0] >> 7;
+       for (y = 0; y < pmac->block_len-1; y++) {
+           pmac->Ls[x][y] = ((pmac->Ls[x-1][y] << 1) | (pmac->Ls[x-1][y+1] >> 7)) & 255;
+       }
+       pmac->Ls[x][pmac->block_len-1] = (pmac->Ls[x-1][pmac->block_len-1] << 1) & 255;
+
+       if (m == 1) {
+          for (y = 0; y < pmac->block_len; y++) {
+              pmac->Ls[x][y] ^= polys[poly].poly_mul[y];
+          }
+       }
+    }
+
+    /* find Lr = L / x */
+    m = L[pmac->block_len-1] & 1;
+
+    /* shift right */
+    for (x = pmac->block_len - 1; x > 0; x--) {
+        pmac->Lr[x] = ((L[x] >> 1) | (L[x-1] << 7)) & 255;
+    }
+    pmac->Lr[0] = L[0] >> 1;
+
+    if (m == 1) {
+       for (x = 0; x < pmac->block_len; x++) {
+           pmac->Lr[x] ^= polys[poly].poly_div[x];
+       }
+    }
+
+    /* zero buffer, counters, etc... */
+    pmac->block_index = 1;
+    pmac->cipher_idx  = cipher;
+    pmac->buflen      = 0;
+    zeromem(pmac->block,    sizeof(pmac->block));
+    zeromem(pmac->Li,       sizeof(pmac->Li));
+    zeromem(pmac->checksum, sizeof(pmac->checksum));
+    err = CRYPT_OK;
+error:
+#ifdef LTC_CLEAN_STACK
+    zeromem(L, pmac->block_len);
+#endif
+
+    XFREE(L);
+
+    return err;
+}
+
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/mac/pmac/pmac_init.c,v $ */
+/* $Revision: 1.8 $ */
+/* $Date: 2006/12/28 01:27:23 $ */
diff --git a/core/lib/libtomcrypt/src/mac/pmac/pmac_memory.c b/core/lib/libtomcrypt/src/mac/pmac/pmac_memory.c
new file mode 100644
index 0000000..03073d8
--- /dev/null
+++ b/core/lib/libtomcrypt/src/mac/pmac/pmac_memory.c
@@ -0,0 +1,101 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/** 
+   @file pmac_memory.c
+   PMAC implementation, process a block of memory, by Tom St Denis 
+*/
+
+#ifdef LTC_PMAC
+
+/**
+   PMAC a block of memory
+   @param cipher   The index of the cipher desired
+   @param key      The secret key
+   @param keylen   The length of the secret key (octets)
+   @param in       The data you wish to send through PMAC
+   @param inlen    The length of data you wish to send through PMAC (octets)
+   @param out      [out] Destination for the authentication tag
+   @param outlen   [in/out] The max size and resulting size of the authentication tag
+   @return CRYPT_OK if successful
+*/
+int pmac_memory(int cipher, 
+                const unsigned char *key, unsigned long keylen,
+                const unsigned char *in, unsigned long inlen,
+                      unsigned char *out, unsigned long *outlen)
+{
+   int err;
+   pmac_state *pmac;
+
+   LTC_ARGCHK(key    != NULL);
+   LTC_ARGCHK(in    != NULL);
+   LTC_ARGCHK(out    != NULL);
+   LTC_ARGCHK(outlen != NULL);
+
+   /* allocate ram for pmac state */
+   pmac = XMALLOC(sizeof(pmac_state));
+   if (pmac == NULL) {
+      return CRYPT_MEM;
+   }
+   
+   if ((err = pmac_init(pmac, cipher, key, keylen)) != CRYPT_OK) {
+      goto LBL_ERR;
+   }
+   if ((err = pmac_process(pmac, in, inlen)) != CRYPT_OK) {
+      goto LBL_ERR;
+   }
+   if ((err = pmac_done(pmac, out, outlen)) != CRYPT_OK) {
+      goto LBL_ERR;
+   }
+
+   err = CRYPT_OK;
+LBL_ERR:
+#ifdef LTC_CLEAN_STACK
+   zeromem(pmac, sizeof(pmac_state));
+#endif
+
+   XFREE(pmac);
+   return err;   
+}
+
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/mac/pmac/pmac_memory.c,v $ */
+/* $Revision: 1.6 $ */
+/* $Date: 2006/12/28 01:27:23 $ */
diff --git a/core/lib/libtomcrypt/src/mac/pmac/pmac_memory_multi.c b/core/lib/libtomcrypt/src/mac/pmac/pmac_memory_multi.c
new file mode 100644
index 0000000..28085cc
--- /dev/null
+++ b/core/lib/libtomcrypt/src/mac/pmac/pmac_memory_multi.c
@@ -0,0 +1,116 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+#include <stdarg.h>
+
+/** 
+   @file pmac_memory_multi.c
+   PMAC implementation, process multiple blocks of memory, by Tom St Denis 
+*/
+
+#ifdef LTC_PMAC
+
+/**
+   PMAC multiple blocks of memory
+   @param cipher   The index of the cipher desired
+   @param key      The secret key
+   @param keylen   The length of the secret key (octets)
+   @param out      [out] Destination for the authentication tag
+   @param outlen   [in/out] The max size and resulting size of the authentication tag
+   @param in       The data you wish to send through PMAC
+   @param inlen    The length of data you wish to send through PMAC (octets)
+   @param ...      tuples of (data,len) pairs to PMAC, terminated with a (NULL,x) (x=don't care)
+   @return CRYPT_OK if successful
+*/
+int pmac_memory_multi(int cipher, 
+                const unsigned char *key, unsigned long  keylen,
+                      unsigned char *out, unsigned long *outlen,
+                const unsigned char *in,  unsigned long  inlen, ...)
+{
+   int                  err;
+   pmac_state          *pmac;
+   va_list              args;
+   const unsigned char *curptr;
+   unsigned long        curlen;
+
+   LTC_ARGCHK(key    != NULL);
+   LTC_ARGCHK(in     != NULL);
+   LTC_ARGCHK(out    != NULL);
+   LTC_ARGCHK(outlen != NULL);
+
+   /* allocate ram for pmac state */
+   pmac = XMALLOC(sizeof(pmac_state));
+   if (pmac == NULL) {
+      return CRYPT_MEM;
+   }
+   
+   if ((err = pmac_init(pmac, cipher, key, keylen)) != CRYPT_OK) {
+      goto LBL_ERR;
+   }
+   va_start(args, inlen);
+   curptr = in; 
+   curlen = inlen;
+   for (;;) {
+      /* process buf */
+      if ((err = pmac_process(pmac, curptr, curlen)) != CRYPT_OK) {
+         goto LBL_ERR;
+      }
+      /* step to next */
+      curptr = va_arg(args, const unsigned char*);
+      if (curptr == NULL) {
+         break;
+      }
+      curlen = va_arg(args, unsigned long);
+   }
+   if ((err = pmac_done(pmac, out, outlen)) != CRYPT_OK) {
+      goto LBL_ERR;
+   }
+LBL_ERR:
+#ifdef LTC_CLEAN_STACK
+   zeromem(pmac, sizeof(pmac_state));
+#endif
+   XFREE(pmac);
+   va_end(args);
+   return err;   
+}
+
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/mac/pmac/pmac_memory_multi.c,v $ */
+/* $Revision: 1.7 $ */
+/* $Date: 2006/12/28 01:27:23 $ */
diff --git a/core/lib/libtomcrypt/src/mac/pmac/pmac_ntz.c b/core/lib/libtomcrypt/src/mac/pmac/pmac_ntz.c
new file mode 100644
index 0000000..cc9464e
--- /dev/null
+++ b/core/lib/libtomcrypt/src/mac/pmac/pmac_ntz.c
@@ -0,0 +1,66 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/** 
+   @file pmac_ntz.c
+   PMAC implementation, internal function, by Tom St Denis 
+*/
+
+#ifdef LTC_PMAC
+
+/**
+  Internal PMAC function
+*/
+int pmac_ntz(unsigned long x)
+{
+   int c;
+   x &= 0xFFFFFFFFUL;
+   c = 0;
+   while ((x & 1) == 0) {
+      ++c;
+      x >>= 1;
+   }
+   return c;
+}
+
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/mac/pmac/pmac_ntz.c,v $ */
+/* $Revision: 1.6 $ */
+/* $Date: 2006/12/28 01:27:23 $ */
diff --git a/core/lib/libtomcrypt/src/mac/pmac/pmac_process.c b/core/lib/libtomcrypt/src/mac/pmac/pmac_process.c
new file mode 100644
index 0000000..beed309
--- /dev/null
+++ b/core/lib/libtomcrypt/src/mac/pmac/pmac_process.c
@@ -0,0 +1,127 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/** 
+   @file pmac_process.c
+   PMAC implementation, process data, by Tom St Denis 
+*/
+
+
+#ifdef LTC_PMAC
+
+/**
+  Process data in a PMAC stream
+  @param pmac     The PMAC state
+  @param in       The data to send through PMAC
+  @param inlen    The length of the data to send through PMAC
+  @return CRYPT_OK if successful
+*/
+int pmac_process(pmac_state *pmac, const unsigned char *in, unsigned long inlen)
+{
+   int err, n;
+   unsigned long x;
+   unsigned char Z[MAXBLOCKSIZE];
+
+   LTC_ARGCHK(pmac != NULL);
+   LTC_ARGCHK(in   != NULL);
+   if ((err = cipher_is_valid(pmac->cipher_idx)) != CRYPT_OK) {
+      return err;
+   }
+
+   if ((pmac->buflen > (int)sizeof(pmac->block)) || (pmac->buflen < 0) ||
+       (pmac->block_len > (int)sizeof(pmac->block)) || (pmac->buflen > pmac->block_len)) {
+      return CRYPT_INVALID_ARG;
+   }
+
+#ifdef LTC_FAST
+   if (pmac->buflen == 0 && inlen > 16) {
+      unsigned long y;
+      for (x = 0; x < (inlen - 16); x += 16) {
+          pmac_shift_xor(pmac);
+          for (y = 0; y < 16; y += sizeof(LTC_FAST_TYPE)) {
+              *((LTC_FAST_TYPE*)(&Z[y])) = *((LTC_FAST_TYPE*)(&in[y])) ^ *((LTC_FAST_TYPE*)(&pmac->Li[y]));
+          }
+          if ((err = cipher_descriptor[pmac->cipher_idx].ecb_encrypt(Z, Z, &pmac->key)) != CRYPT_OK) {
+             return err;
+          }
+          for (y = 0; y < 16; y += sizeof(LTC_FAST_TYPE)) {
+              *((LTC_FAST_TYPE*)(&pmac->checksum[y])) ^= *((LTC_FAST_TYPE*)(&Z[y]));
+          }
+          in += 16;
+      }
+      inlen -= x;
+   }
+#endif
+
+   while (inlen != 0) { 
+       /* ok if the block is full we xor in prev, encrypt and replace prev */
+       if (pmac->buflen == pmac->block_len) {
+          pmac_shift_xor(pmac);
+          for (x = 0; x < (unsigned long)pmac->block_len; x++) {
+               Z[x] = pmac->Li[x] ^ pmac->block[x];
+          }
+          if ((err = cipher_descriptor[pmac->cipher_idx].ecb_encrypt(Z, Z, &pmac->key)) != CRYPT_OK) {
+             return err;
+           }
+          for (x = 0; x < (unsigned long)pmac->block_len; x++) {
+              pmac->checksum[x] ^= Z[x];
+          }
+          pmac->buflen = 0;
+       }
+
+       /* add bytes */
+       n = MIN(inlen, (unsigned long)(pmac->block_len - pmac->buflen));
+       XMEMCPY(pmac->block + pmac->buflen, in, n);
+       pmac->buflen  += n;
+       inlen         -= n;
+       in            += n;
+   }
+
+#ifdef LTC_CLEAN_STACK
+   zeromem(Z, sizeof(Z));
+#endif
+
+   return CRYPT_OK;
+}
+
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/mac/pmac/pmac_process.c,v $ */
+/* $Revision: 1.9 $ */
+/* $Date: 2006/12/28 01:27:23 $ */
diff --git a/core/lib/libtomcrypt/src/mac/pmac/pmac_shift_xor.c b/core/lib/libtomcrypt/src/mac/pmac/pmac_shift_xor.c
new file mode 100644
index 0000000..53a8fcb
--- /dev/null
+++ b/core/lib/libtomcrypt/src/mac/pmac/pmac_shift_xor.c
@@ -0,0 +1,71 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/** 
+   @file pmac_shift_xor.c
+   PMAC implementation, internal function, by Tom St Denis 
+*/
+
+#ifdef LTC_PMAC
+
+/**
+  Internal function.  Performs the state update (adding correct multiple)
+  @param pmac   The PMAC state.
+*/
+void pmac_shift_xor(pmac_state *pmac)
+{
+   int x, y;
+   y = pmac_ntz(pmac->block_index++);
+#ifdef LTC_FAST
+   for (x = 0; x < pmac->block_len; x += sizeof(LTC_FAST_TYPE)) {
+       *((LTC_FAST_TYPE*)((unsigned char *)pmac->Li + x)) ^=
+       *((LTC_FAST_TYPE*)((unsigned char *)pmac->Ls[y] + x));
+   }
+#else
+   for (x = 0; x < pmac->block_len; x++) {
+       pmac->Li[x] ^= pmac->Ls[y][x];
+   }
+#endif
+}
+
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/mac/pmac/pmac_shift_xor.c,v $ */
+/* $Revision: 1.7 $ */
+/* $Date: 2006/12/28 01:27:23 $ */
diff --git a/core/lib/libtomcrypt/src/mac/sub.mk b/core/lib/libtomcrypt/src/mac/sub.mk
new file mode 100644
index 0000000..c4c60ff
--- /dev/null
+++ b/core/lib/libtomcrypt/src/mac/sub.mk
@@ -0,0 +1,2 @@
+subdirs-$(CFG_CRYPTO_HMAC) += hmac
+subdirs-$(CFG_CRYPTO_CMAC) += omac
diff --git a/core/lib/libtomcrypt/src/mac/xcbc/xcbc_done.c b/core/lib/libtomcrypt/src/mac/xcbc/xcbc_done.c
new file mode 100644
index 0000000..34aa199
--- /dev/null
+++ b/core/lib/libtomcrypt/src/mac/xcbc/xcbc_done.c
@@ -0,0 +1,104 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file xcbc_done.c
+  XCBC Support, terminate the state
+*/
+
+#ifdef LTC_XCBC
+
+/** Terminate the XCBC-MAC state
+  @param xcbc     XCBC state to terminate
+  @param out      [out] Destination for the MAC tag
+  @param outlen   [in/out] Destination size and final tag size
+  Return CRYPT_OK on success
+*/
+int xcbc_done(xcbc_state *xcbc, unsigned char *out, unsigned long *outlen)
+{
+   int err, x;
+   LTC_ARGCHK(xcbc != NULL);
+   LTC_ARGCHK(out  != NULL);
+
+   /* check structure */
+   if ((err = cipher_is_valid(xcbc->cipher)) != CRYPT_OK) {
+      return err;
+   }
+
+   if ((xcbc->blocksize > cipher_descriptor[xcbc->cipher].block_length) || (xcbc->blocksize < 0) ||
+       (xcbc->buflen > xcbc->blocksize) || (xcbc->buflen < 0)) {
+      return CRYPT_INVALID_ARG;
+   }
+
+   /* which key do we use? */
+   if (xcbc->buflen == xcbc->blocksize) {
+      /* k2 */
+      for (x = 0; x < xcbc->blocksize; x++) {
+         xcbc->IV[x] ^= xcbc->K[1][x];
+      }
+   } else {
+      xcbc->IV[xcbc->buflen] ^= 0x80;
+      /* k3 */
+      for (x = 0; x < xcbc->blocksize; x++) {
+         xcbc->IV[x] ^= xcbc->K[2][x];
+      }
+   }
+
+   /* encrypt */
+   cipher_descriptor[xcbc->cipher].ecb_encrypt(xcbc->IV, xcbc->IV, &xcbc->key);
+   cipher_descriptor[xcbc->cipher].done(&xcbc->key);
+
+   /* extract tag */
+   for (x = 0; x < xcbc->blocksize && (unsigned long)x < *outlen; x++) {
+      out[x] = xcbc->IV[x];
+   }
+   *outlen = x;
+  
+#ifdef LTC_CLEAN_STACK
+   zeromem(xcbc, sizeof(*xcbc));
+#endif
+   return CRYPT_OK;
+}
+
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/mac/xcbc/xcbc_done.c,v $ */
+/* $Revision: 1.5 $ */
+/* $Date: 2006/12/28 01:27:23 $ */
+
diff --git a/core/lib/libtomcrypt/src/mac/xcbc/xcbc_file.c b/core/lib/libtomcrypt/src/mac/xcbc/xcbc_file.c
new file mode 100644
index 0000000..fc790ef
--- /dev/null
+++ b/core/lib/libtomcrypt/src/mac/xcbc/xcbc_file.c
@@ -0,0 +1,110 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/** 
+  @file xcbc_file.c
+  XCBC support, process a file, Tom St Denis
+*/
+
+#ifdef LTC_XCBC
+
+/**
+   XCBC a file
+   @param cipher   The index of the cipher desired
+   @param key      The secret key
+   @param keylen   The length of the secret key (octets)
+   @param filename The name of the file you wish to XCBC
+   @param out      [out] Where the authentication tag is to be stored
+   @param outlen   [in/out] The max size and resulting size of the authentication tag
+   @return CRYPT_OK if successful, CRYPT_NOP if file support has been disabled
+*/
+int xcbc_file(int cipher, 
+              const unsigned char *key, unsigned long keylen,
+              const char *filename, 
+                    unsigned char *out, unsigned long *outlen)
+{
+#ifdef LTC_NO_FILE
+   return CRYPT_NOP;
+#else
+   int err, x;
+   xcbc_state xcbc;
+   FILE *in;
+   unsigned char buf[512];
+
+   LTC_ARGCHK(key      != NULL);
+   LTC_ARGCHK(filename != NULL);
+   LTC_ARGCHK(out      != NULL);
+   LTC_ARGCHK(outlen   != NULL);
+
+   in = fopen(filename, "rb");
+   if (in == NULL) {
+      return CRYPT_FILE_NOTFOUND;
+   }
+
+   if ((err = xcbc_init(&xcbc, cipher, key, keylen)) != CRYPT_OK) {
+      fclose(in);
+      return err;
+   }
+
+   do {
+      x = fread(buf, 1, sizeof(buf), in);
+      if ((err = xcbc_process(&xcbc, buf, x)) != CRYPT_OK) {
+         fclose(in);
+         return err;
+      }
+   } while (x == sizeof(buf));
+   fclose(in);
+
+   if ((err = xcbc_done(&xcbc, out, outlen)) != CRYPT_OK) {
+      return err;
+   }
+
+#ifdef LTC_CLEAN_STACK
+   zeromem(buf, sizeof(buf));
+#endif
+
+   return CRYPT_OK;
+#endif
+}
+
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/mac/xcbc/xcbc_file.c,v $ */
+/* $Revision: 1.2 $ */
+/* $Date: 2006/12/28 01:27:23 $ */
diff --git a/core/lib/libtomcrypt/src/mac/xcbc/xcbc_init.c b/core/lib/libtomcrypt/src/mac/xcbc/xcbc_init.c
new file mode 100644
index 0000000..14c6313
--- /dev/null
+++ b/core/lib/libtomcrypt/src/mac/xcbc/xcbc_init.c
@@ -0,0 +1,135 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file xcbc_init.c
+  XCBC Support, start an XCBC state
+*/
+
+#ifdef LTC_XCBC
+
+/** Initialize XCBC-MAC state
+  @param xcbc    [out] XCBC state to initialize
+  @param cipher  Index of cipher to use
+  @param key     [in]  Secret key
+  @param keylen  Length of secret key in octets
+  Return CRYPT_OK on success
+*/
+int xcbc_init(xcbc_state *xcbc, int cipher, const unsigned char *key, unsigned long keylen)
+{
+   int            x, y, err;
+   symmetric_key *skey;
+   unsigned long  k1;
+
+   LTC_ARGCHK(xcbc != NULL);
+   LTC_ARGCHK(key  != NULL);
+
+   /* schedule the key */
+   if ((err = cipher_is_valid(cipher)) != CRYPT_OK) {
+      return err;
+   }
+
+#ifdef LTC_FAST
+   if (cipher_descriptor[cipher].block_length % sizeof(LTC_FAST_TYPE)) {
+       return CRYPT_INVALID_ARG;
+   }
+#endif
+
+   skey = NULL;
+
+   /* are we in pure XCBC mode with three keys? */
+   if (keylen & LTC_XCBC_PURE) {
+      keylen &= ~LTC_XCBC_PURE;
+
+      if (keylen < 2UL*cipher_descriptor[cipher].block_length) {
+         return CRYPT_INVALID_ARG;
+      }
+
+      k1      = keylen - 2*cipher_descriptor[cipher].block_length;
+      XMEMCPY(xcbc->K[0], key, k1);
+      XMEMCPY(xcbc->K[1], key+k1, cipher_descriptor[cipher].block_length);
+      XMEMCPY(xcbc->K[2], key+k1 + cipher_descriptor[cipher].block_length, cipher_descriptor[cipher].block_length);
+   } else {
+      /* use the key expansion */
+      k1      = cipher_descriptor[cipher].block_length;
+
+      /* schedule the user key */
+      skey = XCALLOC(1, sizeof(*skey));
+      if (skey == NULL) {
+         return CRYPT_MEM;
+      }
+
+      if ((err = cipher_descriptor[cipher].setup(key, keylen, 0, skey)) != CRYPT_OK) {
+         goto done;
+      }
+   
+      /* make the three keys */
+      for (y = 0; y < 3; y++) {
+        for (x = 0; x < cipher_descriptor[cipher].block_length; x++) {
+           xcbc->K[y][x] = y + 1;
+        }
+        cipher_descriptor[cipher].ecb_encrypt(xcbc->K[y], xcbc->K[y], skey);
+      }
+   }
+     
+   /* setup K1 */
+   err = cipher_descriptor[cipher].setup(xcbc->K[0], k1, 0, &xcbc->key);
+ 
+   /* setup struct */
+   zeromem(xcbc->IV, cipher_descriptor[cipher].block_length);
+   xcbc->blocksize = cipher_descriptor[cipher].block_length;
+   xcbc->cipher    = cipher;
+   xcbc->buflen    = 0;
+done:
+   cipher_descriptor[cipher].done(skey);
+   if (skey != NULL) { 
+#ifdef LTC_CLEAN_STACK
+      zeromem(skey, sizeof(*skey));
+#endif
+      XFREE(skey);
+   }
+   return err;
+}
+
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/mac/xcbc/xcbc_init.c,v $ */
+/* $Revision: 1.7 $ */
+/* $Date: 2007/02/20 13:07:58 $ */
+
diff --git a/core/lib/libtomcrypt/src/mac/xcbc/xcbc_memory.c b/core/lib/libtomcrypt/src/mac/xcbc/xcbc_memory.c
new file mode 100644
index 0000000..81783d2
--- /dev/null
+++ b/core/lib/libtomcrypt/src/mac/xcbc/xcbc_memory.c
@@ -0,0 +1,98 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file xcbc_process.c
+  XCBC Support, XCBC-MAC a block of memory
+*/
+
+#ifdef LTC_XCBC
+
+/** XCBC-MAC a block of memory 
+  @param cipher     Index of cipher to use
+  @param key        [in]  Secret key
+  @param keylen     Length of key in octets
+  @param in         [in]  Message to MAC
+  @param inlen      Length of input in octets
+  @param out        [out] Destination for the MAC tag
+  @param outlen     [in/out] Output size and final tag size
+  Return CRYPT_OK on success.
+*/
+int xcbc_memory(int cipher, 
+               const unsigned char *key, unsigned long keylen,
+               const unsigned char *in,  unsigned long inlen,
+                     unsigned char *out, unsigned long *outlen)
+{
+   xcbc_state *xcbc;
+   int         err;
+
+   /* is the cipher valid? */
+   if ((err = cipher_is_valid(cipher)) != CRYPT_OK) {
+      return err;
+   }
+
+   /* Use accelerator if found */
+   if (cipher_descriptor[cipher].xcbc_memory != NULL) {
+      return cipher_descriptor[cipher].xcbc_memory(key, keylen, in, inlen, out, outlen);
+   }
+
+   xcbc = XCALLOC(1, sizeof(*xcbc));
+   if (xcbc == NULL) {
+      return CRYPT_MEM;
+   }
+
+   if ((err = xcbc_init(xcbc, cipher, key, keylen)) != CRYPT_OK) {
+     goto done;
+   }
+
+   if ((err = xcbc_process(xcbc, in, inlen)) != CRYPT_OK) {
+     goto done;
+   }
+
+   err = xcbc_done(xcbc, out, outlen);
+done:
+   XFREE(xcbc);
+   return err;
+}
+
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/mac/xcbc/xcbc_memory.c,v $ */
+/* $Revision: 1.5 $ */
+/* $Date: 2006/12/28 01:27:23 $ */
diff --git a/core/lib/libtomcrypt/src/mac/xcbc/xcbc_memory_multi.c b/core/lib/libtomcrypt/src/mac/xcbc/xcbc_memory_multi.c
new file mode 100644
index 0000000..8c5ded4
--- /dev/null
+++ b/core/lib/libtomcrypt/src/mac/xcbc/xcbc_memory_multi.c
@@ -0,0 +1,117 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+#include <stdarg.h>
+
+/** 
+  @file xcbc_memory_multi.c
+  XCBC support, process multiple blocks of memory, Tom St Denis
+*/
+
+#ifdef LTC_XCBC
+
+/**
+   XCBC multiple blocks of memory 
+   @param cipher    The index of the desired cipher
+   @param key       The secret key
+   @param keylen    The length of the secret key (octets)
+   @param out       [out] The destination of the authentication tag
+   @param outlen    [in/out]  The max size and resulting size of the authentication tag (octets)
+   @param in        The data to send through XCBC
+   @param inlen     The length of the data to send through XCBC (octets)
+   @param ...       tuples of (data,len) pairs to XCBC, terminated with a (NULL,x) (x=don't care)
+   @return CRYPT_OK if successful
+*/
+int xcbc_memory_multi(int cipher, 
+                const unsigned char *key, unsigned long keylen,
+                      unsigned char *out, unsigned long *outlen,
+                const unsigned char *in,  unsigned long inlen, ...)
+{
+   int                  err;
+   xcbc_state          *xcbc;
+   va_list              args;
+   const unsigned char *curptr;
+   unsigned long        curlen;
+
+   LTC_ARGCHK(key    != NULL);
+   LTC_ARGCHK(in     != NULL);
+   LTC_ARGCHK(out    != NULL);
+   LTC_ARGCHK(outlen != NULL);
+
+   /* allocate ram for xcbc state */
+   xcbc = XMALLOC(sizeof(xcbc_state));
+   if (xcbc == NULL) {
+      return CRYPT_MEM;
+   }
+
+   /* xcbc process the message */
+   if ((err = xcbc_init(xcbc, cipher, key, keylen)) != CRYPT_OK) {
+      goto LBL_ERR;
+   }
+   va_start(args, inlen);
+   curptr = in; 
+   curlen = inlen;
+   for (;;) {
+      /* process buf */
+      if ((err = xcbc_process(xcbc, curptr, curlen)) != CRYPT_OK) {
+         goto LBL_ERR;
+      }
+      /* step to next */
+      curptr = va_arg(args, const unsigned char*);
+      if (curptr == NULL) {
+         break;
+      }
+      curlen = va_arg(args, unsigned long);
+   }
+   if ((err = xcbc_done(xcbc, out, outlen)) != CRYPT_OK) {
+      goto LBL_ERR;
+   }
+LBL_ERR:
+#ifdef LTC_CLEAN_STACK
+   zeromem(xcbc, sizeof(xcbc_state));
+#endif
+   XFREE(xcbc);
+   va_end(args);
+   return err;   
+}
+
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/mac/xcbc/xcbc_memory_multi.c,v $ */
+/* $Revision: 1.2 $ */
+/* $Date: 2006/12/28 01:27:23 $ */
diff --git a/core/lib/libtomcrypt/src/mac/xcbc/xcbc_process.c b/core/lib/libtomcrypt/src/mac/xcbc/xcbc_process.c
new file mode 100644
index 0000000..63f6d5f
--- /dev/null
+++ b/core/lib/libtomcrypt/src/mac/xcbc/xcbc_process.c
@@ -0,0 +1,102 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file xcbc_process.c
+  XCBC Support, process blocks with XCBC
+*/
+
+#ifdef LTC_XCBC
+
+/** Process data through XCBC-MAC
+  @param xcbc     The XCBC-MAC state
+  @param in       Input data to process
+  @param inlen    Length of input in octets
+  Return CRYPT_OK on success
+*/
+int xcbc_process(xcbc_state *xcbc, const unsigned char *in, unsigned long inlen)
+{
+   int err;
+#ifdef LTC_FAST
+   int x;
+#endif
+
+   LTC_ARGCHK(xcbc != NULL);
+   LTC_ARGCHK(in   != NULL);
+
+   /* check structure */
+   if ((err = cipher_is_valid(xcbc->cipher)) != CRYPT_OK) {
+      return err;
+   }
+
+   if ((xcbc->blocksize > cipher_descriptor[xcbc->cipher].block_length) || (xcbc->blocksize < 0) ||
+       (xcbc->buflen > xcbc->blocksize) || (xcbc->buflen < 0)) {
+      return CRYPT_INVALID_ARG;
+   }
+
+#ifdef LTC_FAST
+   if (xcbc->buflen == 0) {
+       while (inlen > (unsigned long)xcbc->blocksize) {
+           for (x = 0; x < xcbc->blocksize; x += sizeof(LTC_FAST_TYPE)) {
+              *((LTC_FAST_TYPE*)&(xcbc->IV[x])) ^= *((LTC_FAST_TYPE*)&(in[x]));
+           }
+           cipher_descriptor[xcbc->cipher].ecb_encrypt(xcbc->IV, xcbc->IV, &xcbc->key);
+           in    += xcbc->blocksize;
+           inlen -= xcbc->blocksize;
+       }
+  }
+#endif
+
+   while (inlen) {
+     if (xcbc->buflen == xcbc->blocksize) {
+         cipher_descriptor[xcbc->cipher].ecb_encrypt(xcbc->IV, xcbc->IV, &xcbc->key);
+         xcbc->buflen = 0;
+     }
+     xcbc->IV[xcbc->buflen++] ^= *in++;
+     --inlen;
+  }
+  return CRYPT_OK;       
+}
+
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/mac/xcbc/xcbc_process.c,v $ */
+/* $Revision: 1.10 $ */
+/* $Date: 2006/12/28 01:27:23 $ */
+
diff --git a/core/lib/libtomcrypt/src/math/fp/ltc_ecc_fp_mulmod.c b/core/lib/libtomcrypt/src/math/fp/ltc_ecc_fp_mulmod.c
new file mode 100644
index 0000000..6dbcc83
--- /dev/null
+++ b/core/lib/libtomcrypt/src/math/fp/ltc_ecc_fp_mulmod.c
@@ -0,0 +1,1614 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file ltc_ecc_fp_mulmod.c
+  ECC Crypto, Tom St Denis
+*/  
+
+#if defined(LTC_MECC) && defined(LTC_MECC_FP)
+#include <limits.h>
+
+/* number of entries in the cache */
+#ifndef FP_ENTRIES
+#define FP_ENTRIES 16
+#endif
+
+/* number of bits in LUT */
+#ifndef FP_LUT
+#define FP_LUT     8U
+#endif
+
+#if (FP_LUT > 12) || (FP_LUT < 2)
+   #error FP_LUT must be between 2 and 12 inclusively
+#endif   
+
+/** Our FP cache */
+static struct {
+   ecc_point *g,              /* cached COPY of base point */
+             *LUT[1U<<FP_LUT]; /* fixed point lookup */ 
+   void      *mu;             /* copy of the montgomery constant */
+   int        lru_count;      /* amount of times this entry has been used */
+   int        lock;           /* flag to indicate cache eviction permitted (0) or not (1) */
+} fp_cache[FP_ENTRIES];
+
+LTC_MUTEX_GLOBAL(ltc_ecc_fp_lock)
+
+/* simple table to help direct the generation of the LUT */
+static const struct {
+   int ham, terma, termb;
+} lut_orders[] = {
+   { 0, 0, 0 }, { 1, 0, 0 }, { 1, 0, 0 }, { 2, 1, 2 }, { 1, 0, 0 }, { 2, 1, 4 }, { 2, 2, 4 }, { 3, 3, 4 }, 
+   { 1, 0, 0 }, { 2, 1, 8 }, { 2, 2, 8 }, { 3, 3, 8 }, { 2, 4, 8 }, { 3, 5, 8 }, { 3, 6, 8 }, { 4, 7, 8 }, 
+   { 1, 0, 0 }, { 2, 1, 16 }, { 2, 2, 16 }, { 3, 3, 16 }, { 2, 4, 16 }, { 3, 5, 16 }, { 3, 6, 16 }, { 4, 7, 16 }, 
+   { 2, 8, 16 }, { 3, 9, 16 }, { 3, 10, 16 }, { 4, 11, 16 }, { 3, 12, 16 }, { 4, 13, 16 }, { 4, 14, 16 }, { 5, 15, 16 }, 
+   { 1, 0, 0 }, { 2, 1, 32 }, { 2, 2, 32 }, { 3, 3, 32 }, { 2, 4, 32 }, { 3, 5, 32 }, { 3, 6, 32 }, { 4, 7, 32 }, 
+   { 2, 8, 32 }, { 3, 9, 32 }, { 3, 10, 32 }, { 4, 11, 32 }, { 3, 12, 32 }, { 4, 13, 32 }, { 4, 14, 32 }, { 5, 15, 32 }, 
+   { 2, 16, 32 }, { 3, 17, 32 }, { 3, 18, 32 }, { 4, 19, 32 }, { 3, 20, 32 }, { 4, 21, 32 }, { 4, 22, 32 }, { 5, 23, 32 }, 
+   { 3, 24, 32 }, { 4, 25, 32 }, { 4, 26, 32 }, { 5, 27, 32 }, { 4, 28, 32 }, { 5, 29, 32 }, { 5, 30, 32 }, { 6, 31, 32 }, 
+#if FP_LUT > 6
+   { 1, 0, 0 }, { 2, 1, 64 }, { 2, 2, 64 }, { 3, 3, 64 }, { 2, 4, 64 }, { 3, 5, 64 }, { 3, 6, 64 }, { 4, 7, 64 }, 
+   { 2, 8, 64 }, { 3, 9, 64 }, { 3, 10, 64 }, { 4, 11, 64 }, { 3, 12, 64 }, { 4, 13, 64 }, { 4, 14, 64 }, { 5, 15, 64 }, 
+   { 2, 16, 64 }, { 3, 17, 64 }, { 3, 18, 64 }, { 4, 19, 64 }, { 3, 20, 64 }, { 4, 21, 64 }, { 4, 22, 64 }, { 5, 23, 64 }, 
+   { 3, 24, 64 }, { 4, 25, 64 }, { 4, 26, 64 }, { 5, 27, 64 }, { 4, 28, 64 }, { 5, 29, 64 }, { 5, 30, 64 }, { 6, 31, 64 }, 
+   { 2, 32, 64 }, { 3, 33, 64 }, { 3, 34, 64 }, { 4, 35, 64 }, { 3, 36, 64 }, { 4, 37, 64 }, { 4, 38, 64 }, { 5, 39, 64 }, 
+   { 3, 40, 64 }, { 4, 41, 64 }, { 4, 42, 64 }, { 5, 43, 64 }, { 4, 44, 64 }, { 5, 45, 64 }, { 5, 46, 64 }, { 6, 47, 64 }, 
+   { 3, 48, 64 }, { 4, 49, 64 }, { 4, 50, 64 }, { 5, 51, 64 }, { 4, 52, 64 }, { 5, 53, 64 }, { 5, 54, 64 }, { 6, 55, 64 }, 
+   { 4, 56, 64 }, { 5, 57, 64 }, { 5, 58, 64 }, { 6, 59, 64 }, { 5, 60, 64 }, { 6, 61, 64 }, { 6, 62, 64 }, { 7, 63, 64 }, 
+#if FP_LUT > 7
+   { 1, 0, 0 }, { 2, 1, 128 }, { 2, 2, 128 }, { 3, 3, 128 }, { 2, 4, 128 }, { 3, 5, 128 }, { 3, 6, 128 }, { 4, 7, 128 }, 
+   { 2, 8, 128 }, { 3, 9, 128 }, { 3, 10, 128 }, { 4, 11, 128 }, { 3, 12, 128 }, { 4, 13, 128 }, { 4, 14, 128 }, { 5, 15, 128 }, 
+   { 2, 16, 128 }, { 3, 17, 128 }, { 3, 18, 128 }, { 4, 19, 128 }, { 3, 20, 128 }, { 4, 21, 128 }, { 4, 22, 128 }, { 5, 23, 128 }, 
+   { 3, 24, 128 }, { 4, 25, 128 }, { 4, 26, 128 }, { 5, 27, 128 }, { 4, 28, 128 }, { 5, 29, 128 }, { 5, 30, 128 }, { 6, 31, 128 }, 
+   { 2, 32, 128 }, { 3, 33, 128 }, { 3, 34, 128 }, { 4, 35, 128 }, { 3, 36, 128 }, { 4, 37, 128 }, { 4, 38, 128 }, { 5, 39, 128 }, 
+   { 3, 40, 128 }, { 4, 41, 128 }, { 4, 42, 128 }, { 5, 43, 128 }, { 4, 44, 128 }, { 5, 45, 128 }, { 5, 46, 128 }, { 6, 47, 128 }, 
+   { 3, 48, 128 }, { 4, 49, 128 }, { 4, 50, 128 }, { 5, 51, 128 }, { 4, 52, 128 }, { 5, 53, 128 }, { 5, 54, 128 }, { 6, 55, 128 }, 
+   { 4, 56, 128 }, { 5, 57, 128 }, { 5, 58, 128 }, { 6, 59, 128 }, { 5, 60, 128 }, { 6, 61, 128 }, { 6, 62, 128 }, { 7, 63, 128 }, 
+   { 2, 64, 128 }, { 3, 65, 128 }, { 3, 66, 128 }, { 4, 67, 128 }, { 3, 68, 128 }, { 4, 69, 128 }, { 4, 70, 128 }, { 5, 71, 128 }, 
+   { 3, 72, 128 }, { 4, 73, 128 }, { 4, 74, 128 }, { 5, 75, 128 }, { 4, 76, 128 }, { 5, 77, 128 }, { 5, 78, 128 }, { 6, 79, 128 }, 
+   { 3, 80, 128 }, { 4, 81, 128 }, { 4, 82, 128 }, { 5, 83, 128 }, { 4, 84, 128 }, { 5, 85, 128 }, { 5, 86, 128 }, { 6, 87, 128 }, 
+   { 4, 88, 128 }, { 5, 89, 128 }, { 5, 90, 128 }, { 6, 91, 128 }, { 5, 92, 128 }, { 6, 93, 128 }, { 6, 94, 128 }, { 7, 95, 128 }, 
+   { 3, 96, 128 }, { 4, 97, 128 }, { 4, 98, 128 }, { 5, 99, 128 }, { 4, 100, 128 }, { 5, 101, 128 }, { 5, 102, 128 }, { 6, 103, 128 }, 
+   { 4, 104, 128 }, { 5, 105, 128 }, { 5, 106, 128 }, { 6, 107, 128 }, { 5, 108, 128 }, { 6, 109, 128 }, { 6, 110, 128 }, { 7, 111, 128 }, 
+   { 4, 112, 128 }, { 5, 113, 128 }, { 5, 114, 128 }, { 6, 115, 128 }, { 5, 116, 128 }, { 6, 117, 128 }, { 6, 118, 128 }, { 7, 119, 128 }, 
+   { 5, 120, 128 }, { 6, 121, 128 }, { 6, 122, 128 }, { 7, 123, 128 }, { 6, 124, 128 }, { 7, 125, 128 }, { 7, 126, 128 }, { 8, 127, 128 }, 
+#if FP_LUT > 8
+   { 1, 0, 0 }, { 2, 1, 256 }, { 2, 2, 256 }, { 3, 3, 256 }, { 2, 4, 256 }, { 3, 5, 256 }, { 3, 6, 256 }, { 4, 7, 256 }, 
+   { 2, 8, 256 }, { 3, 9, 256 }, { 3, 10, 256 }, { 4, 11, 256 }, { 3, 12, 256 }, { 4, 13, 256 }, { 4, 14, 256 }, { 5, 15, 256 }, 
+   { 2, 16, 256 }, { 3, 17, 256 }, { 3, 18, 256 }, { 4, 19, 256 }, { 3, 20, 256 }, { 4, 21, 256 }, { 4, 22, 256 }, { 5, 23, 256 }, 
+   { 3, 24, 256 }, { 4, 25, 256 }, { 4, 26, 256 }, { 5, 27, 256 }, { 4, 28, 256 }, { 5, 29, 256 }, { 5, 30, 256 }, { 6, 31, 256 }, 
+   { 2, 32, 256 }, { 3, 33, 256 }, { 3, 34, 256 }, { 4, 35, 256 }, { 3, 36, 256 }, { 4, 37, 256 }, { 4, 38, 256 }, { 5, 39, 256 }, 
+   { 3, 40, 256 }, { 4, 41, 256 }, { 4, 42, 256 }, { 5, 43, 256 }, { 4, 44, 256 }, { 5, 45, 256 }, { 5, 46, 256 }, { 6, 47, 256 }, 
+   { 3, 48, 256 }, { 4, 49, 256 }, { 4, 50, 256 }, { 5, 51, 256 }, { 4, 52, 256 }, { 5, 53, 256 }, { 5, 54, 256 }, { 6, 55, 256 }, 
+   { 4, 56, 256 }, { 5, 57, 256 }, { 5, 58, 256 }, { 6, 59, 256 }, { 5, 60, 256 }, { 6, 61, 256 }, { 6, 62, 256 }, { 7, 63, 256 }, 
+   { 2, 64, 256 }, { 3, 65, 256 }, { 3, 66, 256 }, { 4, 67, 256 }, { 3, 68, 256 }, { 4, 69, 256 }, { 4, 70, 256 }, { 5, 71, 256 }, 
+   { 3, 72, 256 }, { 4, 73, 256 }, { 4, 74, 256 }, { 5, 75, 256 }, { 4, 76, 256 }, { 5, 77, 256 }, { 5, 78, 256 }, { 6, 79, 256 }, 
+   { 3, 80, 256 }, { 4, 81, 256 }, { 4, 82, 256 }, { 5, 83, 256 }, { 4, 84, 256 }, { 5, 85, 256 }, { 5, 86, 256 }, { 6, 87, 256 }, 
+   { 4, 88, 256 }, { 5, 89, 256 }, { 5, 90, 256 }, { 6, 91, 256 }, { 5, 92, 256 }, { 6, 93, 256 }, { 6, 94, 256 }, { 7, 95, 256 }, 
+   { 3, 96, 256 }, { 4, 97, 256 }, { 4, 98, 256 }, { 5, 99, 256 }, { 4, 100, 256 }, { 5, 101, 256 }, { 5, 102, 256 }, { 6, 103, 256 }, 
+   { 4, 104, 256 }, { 5, 105, 256 }, { 5, 106, 256 }, { 6, 107, 256 }, { 5, 108, 256 }, { 6, 109, 256 }, { 6, 110, 256 }, { 7, 111, 256 }, 
+   { 4, 112, 256 }, { 5, 113, 256 }, { 5, 114, 256 }, { 6, 115, 256 }, { 5, 116, 256 }, { 6, 117, 256 }, { 6, 118, 256 }, { 7, 119, 256 }, 
+   { 5, 120, 256 }, { 6, 121, 256 }, { 6, 122, 256 }, { 7, 123, 256 }, { 6, 124, 256 }, { 7, 125, 256 }, { 7, 126, 256 }, { 8, 127, 256 }, 
+   { 2, 128, 256 }, { 3, 129, 256 }, { 3, 130, 256 }, { 4, 131, 256 }, { 3, 132, 256 }, { 4, 133, 256 }, { 4, 134, 256 }, { 5, 135, 256 }, 
+   { 3, 136, 256 }, { 4, 137, 256 }, { 4, 138, 256 }, { 5, 139, 256 }, { 4, 140, 256 }, { 5, 141, 256 }, { 5, 142, 256 }, { 6, 143, 256 }, 
+   { 3, 144, 256 }, { 4, 145, 256 }, { 4, 146, 256 }, { 5, 147, 256 }, { 4, 148, 256 }, { 5, 149, 256 }, { 5, 150, 256 }, { 6, 151, 256 }, 
+   { 4, 152, 256 }, { 5, 153, 256 }, { 5, 154, 256 }, { 6, 155, 256 }, { 5, 156, 256 }, { 6, 157, 256 }, { 6, 158, 256 }, { 7, 159, 256 }, 
+   { 3, 160, 256 }, { 4, 161, 256 }, { 4, 162, 256 }, { 5, 163, 256 }, { 4, 164, 256 }, { 5, 165, 256 }, { 5, 166, 256 }, { 6, 167, 256 }, 
+   { 4, 168, 256 }, { 5, 169, 256 }, { 5, 170, 256 }, { 6, 171, 256 }, { 5, 172, 256 }, { 6, 173, 256 }, { 6, 174, 256 }, { 7, 175, 256 }, 
+   { 4, 176, 256 }, { 5, 177, 256 }, { 5, 178, 256 }, { 6, 179, 256 }, { 5, 180, 256 }, { 6, 181, 256 }, { 6, 182, 256 }, { 7, 183, 256 }, 
+   { 5, 184, 256 }, { 6, 185, 256 }, { 6, 186, 256 }, { 7, 187, 256 }, { 6, 188, 256 }, { 7, 189, 256 }, { 7, 190, 256 }, { 8, 191, 256 }, 
+   { 3, 192, 256 }, { 4, 193, 256 }, { 4, 194, 256 }, { 5, 195, 256 }, { 4, 196, 256 }, { 5, 197, 256 }, { 5, 198, 256 }, { 6, 199, 256 }, 
+   { 4, 200, 256 }, { 5, 201, 256 }, { 5, 202, 256 }, { 6, 203, 256 }, { 5, 204, 256 }, { 6, 205, 256 }, { 6, 206, 256 }, { 7, 207, 256 }, 
+   { 4, 208, 256 }, { 5, 209, 256 }, { 5, 210, 256 }, { 6, 211, 256 }, { 5, 212, 256 }, { 6, 213, 256 }, { 6, 214, 256 }, { 7, 215, 256 }, 
+   { 5, 216, 256 }, { 6, 217, 256 }, { 6, 218, 256 }, { 7, 219, 256 }, { 6, 220, 256 }, { 7, 221, 256 }, { 7, 222, 256 }, { 8, 223, 256 }, 
+   { 4, 224, 256 }, { 5, 225, 256 }, { 5, 226, 256 }, { 6, 227, 256 }, { 5, 228, 256 }, { 6, 229, 256 }, { 6, 230, 256 }, { 7, 231, 256 }, 
+   { 5, 232, 256 }, { 6, 233, 256 }, { 6, 234, 256 }, { 7, 235, 256 }, { 6, 236, 256 }, { 7, 237, 256 }, { 7, 238, 256 }, { 8, 239, 256 }, 
+   { 5, 240, 256 }, { 6, 241, 256 }, { 6, 242, 256 }, { 7, 243, 256 }, { 6, 244, 256 }, { 7, 245, 256 }, { 7, 246, 256 }, { 8, 247, 256 }, 
+   { 6, 248, 256 }, { 7, 249, 256 }, { 7, 250, 256 }, { 8, 251, 256 }, { 7, 252, 256 }, { 8, 253, 256 }, { 8, 254, 256 }, { 9, 255, 256 }, 
+#if FP_LUT > 9
+   { 1, 0, 0 }, { 2, 1, 512 }, { 2, 2, 512 }, { 3, 3, 512 }, { 2, 4, 512 }, { 3, 5, 512 }, { 3, 6, 512 }, { 4, 7, 512 }, 
+   { 2, 8, 512 }, { 3, 9, 512 }, { 3, 10, 512 }, { 4, 11, 512 }, { 3, 12, 512 }, { 4, 13, 512 }, { 4, 14, 512 }, { 5, 15, 512 }, 
+   { 2, 16, 512 }, { 3, 17, 512 }, { 3, 18, 512 }, { 4, 19, 512 }, { 3, 20, 512 }, { 4, 21, 512 }, { 4, 22, 512 }, { 5, 23, 512 }, 
+   { 3, 24, 512 }, { 4, 25, 512 }, { 4, 26, 512 }, { 5, 27, 512 }, { 4, 28, 512 }, { 5, 29, 512 }, { 5, 30, 512 }, { 6, 31, 512 }, 
+   { 2, 32, 512 }, { 3, 33, 512 }, { 3, 34, 512 }, { 4, 35, 512 }, { 3, 36, 512 }, { 4, 37, 512 }, { 4, 38, 512 }, { 5, 39, 512 }, 
+   { 3, 40, 512 }, { 4, 41, 512 }, { 4, 42, 512 }, { 5, 43, 512 }, { 4, 44, 512 }, { 5, 45, 512 }, { 5, 46, 512 }, { 6, 47, 512 }, 
+   { 3, 48, 512 }, { 4, 49, 512 }, { 4, 50, 512 }, { 5, 51, 512 }, { 4, 52, 512 }, { 5, 53, 512 }, { 5, 54, 512 }, { 6, 55, 512 }, 
+   { 4, 56, 512 }, { 5, 57, 512 }, { 5, 58, 512 }, { 6, 59, 512 }, { 5, 60, 512 }, { 6, 61, 512 }, { 6, 62, 512 }, { 7, 63, 512 }, 
+   { 2, 64, 512 }, { 3, 65, 512 }, { 3, 66, 512 }, { 4, 67, 512 }, { 3, 68, 512 }, { 4, 69, 512 }, { 4, 70, 512 }, { 5, 71, 512 }, 
+   { 3, 72, 512 }, { 4, 73, 512 }, { 4, 74, 512 }, { 5, 75, 512 }, { 4, 76, 512 }, { 5, 77, 512 }, { 5, 78, 512 }, { 6, 79, 512 }, 
+   { 3, 80, 512 }, { 4, 81, 512 }, { 4, 82, 512 }, { 5, 83, 512 }, { 4, 84, 512 }, { 5, 85, 512 }, { 5, 86, 512 }, { 6, 87, 512 }, 
+   { 4, 88, 512 }, { 5, 89, 512 }, { 5, 90, 512 }, { 6, 91, 512 }, { 5, 92, 512 }, { 6, 93, 512 }, { 6, 94, 512 }, { 7, 95, 512 }, 
+   { 3, 96, 512 }, { 4, 97, 512 }, { 4, 98, 512 }, { 5, 99, 512 }, { 4, 100, 512 }, { 5, 101, 512 }, { 5, 102, 512 }, { 6, 103, 512 }, 
+   { 4, 104, 512 }, { 5, 105, 512 }, { 5, 106, 512 }, { 6, 107, 512 }, { 5, 108, 512 }, { 6, 109, 512 }, { 6, 110, 512 }, { 7, 111, 512 }, 
+   { 4, 112, 512 }, { 5, 113, 512 }, { 5, 114, 512 }, { 6, 115, 512 }, { 5, 116, 512 }, { 6, 117, 512 }, { 6, 118, 512 }, { 7, 119, 512 }, 
+   { 5, 120, 512 }, { 6, 121, 512 }, { 6, 122, 512 }, { 7, 123, 512 }, { 6, 124, 512 }, { 7, 125, 512 }, { 7, 126, 512 }, { 8, 127, 512 }, 
+   { 2, 128, 512 }, { 3, 129, 512 }, { 3, 130, 512 }, { 4, 131, 512 }, { 3, 132, 512 }, { 4, 133, 512 }, { 4, 134, 512 }, { 5, 135, 512 }, 
+   { 3, 136, 512 }, { 4, 137, 512 }, { 4, 138, 512 }, { 5, 139, 512 }, { 4, 140, 512 }, { 5, 141, 512 }, { 5, 142, 512 }, { 6, 143, 512 }, 
+   { 3, 144, 512 }, { 4, 145, 512 }, { 4, 146, 512 }, { 5, 147, 512 }, { 4, 148, 512 }, { 5, 149, 512 }, { 5, 150, 512 }, { 6, 151, 512 }, 
+   { 4, 152, 512 }, { 5, 153, 512 }, { 5, 154, 512 }, { 6, 155, 512 }, { 5, 156, 512 }, { 6, 157, 512 }, { 6, 158, 512 }, { 7, 159, 512 }, 
+   { 3, 160, 512 }, { 4, 161, 512 }, { 4, 162, 512 }, { 5, 163, 512 }, { 4, 164, 512 }, { 5, 165, 512 }, { 5, 166, 512 }, { 6, 167, 512 }, 
+   { 4, 168, 512 }, { 5, 169, 512 }, { 5, 170, 512 }, { 6, 171, 512 }, { 5, 172, 512 }, { 6, 173, 512 }, { 6, 174, 512 }, { 7, 175, 512 }, 
+   { 4, 176, 512 }, { 5, 177, 512 }, { 5, 178, 512 }, { 6, 179, 512 }, { 5, 180, 512 }, { 6, 181, 512 }, { 6, 182, 512 }, { 7, 183, 512 }, 
+   { 5, 184, 512 }, { 6, 185, 512 }, { 6, 186, 512 }, { 7, 187, 512 }, { 6, 188, 512 }, { 7, 189, 512 }, { 7, 190, 512 }, { 8, 191, 512 }, 
+   { 3, 192, 512 }, { 4, 193, 512 }, { 4, 194, 512 }, { 5, 195, 512 }, { 4, 196, 512 }, { 5, 197, 512 }, { 5, 198, 512 }, { 6, 199, 512 }, 
+   { 4, 200, 512 }, { 5, 201, 512 }, { 5, 202, 512 }, { 6, 203, 512 }, { 5, 204, 512 }, { 6, 205, 512 }, { 6, 206, 512 }, { 7, 207, 512 }, 
+   { 4, 208, 512 }, { 5, 209, 512 }, { 5, 210, 512 }, { 6, 211, 512 }, { 5, 212, 512 }, { 6, 213, 512 }, { 6, 214, 512 }, { 7, 215, 512 }, 
+   { 5, 216, 512 }, { 6, 217, 512 }, { 6, 218, 512 }, { 7, 219, 512 }, { 6, 220, 512 }, { 7, 221, 512 }, { 7, 222, 512 }, { 8, 223, 512 }, 
+   { 4, 224, 512 }, { 5, 225, 512 }, { 5, 226, 512 }, { 6, 227, 512 }, { 5, 228, 512 }, { 6, 229, 512 }, { 6, 230, 512 }, { 7, 231, 512 }, 
+   { 5, 232, 512 }, { 6, 233, 512 }, { 6, 234, 512 }, { 7, 235, 512 }, { 6, 236, 512 }, { 7, 237, 512 }, { 7, 238, 512 }, { 8, 239, 512 }, 
+   { 5, 240, 512 }, { 6, 241, 512 }, { 6, 242, 512 }, { 7, 243, 512 }, { 6, 244, 512 }, { 7, 245, 512 }, { 7, 246, 512 }, { 8, 247, 512 }, 
+   { 6, 248, 512 }, { 7, 249, 512 }, { 7, 250, 512 }, { 8, 251, 512 }, { 7, 252, 512 }, { 8, 253, 512 }, { 8, 254, 512 }, { 9, 255, 512 }, 
+   { 2, 256, 512 }, { 3, 257, 512 }, { 3, 258, 512 }, { 4, 259, 512 }, { 3, 260, 512 }, { 4, 261, 512 }, { 4, 262, 512 }, { 5, 263, 512 }, 
+   { 3, 264, 512 }, { 4, 265, 512 }, { 4, 266, 512 }, { 5, 267, 512 }, { 4, 268, 512 }, { 5, 269, 512 }, { 5, 270, 512 }, { 6, 271, 512 }, 
+   { 3, 272, 512 }, { 4, 273, 512 }, { 4, 274, 512 }, { 5, 275, 512 }, { 4, 276, 512 }, { 5, 277, 512 }, { 5, 278, 512 }, { 6, 279, 512 }, 
+   { 4, 280, 512 }, { 5, 281, 512 }, { 5, 282, 512 }, { 6, 283, 512 }, { 5, 284, 512 }, { 6, 285, 512 }, { 6, 286, 512 }, { 7, 287, 512 }, 
+   { 3, 288, 512 }, { 4, 289, 512 }, { 4, 290, 512 }, { 5, 291, 512 }, { 4, 292, 512 }, { 5, 293, 512 }, { 5, 294, 512 }, { 6, 295, 512 }, 
+   { 4, 296, 512 }, { 5, 297, 512 }, { 5, 298, 512 }, { 6, 299, 512 }, { 5, 300, 512 }, { 6, 301, 512 }, { 6, 302, 512 }, { 7, 303, 512 }, 
+   { 4, 304, 512 }, { 5, 305, 512 }, { 5, 306, 512 }, { 6, 307, 512 }, { 5, 308, 512 }, { 6, 309, 512 }, { 6, 310, 512 }, { 7, 311, 512 }, 
+   { 5, 312, 512 }, { 6, 313, 512 }, { 6, 314, 512 }, { 7, 315, 512 }, { 6, 316, 512 }, { 7, 317, 512 }, { 7, 318, 512 }, { 8, 319, 512 }, 
+   { 3, 320, 512 }, { 4, 321, 512 }, { 4, 322, 512 }, { 5, 323, 512 }, { 4, 324, 512 }, { 5, 325, 512 }, { 5, 326, 512 }, { 6, 327, 512 }, 
+   { 4, 328, 512 }, { 5, 329, 512 }, { 5, 330, 512 }, { 6, 331, 512 }, { 5, 332, 512 }, { 6, 333, 512 }, { 6, 334, 512 }, { 7, 335, 512 }, 
+   { 4, 336, 512 }, { 5, 337, 512 }, { 5, 338, 512 }, { 6, 339, 512 }, { 5, 340, 512 }, { 6, 341, 512 }, { 6, 342, 512 }, { 7, 343, 512 }, 
+   { 5, 344, 512 }, { 6, 345, 512 }, { 6, 346, 512 }, { 7, 347, 512 }, { 6, 348, 512 }, { 7, 349, 512 }, { 7, 350, 512 }, { 8, 351, 512 }, 
+   { 4, 352, 512 }, { 5, 353, 512 }, { 5, 354, 512 }, { 6, 355, 512 }, { 5, 356, 512 }, { 6, 357, 512 }, { 6, 358, 512 }, { 7, 359, 512 }, 
+   { 5, 360, 512 }, { 6, 361, 512 }, { 6, 362, 512 }, { 7, 363, 512 }, { 6, 364, 512 }, { 7, 365, 512 }, { 7, 366, 512 }, { 8, 367, 512 }, 
+   { 5, 368, 512 }, { 6, 369, 512 }, { 6, 370, 512 }, { 7, 371, 512 }, { 6, 372, 512 }, { 7, 373, 512 }, { 7, 374, 512 }, { 8, 375, 512 }, 
+   { 6, 376, 512 }, { 7, 377, 512 }, { 7, 378, 512 }, { 8, 379, 512 }, { 7, 380, 512 }, { 8, 381, 512 }, { 8, 382, 512 }, { 9, 383, 512 }, 
+   { 3, 384, 512 }, { 4, 385, 512 }, { 4, 386, 512 }, { 5, 387, 512 }, { 4, 388, 512 }, { 5, 389, 512 }, { 5, 390, 512 }, { 6, 391, 512 }, 
+   { 4, 392, 512 }, { 5, 393, 512 }, { 5, 394, 512 }, { 6, 395, 512 }, { 5, 396, 512 }, { 6, 397, 512 }, { 6, 398, 512 }, { 7, 399, 512 }, 
+   { 4, 400, 512 }, { 5, 401, 512 }, { 5, 402, 512 }, { 6, 403, 512 }, { 5, 404, 512 }, { 6, 405, 512 }, { 6, 406, 512 }, { 7, 407, 512 }, 
+   { 5, 408, 512 }, { 6, 409, 512 }, { 6, 410, 512 }, { 7, 411, 512 }, { 6, 412, 512 }, { 7, 413, 512 }, { 7, 414, 512 }, { 8, 415, 512 }, 
+   { 4, 416, 512 }, { 5, 417, 512 }, { 5, 418, 512 }, { 6, 419, 512 }, { 5, 420, 512 }, { 6, 421, 512 }, { 6, 422, 512 }, { 7, 423, 512 }, 
+   { 5, 424, 512 }, { 6, 425, 512 }, { 6, 426, 512 }, { 7, 427, 512 }, { 6, 428, 512 }, { 7, 429, 512 }, { 7, 430, 512 }, { 8, 431, 512 }, 
+   { 5, 432, 512 }, { 6, 433, 512 }, { 6, 434, 512 }, { 7, 435, 512 }, { 6, 436, 512 }, { 7, 437, 512 }, { 7, 438, 512 }, { 8, 439, 512 }, 
+   { 6, 440, 512 }, { 7, 441, 512 }, { 7, 442, 512 }, { 8, 443, 512 }, { 7, 444, 512 }, { 8, 445, 512 }, { 8, 446, 512 }, { 9, 447, 512 }, 
+   { 4, 448, 512 }, { 5, 449, 512 }, { 5, 450, 512 }, { 6, 451, 512 }, { 5, 452, 512 }, { 6, 453, 512 }, { 6, 454, 512 }, { 7, 455, 512 }, 
+   { 5, 456, 512 }, { 6, 457, 512 }, { 6, 458, 512 }, { 7, 459, 512 }, { 6, 460, 512 }, { 7, 461, 512 }, { 7, 462, 512 }, { 8, 463, 512 }, 
+   { 5, 464, 512 }, { 6, 465, 512 }, { 6, 466, 512 }, { 7, 467, 512 }, { 6, 468, 512 }, { 7, 469, 512 }, { 7, 470, 512 }, { 8, 471, 512 }, 
+   { 6, 472, 512 }, { 7, 473, 512 }, { 7, 474, 512 }, { 8, 475, 512 }, { 7, 476, 512 }, { 8, 477, 512 }, { 8, 478, 512 }, { 9, 479, 512 }, 
+   { 5, 480, 512 }, { 6, 481, 512 }, { 6, 482, 512 }, { 7, 483, 512 }, { 6, 484, 512 }, { 7, 485, 512 }, { 7, 486, 512 }, { 8, 487, 512 }, 
+   { 6, 488, 512 }, { 7, 489, 512 }, { 7, 490, 512 }, { 8, 491, 512 }, { 7, 492, 512 }, { 8, 493, 512 }, { 8, 494, 512 }, { 9, 495, 512 }, 
+   { 6, 496, 512 }, { 7, 497, 512 }, { 7, 498, 512 }, { 8, 499, 512 }, { 7, 500, 512 }, { 8, 501, 512 }, { 8, 502, 512 }, { 9, 503, 512 }, 
+   { 7, 504, 512 }, { 8, 505, 512 }, { 8, 506, 512 }, { 9, 507, 512 }, { 8, 508, 512 }, { 9, 509, 512 }, { 9, 510, 512 }, { 10, 511, 512 }, 
+#if FP_LUT > 10
+   { 1, 0, 0 }, { 2, 1, 1024 }, { 2, 2, 1024 }, { 3, 3, 1024 }, { 2, 4, 1024 }, { 3, 5, 1024 }, { 3, 6, 1024 }, { 4, 7, 1024 }, 
+   { 2, 8, 1024 }, { 3, 9, 1024 }, { 3, 10, 1024 }, { 4, 11, 1024 }, { 3, 12, 1024 }, { 4, 13, 1024 }, { 4, 14, 1024 }, { 5, 15, 1024 }, 
+   { 2, 16, 1024 }, { 3, 17, 1024 }, { 3, 18, 1024 }, { 4, 19, 1024 }, { 3, 20, 1024 }, { 4, 21, 1024 }, { 4, 22, 1024 }, { 5, 23, 1024 }, 
+   { 3, 24, 1024 }, { 4, 25, 1024 }, { 4, 26, 1024 }, { 5, 27, 1024 }, { 4, 28, 1024 }, { 5, 29, 1024 }, { 5, 30, 1024 }, { 6, 31, 1024 }, 
+   { 2, 32, 1024 }, { 3, 33, 1024 }, { 3, 34, 1024 }, { 4, 35, 1024 }, { 3, 36, 1024 }, { 4, 37, 1024 }, { 4, 38, 1024 }, { 5, 39, 1024 }, 
+   { 3, 40, 1024 }, { 4, 41, 1024 }, { 4, 42, 1024 }, { 5, 43, 1024 }, { 4, 44, 1024 }, { 5, 45, 1024 }, { 5, 46, 1024 }, { 6, 47, 1024 }, 
+   { 3, 48, 1024 }, { 4, 49, 1024 }, { 4, 50, 1024 }, { 5, 51, 1024 }, { 4, 52, 1024 }, { 5, 53, 1024 }, { 5, 54, 1024 }, { 6, 55, 1024 }, 
+   { 4, 56, 1024 }, { 5, 57, 1024 }, { 5, 58, 1024 }, { 6, 59, 1024 }, { 5, 60, 1024 }, { 6, 61, 1024 }, { 6, 62, 1024 }, { 7, 63, 1024 }, 
+   { 2, 64, 1024 }, { 3, 65, 1024 }, { 3, 66, 1024 }, { 4, 67, 1024 }, { 3, 68, 1024 }, { 4, 69, 1024 }, { 4, 70, 1024 }, { 5, 71, 1024 }, 
+   { 3, 72, 1024 }, { 4, 73, 1024 }, { 4, 74, 1024 }, { 5, 75, 1024 }, { 4, 76, 1024 }, { 5, 77, 1024 }, { 5, 78, 1024 }, { 6, 79, 1024 }, 
+   { 3, 80, 1024 }, { 4, 81, 1024 }, { 4, 82, 1024 }, { 5, 83, 1024 }, { 4, 84, 1024 }, { 5, 85, 1024 }, { 5, 86, 1024 }, { 6, 87, 1024 }, 
+   { 4, 88, 1024 }, { 5, 89, 1024 }, { 5, 90, 1024 }, { 6, 91, 1024 }, { 5, 92, 1024 }, { 6, 93, 1024 }, { 6, 94, 1024 }, { 7, 95, 1024 }, 
+   { 3, 96, 1024 }, { 4, 97, 1024 }, { 4, 98, 1024 }, { 5, 99, 1024 }, { 4, 100, 1024 }, { 5, 101, 1024 }, { 5, 102, 1024 }, { 6, 103, 1024 }, 
+   { 4, 104, 1024 }, { 5, 105, 1024 }, { 5, 106, 1024 }, { 6, 107, 1024 }, { 5, 108, 1024 }, { 6, 109, 1024 }, { 6, 110, 1024 }, { 7, 111, 1024 }, 
+   { 4, 112, 1024 }, { 5, 113, 1024 }, { 5, 114, 1024 }, { 6, 115, 1024 }, { 5, 116, 1024 }, { 6, 117, 1024 }, { 6, 118, 1024 }, { 7, 119, 1024 }, 
+   { 5, 120, 1024 }, { 6, 121, 1024 }, { 6, 122, 1024 }, { 7, 123, 1024 }, { 6, 124, 1024 }, { 7, 125, 1024 }, { 7, 126, 1024 }, { 8, 127, 1024 }, 
+   { 2, 128, 1024 }, { 3, 129, 1024 }, { 3, 130, 1024 }, { 4, 131, 1024 }, { 3, 132, 1024 }, { 4, 133, 1024 }, { 4, 134, 1024 }, { 5, 135, 1024 }, 
+   { 3, 136, 1024 }, { 4, 137, 1024 }, { 4, 138, 1024 }, { 5, 139, 1024 }, { 4, 140, 1024 }, { 5, 141, 1024 }, { 5, 142, 1024 }, { 6, 143, 1024 }, 
+   { 3, 144, 1024 }, { 4, 145, 1024 }, { 4, 146, 1024 }, { 5, 147, 1024 }, { 4, 148, 1024 }, { 5, 149, 1024 }, { 5, 150, 1024 }, { 6, 151, 1024 }, 
+   { 4, 152, 1024 }, { 5, 153, 1024 }, { 5, 154, 1024 }, { 6, 155, 1024 }, { 5, 156, 1024 }, { 6, 157, 1024 }, { 6, 158, 1024 }, { 7, 159, 1024 }, 
+   { 3, 160, 1024 }, { 4, 161, 1024 }, { 4, 162, 1024 }, { 5, 163, 1024 }, { 4, 164, 1024 }, { 5, 165, 1024 }, { 5, 166, 1024 }, { 6, 167, 1024 }, 
+   { 4, 168, 1024 }, { 5, 169, 1024 }, { 5, 170, 1024 }, { 6, 171, 1024 }, { 5, 172, 1024 }, { 6, 173, 1024 }, { 6, 174, 1024 }, { 7, 175, 1024 }, 
+   { 4, 176, 1024 }, { 5, 177, 1024 }, { 5, 178, 1024 }, { 6, 179, 1024 }, { 5, 180, 1024 }, { 6, 181, 1024 }, { 6, 182, 1024 }, { 7, 183, 1024 }, 
+   { 5, 184, 1024 }, { 6, 185, 1024 }, { 6, 186, 1024 }, { 7, 187, 1024 }, { 6, 188, 1024 }, { 7, 189, 1024 }, { 7, 190, 1024 }, { 8, 191, 1024 }, 
+   { 3, 192, 1024 }, { 4, 193, 1024 }, { 4, 194, 1024 }, { 5, 195, 1024 }, { 4, 196, 1024 }, { 5, 197, 1024 }, { 5, 198, 1024 }, { 6, 199, 1024 }, 
+   { 4, 200, 1024 }, { 5, 201, 1024 }, { 5, 202, 1024 }, { 6, 203, 1024 }, { 5, 204, 1024 }, { 6, 205, 1024 }, { 6, 206, 1024 }, { 7, 207, 1024 }, 
+   { 4, 208, 1024 }, { 5, 209, 1024 }, { 5, 210, 1024 }, { 6, 211, 1024 }, { 5, 212, 1024 }, { 6, 213, 1024 }, { 6, 214, 1024 }, { 7, 215, 1024 }, 
+   { 5, 216, 1024 }, { 6, 217, 1024 }, { 6, 218, 1024 }, { 7, 219, 1024 }, { 6, 220, 1024 }, { 7, 221, 1024 }, { 7, 222, 1024 }, { 8, 223, 1024 }, 
+   { 4, 224, 1024 }, { 5, 225, 1024 }, { 5, 226, 1024 }, { 6, 227, 1024 }, { 5, 228, 1024 }, { 6, 229, 1024 }, { 6, 230, 1024 }, { 7, 231, 1024 }, 
+   { 5, 232, 1024 }, { 6, 233, 1024 }, { 6, 234, 1024 }, { 7, 235, 1024 }, { 6, 236, 1024 }, { 7, 237, 1024 }, { 7, 238, 1024 }, { 8, 239, 1024 }, 
+   { 5, 240, 1024 }, { 6, 241, 1024 }, { 6, 242, 1024 }, { 7, 243, 1024 }, { 6, 244, 1024 }, { 7, 245, 1024 }, { 7, 246, 1024 }, { 8, 247, 1024 }, 
+   { 6, 248, 1024 }, { 7, 249, 1024 }, { 7, 250, 1024 }, { 8, 251, 1024 }, { 7, 252, 1024 }, { 8, 253, 1024 }, { 8, 254, 1024 }, { 9, 255, 1024 }, 
+   { 2, 256, 1024 }, { 3, 257, 1024 }, { 3, 258, 1024 }, { 4, 259, 1024 }, { 3, 260, 1024 }, { 4, 261, 1024 }, { 4, 262, 1024 }, { 5, 263, 1024 }, 
+   { 3, 264, 1024 }, { 4, 265, 1024 }, { 4, 266, 1024 }, { 5, 267, 1024 }, { 4, 268, 1024 }, { 5, 269, 1024 }, { 5, 270, 1024 }, { 6, 271, 1024 }, 
+   { 3, 272, 1024 }, { 4, 273, 1024 }, { 4, 274, 1024 }, { 5, 275, 1024 }, { 4, 276, 1024 }, { 5, 277, 1024 }, { 5, 278, 1024 }, { 6, 279, 1024 }, 
+   { 4, 280, 1024 }, { 5, 281, 1024 }, { 5, 282, 1024 }, { 6, 283, 1024 }, { 5, 284, 1024 }, { 6, 285, 1024 }, { 6, 286, 1024 }, { 7, 287, 1024 }, 
+   { 3, 288, 1024 }, { 4, 289, 1024 }, { 4, 290, 1024 }, { 5, 291, 1024 }, { 4, 292, 1024 }, { 5, 293, 1024 }, { 5, 294, 1024 }, { 6, 295, 1024 }, 
+   { 4, 296, 1024 }, { 5, 297, 1024 }, { 5, 298, 1024 }, { 6, 299, 1024 }, { 5, 300, 1024 }, { 6, 301, 1024 }, { 6, 302, 1024 }, { 7, 303, 1024 }, 
+   { 4, 304, 1024 }, { 5, 305, 1024 }, { 5, 306, 1024 }, { 6, 307, 1024 }, { 5, 308, 1024 }, { 6, 309, 1024 }, { 6, 310, 1024 }, { 7, 311, 1024 }, 
+   { 5, 312, 1024 }, { 6, 313, 1024 }, { 6, 314, 1024 }, { 7, 315, 1024 }, { 6, 316, 1024 }, { 7, 317, 1024 }, { 7, 318, 1024 }, { 8, 319, 1024 }, 
+   { 3, 320, 1024 }, { 4, 321, 1024 }, { 4, 322, 1024 }, { 5, 323, 1024 }, { 4, 324, 1024 }, { 5, 325, 1024 }, { 5, 326, 1024 }, { 6, 327, 1024 }, 
+   { 4, 328, 1024 }, { 5, 329, 1024 }, { 5, 330, 1024 }, { 6, 331, 1024 }, { 5, 332, 1024 }, { 6, 333, 1024 }, { 6, 334, 1024 }, { 7, 335, 1024 }, 
+   { 4, 336, 1024 }, { 5, 337, 1024 }, { 5, 338, 1024 }, { 6, 339, 1024 }, { 5, 340, 1024 }, { 6, 341, 1024 }, { 6, 342, 1024 }, { 7, 343, 1024 }, 
+   { 5, 344, 1024 }, { 6, 345, 1024 }, { 6, 346, 1024 }, { 7, 347, 1024 }, { 6, 348, 1024 }, { 7, 349, 1024 }, { 7, 350, 1024 }, { 8, 351, 1024 }, 
+   { 4, 352, 1024 }, { 5, 353, 1024 }, { 5, 354, 1024 }, { 6, 355, 1024 }, { 5, 356, 1024 }, { 6, 357, 1024 }, { 6, 358, 1024 }, { 7, 359, 1024 }, 
+   { 5, 360, 1024 }, { 6, 361, 1024 }, { 6, 362, 1024 }, { 7, 363, 1024 }, { 6, 364, 1024 }, { 7, 365, 1024 }, { 7, 366, 1024 }, { 8, 367, 1024 }, 
+   { 5, 368, 1024 }, { 6, 369, 1024 }, { 6, 370, 1024 }, { 7, 371, 1024 }, { 6, 372, 1024 }, { 7, 373, 1024 }, { 7, 374, 1024 }, { 8, 375, 1024 }, 
+   { 6, 376, 1024 }, { 7, 377, 1024 }, { 7, 378, 1024 }, { 8, 379, 1024 }, { 7, 380, 1024 }, { 8, 381, 1024 }, { 8, 382, 1024 }, { 9, 383, 1024 }, 
+   { 3, 384, 1024 }, { 4, 385, 1024 }, { 4, 386, 1024 }, { 5, 387, 1024 }, { 4, 388, 1024 }, { 5, 389, 1024 }, { 5, 390, 1024 }, { 6, 391, 1024 }, 
+   { 4, 392, 1024 }, { 5, 393, 1024 }, { 5, 394, 1024 }, { 6, 395, 1024 }, { 5, 396, 1024 }, { 6, 397, 1024 }, { 6, 398, 1024 }, { 7, 399, 1024 }, 
+   { 4, 400, 1024 }, { 5, 401, 1024 }, { 5, 402, 1024 }, { 6, 403, 1024 }, { 5, 404, 1024 }, { 6, 405, 1024 }, { 6, 406, 1024 }, { 7, 407, 1024 }, 
+   { 5, 408, 1024 }, { 6, 409, 1024 }, { 6, 410, 1024 }, { 7, 411, 1024 }, { 6, 412, 1024 }, { 7, 413, 1024 }, { 7, 414, 1024 }, { 8, 415, 1024 }, 
+   { 4, 416, 1024 }, { 5, 417, 1024 }, { 5, 418, 1024 }, { 6, 419, 1024 }, { 5, 420, 1024 }, { 6, 421, 1024 }, { 6, 422, 1024 }, { 7, 423, 1024 }, 
+   { 5, 424, 1024 }, { 6, 425, 1024 }, { 6, 426, 1024 }, { 7, 427, 1024 }, { 6, 428, 1024 }, { 7, 429, 1024 }, { 7, 430, 1024 }, { 8, 431, 1024 }, 
+   { 5, 432, 1024 }, { 6, 433, 1024 }, { 6, 434, 1024 }, { 7, 435, 1024 }, { 6, 436, 1024 }, { 7, 437, 1024 }, { 7, 438, 1024 }, { 8, 439, 1024 }, 
+   { 6, 440, 1024 }, { 7, 441, 1024 }, { 7, 442, 1024 }, { 8, 443, 1024 }, { 7, 444, 1024 }, { 8, 445, 1024 }, { 8, 446, 1024 }, { 9, 447, 1024 }, 
+   { 4, 448, 1024 }, { 5, 449, 1024 }, { 5, 450, 1024 }, { 6, 451, 1024 }, { 5, 452, 1024 }, { 6, 453, 1024 }, { 6, 454, 1024 }, { 7, 455, 1024 }, 
+   { 5, 456, 1024 }, { 6, 457, 1024 }, { 6, 458, 1024 }, { 7, 459, 1024 }, { 6, 460, 1024 }, { 7, 461, 1024 }, { 7, 462, 1024 }, { 8, 463, 1024 }, 
+   { 5, 464, 1024 }, { 6, 465, 1024 }, { 6, 466, 1024 }, { 7, 467, 1024 }, { 6, 468, 1024 }, { 7, 469, 1024 }, { 7, 470, 1024 }, { 8, 471, 1024 }, 
+   { 6, 472, 1024 }, { 7, 473, 1024 }, { 7, 474, 1024 }, { 8, 475, 1024 }, { 7, 476, 1024 }, { 8, 477, 1024 }, { 8, 478, 1024 }, { 9, 479, 1024 }, 
+   { 5, 480, 1024 }, { 6, 481, 1024 }, { 6, 482, 1024 }, { 7, 483, 1024 }, { 6, 484, 1024 }, { 7, 485, 1024 }, { 7, 486, 1024 }, { 8, 487, 1024 }, 
+   { 6, 488, 1024 }, { 7, 489, 1024 }, { 7, 490, 1024 }, { 8, 491, 1024 }, { 7, 492, 1024 }, { 8, 493, 1024 }, { 8, 494, 1024 }, { 9, 495, 1024 }, 
+   { 6, 496, 1024 }, { 7, 497, 1024 }, { 7, 498, 1024 }, { 8, 499, 1024 }, { 7, 500, 1024 }, { 8, 501, 1024 }, { 8, 502, 1024 }, { 9, 503, 1024 }, 
+   { 7, 504, 1024 }, { 8, 505, 1024 }, { 8, 506, 1024 }, { 9, 507, 1024 }, { 8, 508, 1024 }, { 9, 509, 1024 }, { 9, 510, 1024 }, { 10, 511, 1024 }, 
+   { 2, 512, 1024 }, { 3, 513, 1024 }, { 3, 514, 1024 }, { 4, 515, 1024 }, { 3, 516, 1024 }, { 4, 517, 1024 }, { 4, 518, 1024 }, { 5, 519, 1024 }, 
+   { 3, 520, 1024 }, { 4, 521, 1024 }, { 4, 522, 1024 }, { 5, 523, 1024 }, { 4, 524, 1024 }, { 5, 525, 1024 }, { 5, 526, 1024 }, { 6, 527, 1024 }, 
+   { 3, 528, 1024 }, { 4, 529, 1024 }, { 4, 530, 1024 }, { 5, 531, 1024 }, { 4, 532, 1024 }, { 5, 533, 1024 }, { 5, 534, 1024 }, { 6, 535, 1024 }, 
+   { 4, 536, 1024 }, { 5, 537, 1024 }, { 5, 538, 1024 }, { 6, 539, 1024 }, { 5, 540, 1024 }, { 6, 541, 1024 }, { 6, 542, 1024 }, { 7, 543, 1024 }, 
+   { 3, 544, 1024 }, { 4, 545, 1024 }, { 4, 546, 1024 }, { 5, 547, 1024 }, { 4, 548, 1024 }, { 5, 549, 1024 }, { 5, 550, 1024 }, { 6, 551, 1024 }, 
+   { 4, 552, 1024 }, { 5, 553, 1024 }, { 5, 554, 1024 }, { 6, 555, 1024 }, { 5, 556, 1024 }, { 6, 557, 1024 }, { 6, 558, 1024 }, { 7, 559, 1024 }, 
+   { 4, 560, 1024 }, { 5, 561, 1024 }, { 5, 562, 1024 }, { 6, 563, 1024 }, { 5, 564, 1024 }, { 6, 565, 1024 }, { 6, 566, 1024 }, { 7, 567, 1024 }, 
+   { 5, 568, 1024 }, { 6, 569, 1024 }, { 6, 570, 1024 }, { 7, 571, 1024 }, { 6, 572, 1024 }, { 7, 573, 1024 }, { 7, 574, 1024 }, { 8, 575, 1024 }, 
+   { 3, 576, 1024 }, { 4, 577, 1024 }, { 4, 578, 1024 }, { 5, 579, 1024 }, { 4, 580, 1024 }, { 5, 581, 1024 }, { 5, 582, 1024 }, { 6, 583, 1024 }, 
+   { 4, 584, 1024 }, { 5, 585, 1024 }, { 5, 586, 1024 }, { 6, 587, 1024 }, { 5, 588, 1024 }, { 6, 589, 1024 }, { 6, 590, 1024 }, { 7, 591, 1024 }, 
+   { 4, 592, 1024 }, { 5, 593, 1024 }, { 5, 594, 1024 }, { 6, 595, 1024 }, { 5, 596, 1024 }, { 6, 597, 1024 }, { 6, 598, 1024 }, { 7, 599, 1024 }, 
+   { 5, 600, 1024 }, { 6, 601, 1024 }, { 6, 602, 1024 }, { 7, 603, 1024 }, { 6, 604, 1024 }, { 7, 605, 1024 }, { 7, 606, 1024 }, { 8, 607, 1024 }, 
+   { 4, 608, 1024 }, { 5, 609, 1024 }, { 5, 610, 1024 }, { 6, 611, 1024 }, { 5, 612, 1024 }, { 6, 613, 1024 }, { 6, 614, 1024 }, { 7, 615, 1024 }, 
+   { 5, 616, 1024 }, { 6, 617, 1024 }, { 6, 618, 1024 }, { 7, 619, 1024 }, { 6, 620, 1024 }, { 7, 621, 1024 }, { 7, 622, 1024 }, { 8, 623, 1024 }, 
+   { 5, 624, 1024 }, { 6, 625, 1024 }, { 6, 626, 1024 }, { 7, 627, 1024 }, { 6, 628, 1024 }, { 7, 629, 1024 }, { 7, 630, 1024 }, { 8, 631, 1024 }, 
+   { 6, 632, 1024 }, { 7, 633, 1024 }, { 7, 634, 1024 }, { 8, 635, 1024 }, { 7, 636, 1024 }, { 8, 637, 1024 }, { 8, 638, 1024 }, { 9, 639, 1024 }, 
+   { 3, 640, 1024 }, { 4, 641, 1024 }, { 4, 642, 1024 }, { 5, 643, 1024 }, { 4, 644, 1024 }, { 5, 645, 1024 }, { 5, 646, 1024 }, { 6, 647, 1024 }, 
+   { 4, 648, 1024 }, { 5, 649, 1024 }, { 5, 650, 1024 }, { 6, 651, 1024 }, { 5, 652, 1024 }, { 6, 653, 1024 }, { 6, 654, 1024 }, { 7, 655, 1024 }, 
+   { 4, 656, 1024 }, { 5, 657, 1024 }, { 5, 658, 1024 }, { 6, 659, 1024 }, { 5, 660, 1024 }, { 6, 661, 1024 }, { 6, 662, 1024 }, { 7, 663, 1024 }, 
+   { 5, 664, 1024 }, { 6, 665, 1024 }, { 6, 666, 1024 }, { 7, 667, 1024 }, { 6, 668, 1024 }, { 7, 669, 1024 }, { 7, 670, 1024 }, { 8, 671, 1024 }, 
+   { 4, 672, 1024 }, { 5, 673, 1024 }, { 5, 674, 1024 }, { 6, 675, 1024 }, { 5, 676, 1024 }, { 6, 677, 1024 }, { 6, 678, 1024 }, { 7, 679, 1024 }, 
+   { 5, 680, 1024 }, { 6, 681, 1024 }, { 6, 682, 1024 }, { 7, 683, 1024 }, { 6, 684, 1024 }, { 7, 685, 1024 }, { 7, 686, 1024 }, { 8, 687, 1024 }, 
+   { 5, 688, 1024 }, { 6, 689, 1024 }, { 6, 690, 1024 }, { 7, 691, 1024 }, { 6, 692, 1024 }, { 7, 693, 1024 }, { 7, 694, 1024 }, { 8, 695, 1024 }, 
+   { 6, 696, 1024 }, { 7, 697, 1024 }, { 7, 698, 1024 }, { 8, 699, 1024 }, { 7, 700, 1024 }, { 8, 701, 1024 }, { 8, 702, 1024 }, { 9, 703, 1024 }, 
+   { 4, 704, 1024 }, { 5, 705, 1024 }, { 5, 706, 1024 }, { 6, 707, 1024 }, { 5, 708, 1024 }, { 6, 709, 1024 }, { 6, 710, 1024 }, { 7, 711, 1024 }, 
+   { 5, 712, 1024 }, { 6, 713, 1024 }, { 6, 714, 1024 }, { 7, 715, 1024 }, { 6, 716, 1024 }, { 7, 717, 1024 }, { 7, 718, 1024 }, { 8, 719, 1024 }, 
+   { 5, 720, 1024 }, { 6, 721, 1024 }, { 6, 722, 1024 }, { 7, 723, 1024 }, { 6, 724, 1024 }, { 7, 725, 1024 }, { 7, 726, 1024 }, { 8, 727, 1024 }, 
+   { 6, 728, 1024 }, { 7, 729, 1024 }, { 7, 730, 1024 }, { 8, 731, 1024 }, { 7, 732, 1024 }, { 8, 733, 1024 }, { 8, 734, 1024 }, { 9, 735, 1024 }, 
+   { 5, 736, 1024 }, { 6, 737, 1024 }, { 6, 738, 1024 }, { 7, 739, 1024 }, { 6, 740, 1024 }, { 7, 741, 1024 }, { 7, 742, 1024 }, { 8, 743, 1024 }, 
+   { 6, 744, 1024 }, { 7, 745, 1024 }, { 7, 746, 1024 }, { 8, 747, 1024 }, { 7, 748, 1024 }, { 8, 749, 1024 }, { 8, 750, 1024 }, { 9, 751, 1024 }, 
+   { 6, 752, 1024 }, { 7, 753, 1024 }, { 7, 754, 1024 }, { 8, 755, 1024 }, { 7, 756, 1024 }, { 8, 757, 1024 }, { 8, 758, 1024 }, { 9, 759, 1024 }, 
+   { 7, 760, 1024 }, { 8, 761, 1024 }, { 8, 762, 1024 }, { 9, 763, 1024 }, { 8, 764, 1024 }, { 9, 765, 1024 }, { 9, 766, 1024 }, { 10, 767, 1024 }, 
+   { 3, 768, 1024 }, { 4, 769, 1024 }, { 4, 770, 1024 }, { 5, 771, 1024 }, { 4, 772, 1024 }, { 5, 773, 1024 }, { 5, 774, 1024 }, { 6, 775, 1024 }, 
+   { 4, 776, 1024 }, { 5, 777, 1024 }, { 5, 778, 1024 }, { 6, 779, 1024 }, { 5, 780, 1024 }, { 6, 781, 1024 }, { 6, 782, 1024 }, { 7, 783, 1024 }, 
+   { 4, 784, 1024 }, { 5, 785, 1024 }, { 5, 786, 1024 }, { 6, 787, 1024 }, { 5, 788, 1024 }, { 6, 789, 1024 }, { 6, 790, 1024 }, { 7, 791, 1024 }, 
+   { 5, 792, 1024 }, { 6, 793, 1024 }, { 6, 794, 1024 }, { 7, 795, 1024 }, { 6, 796, 1024 }, { 7, 797, 1024 }, { 7, 798, 1024 }, { 8, 799, 1024 }, 
+   { 4, 800, 1024 }, { 5, 801, 1024 }, { 5, 802, 1024 }, { 6, 803, 1024 }, { 5, 804, 1024 }, { 6, 805, 1024 }, { 6, 806, 1024 }, { 7, 807, 1024 }, 
+   { 5, 808, 1024 }, { 6, 809, 1024 }, { 6, 810, 1024 }, { 7, 811, 1024 }, { 6, 812, 1024 }, { 7, 813, 1024 }, { 7, 814, 1024 }, { 8, 815, 1024 }, 
+   { 5, 816, 1024 }, { 6, 817, 1024 }, { 6, 818, 1024 }, { 7, 819, 1024 }, { 6, 820, 1024 }, { 7, 821, 1024 }, { 7, 822, 1024 }, { 8, 823, 1024 }, 
+   { 6, 824, 1024 }, { 7, 825, 1024 }, { 7, 826, 1024 }, { 8, 827, 1024 }, { 7, 828, 1024 }, { 8, 829, 1024 }, { 8, 830, 1024 }, { 9, 831, 1024 }, 
+   { 4, 832, 1024 }, { 5, 833, 1024 }, { 5, 834, 1024 }, { 6, 835, 1024 }, { 5, 836, 1024 }, { 6, 837, 1024 }, { 6, 838, 1024 }, { 7, 839, 1024 }, 
+   { 5, 840, 1024 }, { 6, 841, 1024 }, { 6, 842, 1024 }, { 7, 843, 1024 }, { 6, 844, 1024 }, { 7, 845, 1024 }, { 7, 846, 1024 }, { 8, 847, 1024 }, 
+   { 5, 848, 1024 }, { 6, 849, 1024 }, { 6, 850, 1024 }, { 7, 851, 1024 }, { 6, 852, 1024 }, { 7, 853, 1024 }, { 7, 854, 1024 }, { 8, 855, 1024 }, 
+   { 6, 856, 1024 }, { 7, 857, 1024 }, { 7, 858, 1024 }, { 8, 859, 1024 }, { 7, 860, 1024 }, { 8, 861, 1024 }, { 8, 862, 1024 }, { 9, 863, 1024 }, 
+   { 5, 864, 1024 }, { 6, 865, 1024 }, { 6, 866, 1024 }, { 7, 867, 1024 }, { 6, 868, 1024 }, { 7, 869, 1024 }, { 7, 870, 1024 }, { 8, 871, 1024 }, 
+   { 6, 872, 1024 }, { 7, 873, 1024 }, { 7, 874, 1024 }, { 8, 875, 1024 }, { 7, 876, 1024 }, { 8, 877, 1024 }, { 8, 878, 1024 }, { 9, 879, 1024 }, 
+   { 6, 880, 1024 }, { 7, 881, 1024 }, { 7, 882, 1024 }, { 8, 883, 1024 }, { 7, 884, 1024 }, { 8, 885, 1024 }, { 8, 886, 1024 }, { 9, 887, 1024 }, 
+   { 7, 888, 1024 }, { 8, 889, 1024 }, { 8, 890, 1024 }, { 9, 891, 1024 }, { 8, 892, 1024 }, { 9, 893, 1024 }, { 9, 894, 1024 }, { 10, 895, 1024 }, 
+   { 4, 896, 1024 }, { 5, 897, 1024 }, { 5, 898, 1024 }, { 6, 899, 1024 }, { 5, 900, 1024 }, { 6, 901, 1024 }, { 6, 902, 1024 }, { 7, 903, 1024 }, 
+   { 5, 904, 1024 }, { 6, 905, 1024 }, { 6, 906, 1024 }, { 7, 907, 1024 }, { 6, 908, 1024 }, { 7, 909, 1024 }, { 7, 910, 1024 }, { 8, 911, 1024 }, 
+   { 5, 912, 1024 }, { 6, 913, 1024 }, { 6, 914, 1024 }, { 7, 915, 1024 }, { 6, 916, 1024 }, { 7, 917, 1024 }, { 7, 918, 1024 }, { 8, 919, 1024 }, 
+   { 6, 920, 1024 }, { 7, 921, 1024 }, { 7, 922, 1024 }, { 8, 923, 1024 }, { 7, 924, 1024 }, { 8, 925, 1024 }, { 8, 926, 1024 }, { 9, 927, 1024 }, 
+   { 5, 928, 1024 }, { 6, 929, 1024 }, { 6, 930, 1024 }, { 7, 931, 1024 }, { 6, 932, 1024 }, { 7, 933, 1024 }, { 7, 934, 1024 }, { 8, 935, 1024 }, 
+   { 6, 936, 1024 }, { 7, 937, 1024 }, { 7, 938, 1024 }, { 8, 939, 1024 }, { 7, 940, 1024 }, { 8, 941, 1024 }, { 8, 942, 1024 }, { 9, 943, 1024 }, 
+   { 6, 944, 1024 }, { 7, 945, 1024 }, { 7, 946, 1024 }, { 8, 947, 1024 }, { 7, 948, 1024 }, { 8, 949, 1024 }, { 8, 950, 1024 }, { 9, 951, 1024 }, 
+   { 7, 952, 1024 }, { 8, 953, 1024 }, { 8, 954, 1024 }, { 9, 955, 1024 }, { 8, 956, 1024 }, { 9, 957, 1024 }, { 9, 958, 1024 }, { 10, 959, 1024 }, 
+   { 5, 960, 1024 }, { 6, 961, 1024 }, { 6, 962, 1024 }, { 7, 963, 1024 }, { 6, 964, 1024 }, { 7, 965, 1024 }, { 7, 966, 1024 }, { 8, 967, 1024 }, 
+   { 6, 968, 1024 }, { 7, 969, 1024 }, { 7, 970, 1024 }, { 8, 971, 1024 }, { 7, 972, 1024 }, { 8, 973, 1024 }, { 8, 974, 1024 }, { 9, 975, 1024 }, 
+   { 6, 976, 1024 }, { 7, 977, 1024 }, { 7, 978, 1024 }, { 8, 979, 1024 }, { 7, 980, 1024 }, { 8, 981, 1024 }, { 8, 982, 1024 }, { 9, 983, 1024 }, 
+   { 7, 984, 1024 }, { 8, 985, 1024 }, { 8, 986, 1024 }, { 9, 987, 1024 }, { 8, 988, 1024 }, { 9, 989, 1024 }, { 9, 990, 1024 }, { 10, 991, 1024 }, 
+   { 6, 992, 1024 }, { 7, 993, 1024 }, { 7, 994, 1024 }, { 8, 995, 1024 }, { 7, 996, 1024 }, { 8, 997, 1024 }, { 8, 998, 1024 }, { 9, 999, 1024 }, 
+   { 7, 1000, 1024 }, { 8, 1001, 1024 }, { 8, 1002, 1024 }, { 9, 1003, 1024 }, { 8, 1004, 1024 }, { 9, 1005, 1024 }, { 9, 1006, 1024 }, { 10, 1007, 1024 }, 
+   { 7, 1008, 1024 }, { 8, 1009, 1024 }, { 8, 1010, 1024 }, { 9, 1011, 1024 }, { 8, 1012, 1024 }, { 9, 1013, 1024 }, { 9, 1014, 1024 }, { 10, 1015, 1024 }, 
+   { 8, 1016, 1024 }, { 9, 1017, 1024 }, { 9, 1018, 1024 }, { 10, 1019, 1024 }, { 9, 1020, 1024 }, { 10, 1021, 1024 }, { 10, 1022, 1024 }, { 11, 1023, 1024 }, 
+#if FP_LUT > 11
+   { 1, 0, 0 }, { 2, 1, 2048 }, { 2, 2, 2048 }, { 3, 3, 2048 }, { 2, 4, 2048 }, { 3, 5, 2048 }, { 3, 6, 2048 }, { 4, 7, 2048 }, 
+   { 2, 8, 2048 }, { 3, 9, 2048 }, { 3, 10, 2048 }, { 4, 11, 2048 }, { 3, 12, 2048 }, { 4, 13, 2048 }, { 4, 14, 2048 }, { 5, 15, 2048 }, 
+   { 2, 16, 2048 }, { 3, 17, 2048 }, { 3, 18, 2048 }, { 4, 19, 2048 }, { 3, 20, 2048 }, { 4, 21, 2048 }, { 4, 22, 2048 }, { 5, 23, 2048 }, 
+   { 3, 24, 2048 }, { 4, 25, 2048 }, { 4, 26, 2048 }, { 5, 27, 2048 }, { 4, 28, 2048 }, { 5, 29, 2048 }, { 5, 30, 2048 }, { 6, 31, 2048 }, 
+   { 2, 32, 2048 }, { 3, 33, 2048 }, { 3, 34, 2048 }, { 4, 35, 2048 }, { 3, 36, 2048 }, { 4, 37, 2048 }, { 4, 38, 2048 }, { 5, 39, 2048 }, 
+   { 3, 40, 2048 }, { 4, 41, 2048 }, { 4, 42, 2048 }, { 5, 43, 2048 }, { 4, 44, 2048 }, { 5, 45, 2048 }, { 5, 46, 2048 }, { 6, 47, 2048 }, 
+   { 3, 48, 2048 }, { 4, 49, 2048 }, { 4, 50, 2048 }, { 5, 51, 2048 }, { 4, 52, 2048 }, { 5, 53, 2048 }, { 5, 54, 2048 }, { 6, 55, 2048 }, 
+   { 4, 56, 2048 }, { 5, 57, 2048 }, { 5, 58, 2048 }, { 6, 59, 2048 }, { 5, 60, 2048 }, { 6, 61, 2048 }, { 6, 62, 2048 }, { 7, 63, 2048 }, 
+   { 2, 64, 2048 }, { 3, 65, 2048 }, { 3, 66, 2048 }, { 4, 67, 2048 }, { 3, 68, 2048 }, { 4, 69, 2048 }, { 4, 70, 2048 }, { 5, 71, 2048 }, 
+   { 3, 72, 2048 }, { 4, 73, 2048 }, { 4, 74, 2048 }, { 5, 75, 2048 }, { 4, 76, 2048 }, { 5, 77, 2048 }, { 5, 78, 2048 }, { 6, 79, 2048 }, 
+   { 3, 80, 2048 }, { 4, 81, 2048 }, { 4, 82, 2048 }, { 5, 83, 2048 }, { 4, 84, 2048 }, { 5, 85, 2048 }, { 5, 86, 2048 }, { 6, 87, 2048 }, 
+   { 4, 88, 2048 }, { 5, 89, 2048 }, { 5, 90, 2048 }, { 6, 91, 2048 }, { 5, 92, 2048 }, { 6, 93, 2048 }, { 6, 94, 2048 }, { 7, 95, 2048 }, 
+   { 3, 96, 2048 }, { 4, 97, 2048 }, { 4, 98, 2048 }, { 5, 99, 2048 }, { 4, 100, 2048 }, { 5, 101, 2048 }, { 5, 102, 2048 }, { 6, 103, 2048 }, 
+   { 4, 104, 2048 }, { 5, 105, 2048 }, { 5, 106, 2048 }, { 6, 107, 2048 }, { 5, 108, 2048 }, { 6, 109, 2048 }, { 6, 110, 2048 }, { 7, 111, 2048 }, 
+   { 4, 112, 2048 }, { 5, 113, 2048 }, { 5, 114, 2048 }, { 6, 115, 2048 }, { 5, 116, 2048 }, { 6, 117, 2048 }, { 6, 118, 2048 }, { 7, 119, 2048 }, 
+   { 5, 120, 2048 }, { 6, 121, 2048 }, { 6, 122, 2048 }, { 7, 123, 2048 }, { 6, 124, 2048 }, { 7, 125, 2048 }, { 7, 126, 2048 }, { 8, 127, 2048 }, 
+   { 2, 128, 2048 }, { 3, 129, 2048 }, { 3, 130, 2048 }, { 4, 131, 2048 }, { 3, 132, 2048 }, { 4, 133, 2048 }, { 4, 134, 2048 }, { 5, 135, 2048 }, 
+   { 3, 136, 2048 }, { 4, 137, 2048 }, { 4, 138, 2048 }, { 5, 139, 2048 }, { 4, 140, 2048 }, { 5, 141, 2048 }, { 5, 142, 2048 }, { 6, 143, 2048 }, 
+   { 3, 144, 2048 }, { 4, 145, 2048 }, { 4, 146, 2048 }, { 5, 147, 2048 }, { 4, 148, 2048 }, { 5, 149, 2048 }, { 5, 150, 2048 }, { 6, 151, 2048 }, 
+   { 4, 152, 2048 }, { 5, 153, 2048 }, { 5, 154, 2048 }, { 6, 155, 2048 }, { 5, 156, 2048 }, { 6, 157, 2048 }, { 6, 158, 2048 }, { 7, 159, 2048 }, 
+   { 3, 160, 2048 }, { 4, 161, 2048 }, { 4, 162, 2048 }, { 5, 163, 2048 }, { 4, 164, 2048 }, { 5, 165, 2048 }, { 5, 166, 2048 }, { 6, 167, 2048 }, 
+   { 4, 168, 2048 }, { 5, 169, 2048 }, { 5, 170, 2048 }, { 6, 171, 2048 }, { 5, 172, 2048 }, { 6, 173, 2048 }, { 6, 174, 2048 }, { 7, 175, 2048 }, 
+   { 4, 176, 2048 }, { 5, 177, 2048 }, { 5, 178, 2048 }, { 6, 179, 2048 }, { 5, 180, 2048 }, { 6, 181, 2048 }, { 6, 182, 2048 }, { 7, 183, 2048 }, 
+   { 5, 184, 2048 }, { 6, 185, 2048 }, { 6, 186, 2048 }, { 7, 187, 2048 }, { 6, 188, 2048 }, { 7, 189, 2048 }, { 7, 190, 2048 }, { 8, 191, 2048 }, 
+   { 3, 192, 2048 }, { 4, 193, 2048 }, { 4, 194, 2048 }, { 5, 195, 2048 }, { 4, 196, 2048 }, { 5, 197, 2048 }, { 5, 198, 2048 }, { 6, 199, 2048 }, 
+   { 4, 200, 2048 }, { 5, 201, 2048 }, { 5, 202, 2048 }, { 6, 203, 2048 }, { 5, 204, 2048 }, { 6, 205, 2048 }, { 6, 206, 2048 }, { 7, 207, 2048 }, 
+   { 4, 208, 2048 }, { 5, 209, 2048 }, { 5, 210, 2048 }, { 6, 211, 2048 }, { 5, 212, 2048 }, { 6, 213, 2048 }, { 6, 214, 2048 }, { 7, 215, 2048 }, 
+   { 5, 216, 2048 }, { 6, 217, 2048 }, { 6, 218, 2048 }, { 7, 219, 2048 }, { 6, 220, 2048 }, { 7, 221, 2048 }, { 7, 222, 2048 }, { 8, 223, 2048 }, 
+   { 4, 224, 2048 }, { 5, 225, 2048 }, { 5, 226, 2048 }, { 6, 227, 2048 }, { 5, 228, 2048 }, { 6, 229, 2048 }, { 6, 230, 2048 }, { 7, 231, 2048 }, 
+   { 5, 232, 2048 }, { 6, 233, 2048 }, { 6, 234, 2048 }, { 7, 235, 2048 }, { 6, 236, 2048 }, { 7, 237, 2048 }, { 7, 238, 2048 }, { 8, 239, 2048 }, 
+   { 5, 240, 2048 }, { 6, 241, 2048 }, { 6, 242, 2048 }, { 7, 243, 2048 }, { 6, 244, 2048 }, { 7, 245, 2048 }, { 7, 246, 2048 }, { 8, 247, 2048 }, 
+   { 6, 248, 2048 }, { 7, 249, 2048 }, { 7, 250, 2048 }, { 8, 251, 2048 }, { 7, 252, 2048 }, { 8, 253, 2048 }, { 8, 254, 2048 }, { 9, 255, 2048 }, 
+   { 2, 256, 2048 }, { 3, 257, 2048 }, { 3, 258, 2048 }, { 4, 259, 2048 }, { 3, 260, 2048 }, { 4, 261, 2048 }, { 4, 262, 2048 }, { 5, 263, 2048 }, 
+   { 3, 264, 2048 }, { 4, 265, 2048 }, { 4, 266, 2048 }, { 5, 267, 2048 }, { 4, 268, 2048 }, { 5, 269, 2048 }, { 5, 270, 2048 }, { 6, 271, 2048 }, 
+   { 3, 272, 2048 }, { 4, 273, 2048 }, { 4, 274, 2048 }, { 5, 275, 2048 }, { 4, 276, 2048 }, { 5, 277, 2048 }, { 5, 278, 2048 }, { 6, 279, 2048 }, 
+   { 4, 280, 2048 }, { 5, 281, 2048 }, { 5, 282, 2048 }, { 6, 283, 2048 }, { 5, 284, 2048 }, { 6, 285, 2048 }, { 6, 286, 2048 }, { 7, 287, 2048 }, 
+   { 3, 288, 2048 }, { 4, 289, 2048 }, { 4, 290, 2048 }, { 5, 291, 2048 }, { 4, 292, 2048 }, { 5, 293, 2048 }, { 5, 294, 2048 }, { 6, 295, 2048 }, 
+   { 4, 296, 2048 }, { 5, 297, 2048 }, { 5, 298, 2048 }, { 6, 299, 2048 }, { 5, 300, 2048 }, { 6, 301, 2048 }, { 6, 302, 2048 }, { 7, 303, 2048 }, 
+   { 4, 304, 2048 }, { 5, 305, 2048 }, { 5, 306, 2048 }, { 6, 307, 2048 }, { 5, 308, 2048 }, { 6, 309, 2048 }, { 6, 310, 2048 }, { 7, 311, 2048 }, 
+   { 5, 312, 2048 }, { 6, 313, 2048 }, { 6, 314, 2048 }, { 7, 315, 2048 }, { 6, 316, 2048 }, { 7, 317, 2048 }, { 7, 318, 2048 }, { 8, 319, 2048 }, 
+   { 3, 320, 2048 }, { 4, 321, 2048 }, { 4, 322, 2048 }, { 5, 323, 2048 }, { 4, 324, 2048 }, { 5, 325, 2048 }, { 5, 326, 2048 }, { 6, 327, 2048 }, 
+   { 4, 328, 2048 }, { 5, 329, 2048 }, { 5, 330, 2048 }, { 6, 331, 2048 }, { 5, 332, 2048 }, { 6, 333, 2048 }, { 6, 334, 2048 }, { 7, 335, 2048 }, 
+   { 4, 336, 2048 }, { 5, 337, 2048 }, { 5, 338, 2048 }, { 6, 339, 2048 }, { 5, 340, 2048 }, { 6, 341, 2048 }, { 6, 342, 2048 }, { 7, 343, 2048 }, 
+   { 5, 344, 2048 }, { 6, 345, 2048 }, { 6, 346, 2048 }, { 7, 347, 2048 }, { 6, 348, 2048 }, { 7, 349, 2048 }, { 7, 350, 2048 }, { 8, 351, 2048 }, 
+   { 4, 352, 2048 }, { 5, 353, 2048 }, { 5, 354, 2048 }, { 6, 355, 2048 }, { 5, 356, 2048 }, { 6, 357, 2048 }, { 6, 358, 2048 }, { 7, 359, 2048 }, 
+   { 5, 360, 2048 }, { 6, 361, 2048 }, { 6, 362, 2048 }, { 7, 363, 2048 }, { 6, 364, 2048 }, { 7, 365, 2048 }, { 7, 366, 2048 }, { 8, 367, 2048 }, 
+   { 5, 368, 2048 }, { 6, 369, 2048 }, { 6, 370, 2048 }, { 7, 371, 2048 }, { 6, 372, 2048 }, { 7, 373, 2048 }, { 7, 374, 2048 }, { 8, 375, 2048 }, 
+   { 6, 376, 2048 }, { 7, 377, 2048 }, { 7, 378, 2048 }, { 8, 379, 2048 }, { 7, 380, 2048 }, { 8, 381, 2048 }, { 8, 382, 2048 }, { 9, 383, 2048 }, 
+   { 3, 384, 2048 }, { 4, 385, 2048 }, { 4, 386, 2048 }, { 5, 387, 2048 }, { 4, 388, 2048 }, { 5, 389, 2048 }, { 5, 390, 2048 }, { 6, 391, 2048 }, 
+   { 4, 392, 2048 }, { 5, 393, 2048 }, { 5, 394, 2048 }, { 6, 395, 2048 }, { 5, 396, 2048 }, { 6, 397, 2048 }, { 6, 398, 2048 }, { 7, 399, 2048 }, 
+   { 4, 400, 2048 }, { 5, 401, 2048 }, { 5, 402, 2048 }, { 6, 403, 2048 }, { 5, 404, 2048 }, { 6, 405, 2048 }, { 6, 406, 2048 }, { 7, 407, 2048 }, 
+   { 5, 408, 2048 }, { 6, 409, 2048 }, { 6, 410, 2048 }, { 7, 411, 2048 }, { 6, 412, 2048 }, { 7, 413, 2048 }, { 7, 414, 2048 }, { 8, 415, 2048 }, 
+   { 4, 416, 2048 }, { 5, 417, 2048 }, { 5, 418, 2048 }, { 6, 419, 2048 }, { 5, 420, 2048 }, { 6, 421, 2048 }, { 6, 422, 2048 }, { 7, 423, 2048 }, 
+   { 5, 424, 2048 }, { 6, 425, 2048 }, { 6, 426, 2048 }, { 7, 427, 2048 }, { 6, 428, 2048 }, { 7, 429, 2048 }, { 7, 430, 2048 }, { 8, 431, 2048 }, 
+   { 5, 432, 2048 }, { 6, 433, 2048 }, { 6, 434, 2048 }, { 7, 435, 2048 }, { 6, 436, 2048 }, { 7, 437, 2048 }, { 7, 438, 2048 }, { 8, 439, 2048 }, 
+   { 6, 440, 2048 }, { 7, 441, 2048 }, { 7, 442, 2048 }, { 8, 443, 2048 }, { 7, 444, 2048 }, { 8, 445, 2048 }, { 8, 446, 2048 }, { 9, 447, 2048 }, 
+   { 4, 448, 2048 }, { 5, 449, 2048 }, { 5, 450, 2048 }, { 6, 451, 2048 }, { 5, 452, 2048 }, { 6, 453, 2048 }, { 6, 454, 2048 }, { 7, 455, 2048 }, 
+   { 5, 456, 2048 }, { 6, 457, 2048 }, { 6, 458, 2048 }, { 7, 459, 2048 }, { 6, 460, 2048 }, { 7, 461, 2048 }, { 7, 462, 2048 }, { 8, 463, 2048 }, 
+   { 5, 464, 2048 }, { 6, 465, 2048 }, { 6, 466, 2048 }, { 7, 467, 2048 }, { 6, 468, 2048 }, { 7, 469, 2048 }, { 7, 470, 2048 }, { 8, 471, 2048 }, 
+   { 6, 472, 2048 }, { 7, 473, 2048 }, { 7, 474, 2048 }, { 8, 475, 2048 }, { 7, 476, 2048 }, { 8, 477, 2048 }, { 8, 478, 2048 }, { 9, 479, 2048 }, 
+   { 5, 480, 2048 }, { 6, 481, 2048 }, { 6, 482, 2048 }, { 7, 483, 2048 }, { 6, 484, 2048 }, { 7, 485, 2048 }, { 7, 486, 2048 }, { 8, 487, 2048 }, 
+   { 6, 488, 2048 }, { 7, 489, 2048 }, { 7, 490, 2048 }, { 8, 491, 2048 }, { 7, 492, 2048 }, { 8, 493, 2048 }, { 8, 494, 2048 }, { 9, 495, 2048 }, 
+   { 6, 496, 2048 }, { 7, 497, 2048 }, { 7, 498, 2048 }, { 8, 499, 2048 }, { 7, 500, 2048 }, { 8, 501, 2048 }, { 8, 502, 2048 }, { 9, 503, 2048 }, 
+   { 7, 504, 2048 }, { 8, 505, 2048 }, { 8, 506, 2048 }, { 9, 507, 2048 }, { 8, 508, 2048 }, { 9, 509, 2048 }, { 9, 510, 2048 }, { 10, 511, 2048 }, 
+   { 2, 512, 2048 }, { 3, 513, 2048 }, { 3, 514, 2048 }, { 4, 515, 2048 }, { 3, 516, 2048 }, { 4, 517, 2048 }, { 4, 518, 2048 }, { 5, 519, 2048 }, 
+   { 3, 520, 2048 }, { 4, 521, 2048 }, { 4, 522, 2048 }, { 5, 523, 2048 }, { 4, 524, 2048 }, { 5, 525, 2048 }, { 5, 526, 2048 }, { 6, 527, 2048 }, 
+   { 3, 528, 2048 }, { 4, 529, 2048 }, { 4, 530, 2048 }, { 5, 531, 2048 }, { 4, 532, 2048 }, { 5, 533, 2048 }, { 5, 534, 2048 }, { 6, 535, 2048 }, 
+   { 4, 536, 2048 }, { 5, 537, 2048 }, { 5, 538, 2048 }, { 6, 539, 2048 }, { 5, 540, 2048 }, { 6, 541, 2048 }, { 6, 542, 2048 }, { 7, 543, 2048 }, 
+   { 3, 544, 2048 }, { 4, 545, 2048 }, { 4, 546, 2048 }, { 5, 547, 2048 }, { 4, 548, 2048 }, { 5, 549, 2048 }, { 5, 550, 2048 }, { 6, 551, 2048 }, 
+   { 4, 552, 2048 }, { 5, 553, 2048 }, { 5, 554, 2048 }, { 6, 555, 2048 }, { 5, 556, 2048 }, { 6, 557, 2048 }, { 6, 558, 2048 }, { 7, 559, 2048 }, 
+   { 4, 560, 2048 }, { 5, 561, 2048 }, { 5, 562, 2048 }, { 6, 563, 2048 }, { 5, 564, 2048 }, { 6, 565, 2048 }, { 6, 566, 2048 }, { 7, 567, 2048 }, 
+   { 5, 568, 2048 }, { 6, 569, 2048 }, { 6, 570, 2048 }, { 7, 571, 2048 }, { 6, 572, 2048 }, { 7, 573, 2048 }, { 7, 574, 2048 }, { 8, 575, 2048 }, 
+   { 3, 576, 2048 }, { 4, 577, 2048 }, { 4, 578, 2048 }, { 5, 579, 2048 }, { 4, 580, 2048 }, { 5, 581, 2048 }, { 5, 582, 2048 }, { 6, 583, 2048 }, 
+   { 4, 584, 2048 }, { 5, 585, 2048 }, { 5, 586, 2048 }, { 6, 587, 2048 }, { 5, 588, 2048 }, { 6, 589, 2048 }, { 6, 590, 2048 }, { 7, 591, 2048 }, 
+   { 4, 592, 2048 }, { 5, 593, 2048 }, { 5, 594, 2048 }, { 6, 595, 2048 }, { 5, 596, 2048 }, { 6, 597, 2048 }, { 6, 598, 2048 }, { 7, 599, 2048 }, 
+   { 5, 600, 2048 }, { 6, 601, 2048 }, { 6, 602, 2048 }, { 7, 603, 2048 }, { 6, 604, 2048 }, { 7, 605, 2048 }, { 7, 606, 2048 }, { 8, 607, 2048 }, 
+   { 4, 608, 2048 }, { 5, 609, 2048 }, { 5, 610, 2048 }, { 6, 611, 2048 }, { 5, 612, 2048 }, { 6, 613, 2048 }, { 6, 614, 2048 }, { 7, 615, 2048 }, 
+   { 5, 616, 2048 }, { 6, 617, 2048 }, { 6, 618, 2048 }, { 7, 619, 2048 }, { 6, 620, 2048 }, { 7, 621, 2048 }, { 7, 622, 2048 }, { 8, 623, 2048 }, 
+   { 5, 624, 2048 }, { 6, 625, 2048 }, { 6, 626, 2048 }, { 7, 627, 2048 }, { 6, 628, 2048 }, { 7, 629, 2048 }, { 7, 630, 2048 }, { 8, 631, 2048 }, 
+   { 6, 632, 2048 }, { 7, 633, 2048 }, { 7, 634, 2048 }, { 8, 635, 2048 }, { 7, 636, 2048 }, { 8, 637, 2048 }, { 8, 638, 2048 }, { 9, 639, 2048 }, 
+   { 3, 640, 2048 }, { 4, 641, 2048 }, { 4, 642, 2048 }, { 5, 643, 2048 }, { 4, 644, 2048 }, { 5, 645, 2048 }, { 5, 646, 2048 }, { 6, 647, 2048 }, 
+   { 4, 648, 2048 }, { 5, 649, 2048 }, { 5, 650, 2048 }, { 6, 651, 2048 }, { 5, 652, 2048 }, { 6, 653, 2048 }, { 6, 654, 2048 }, { 7, 655, 2048 }, 
+   { 4, 656, 2048 }, { 5, 657, 2048 }, { 5, 658, 2048 }, { 6, 659, 2048 }, { 5, 660, 2048 }, { 6, 661, 2048 }, { 6, 662, 2048 }, { 7, 663, 2048 }, 
+   { 5, 664, 2048 }, { 6, 665, 2048 }, { 6, 666, 2048 }, { 7, 667, 2048 }, { 6, 668, 2048 }, { 7, 669, 2048 }, { 7, 670, 2048 }, { 8, 671, 2048 }, 
+   { 4, 672, 2048 }, { 5, 673, 2048 }, { 5, 674, 2048 }, { 6, 675, 2048 }, { 5, 676, 2048 }, { 6, 677, 2048 }, { 6, 678, 2048 }, { 7, 679, 2048 }, 
+   { 5, 680, 2048 }, { 6, 681, 2048 }, { 6, 682, 2048 }, { 7, 683, 2048 }, { 6, 684, 2048 }, { 7, 685, 2048 }, { 7, 686, 2048 }, { 8, 687, 2048 }, 
+   { 5, 688, 2048 }, { 6, 689, 2048 }, { 6, 690, 2048 }, { 7, 691, 2048 }, { 6, 692, 2048 }, { 7, 693, 2048 }, { 7, 694, 2048 }, { 8, 695, 2048 }, 
+   { 6, 696, 2048 }, { 7, 697, 2048 }, { 7, 698, 2048 }, { 8, 699, 2048 }, { 7, 700, 2048 }, { 8, 701, 2048 }, { 8, 702, 2048 }, { 9, 703, 2048 }, 
+   { 4, 704, 2048 }, { 5, 705, 2048 }, { 5, 706, 2048 }, { 6, 707, 2048 }, { 5, 708, 2048 }, { 6, 709, 2048 }, { 6, 710, 2048 }, { 7, 711, 2048 }, 
+   { 5, 712, 2048 }, { 6, 713, 2048 }, { 6, 714, 2048 }, { 7, 715, 2048 }, { 6, 716, 2048 }, { 7, 717, 2048 }, { 7, 718, 2048 }, { 8, 719, 2048 }, 
+   { 5, 720, 2048 }, { 6, 721, 2048 }, { 6, 722, 2048 }, { 7, 723, 2048 }, { 6, 724, 2048 }, { 7, 725, 2048 }, { 7, 726, 2048 }, { 8, 727, 2048 }, 
+   { 6, 728, 2048 }, { 7, 729, 2048 }, { 7, 730, 2048 }, { 8, 731, 2048 }, { 7, 732, 2048 }, { 8, 733, 2048 }, { 8, 734, 2048 }, { 9, 735, 2048 }, 
+   { 5, 736, 2048 }, { 6, 737, 2048 }, { 6, 738, 2048 }, { 7, 739, 2048 }, { 6, 740, 2048 }, { 7, 741, 2048 }, { 7, 742, 2048 }, { 8, 743, 2048 }, 
+   { 6, 744, 2048 }, { 7, 745, 2048 }, { 7, 746, 2048 }, { 8, 747, 2048 }, { 7, 748, 2048 }, { 8, 749, 2048 }, { 8, 750, 2048 }, { 9, 751, 2048 }, 
+   { 6, 752, 2048 }, { 7, 753, 2048 }, { 7, 754, 2048 }, { 8, 755, 2048 }, { 7, 756, 2048 }, { 8, 757, 2048 }, { 8, 758, 2048 }, { 9, 759, 2048 }, 
+   { 7, 760, 2048 }, { 8, 761, 2048 }, { 8, 762, 2048 }, { 9, 763, 2048 }, { 8, 764, 2048 }, { 9, 765, 2048 }, { 9, 766, 2048 }, { 10, 767, 2048 }, 
+   { 3, 768, 2048 }, { 4, 769, 2048 }, { 4, 770, 2048 }, { 5, 771, 2048 }, { 4, 772, 2048 }, { 5, 773, 2048 }, { 5, 774, 2048 }, { 6, 775, 2048 }, 
+   { 4, 776, 2048 }, { 5, 777, 2048 }, { 5, 778, 2048 }, { 6, 779, 2048 }, { 5, 780, 2048 }, { 6, 781, 2048 }, { 6, 782, 2048 }, { 7, 783, 2048 }, 
+   { 4, 784, 2048 }, { 5, 785, 2048 }, { 5, 786, 2048 }, { 6, 787, 2048 }, { 5, 788, 2048 }, { 6, 789, 2048 }, { 6, 790, 2048 }, { 7, 791, 2048 }, 
+   { 5, 792, 2048 }, { 6, 793, 2048 }, { 6, 794, 2048 }, { 7, 795, 2048 }, { 6, 796, 2048 }, { 7, 797, 2048 }, { 7, 798, 2048 }, { 8, 799, 2048 }, 
+   { 4, 800, 2048 }, { 5, 801, 2048 }, { 5, 802, 2048 }, { 6, 803, 2048 }, { 5, 804, 2048 }, { 6, 805, 2048 }, { 6, 806, 2048 }, { 7, 807, 2048 }, 
+   { 5, 808, 2048 }, { 6, 809, 2048 }, { 6, 810, 2048 }, { 7, 811, 2048 }, { 6, 812, 2048 }, { 7, 813, 2048 }, { 7, 814, 2048 }, { 8, 815, 2048 }, 
+   { 5, 816, 2048 }, { 6, 817, 2048 }, { 6, 818, 2048 }, { 7, 819, 2048 }, { 6, 820, 2048 }, { 7, 821, 2048 }, { 7, 822, 2048 }, { 8, 823, 2048 }, 
+   { 6, 824, 2048 }, { 7, 825, 2048 }, { 7, 826, 2048 }, { 8, 827, 2048 }, { 7, 828, 2048 }, { 8, 829, 2048 }, { 8, 830, 2048 }, { 9, 831, 2048 }, 
+   { 4, 832, 2048 }, { 5, 833, 2048 }, { 5, 834, 2048 }, { 6, 835, 2048 }, { 5, 836, 2048 }, { 6, 837, 2048 }, { 6, 838, 2048 }, { 7, 839, 2048 }, 
+   { 5, 840, 2048 }, { 6, 841, 2048 }, { 6, 842, 2048 }, { 7, 843, 2048 }, { 6, 844, 2048 }, { 7, 845, 2048 }, { 7, 846, 2048 }, { 8, 847, 2048 }, 
+   { 5, 848, 2048 }, { 6, 849, 2048 }, { 6, 850, 2048 }, { 7, 851, 2048 }, { 6, 852, 2048 }, { 7, 853, 2048 }, { 7, 854, 2048 }, { 8, 855, 2048 }, 
+   { 6, 856, 2048 }, { 7, 857, 2048 }, { 7, 858, 2048 }, { 8, 859, 2048 }, { 7, 860, 2048 }, { 8, 861, 2048 }, { 8, 862, 2048 }, { 9, 863, 2048 }, 
+   { 5, 864, 2048 }, { 6, 865, 2048 }, { 6, 866, 2048 }, { 7, 867, 2048 }, { 6, 868, 2048 }, { 7, 869, 2048 }, { 7, 870, 2048 }, { 8, 871, 2048 }, 
+   { 6, 872, 2048 }, { 7, 873, 2048 }, { 7, 874, 2048 }, { 8, 875, 2048 }, { 7, 876, 2048 }, { 8, 877, 2048 }, { 8, 878, 2048 }, { 9, 879, 2048 }, 
+   { 6, 880, 2048 }, { 7, 881, 2048 }, { 7, 882, 2048 }, { 8, 883, 2048 }, { 7, 884, 2048 }, { 8, 885, 2048 }, { 8, 886, 2048 }, { 9, 887, 2048 }, 
+   { 7, 888, 2048 }, { 8, 889, 2048 }, { 8, 890, 2048 }, { 9, 891, 2048 }, { 8, 892, 2048 }, { 9, 893, 2048 }, { 9, 894, 2048 }, { 10, 895, 2048 }, 
+   { 4, 896, 2048 }, { 5, 897, 2048 }, { 5, 898, 2048 }, { 6, 899, 2048 }, { 5, 900, 2048 }, { 6, 901, 2048 }, { 6, 902, 2048 }, { 7, 903, 2048 }, 
+   { 5, 904, 2048 }, { 6, 905, 2048 }, { 6, 906, 2048 }, { 7, 907, 2048 }, { 6, 908, 2048 }, { 7, 909, 2048 }, { 7, 910, 2048 }, { 8, 911, 2048 }, 
+   { 5, 912, 2048 }, { 6, 913, 2048 }, { 6, 914, 2048 }, { 7, 915, 2048 }, { 6, 916, 2048 }, { 7, 917, 2048 }, { 7, 918, 2048 }, { 8, 919, 2048 }, 
+   { 6, 920, 2048 }, { 7, 921, 2048 }, { 7, 922, 2048 }, { 8, 923, 2048 }, { 7, 924, 2048 }, { 8, 925, 2048 }, { 8, 926, 2048 }, { 9, 927, 2048 }, 
+   { 5, 928, 2048 }, { 6, 929, 2048 }, { 6, 930, 2048 }, { 7, 931, 2048 }, { 6, 932, 2048 }, { 7, 933, 2048 }, { 7, 934, 2048 }, { 8, 935, 2048 }, 
+   { 6, 936, 2048 }, { 7, 937, 2048 }, { 7, 938, 2048 }, { 8, 939, 2048 }, { 7, 940, 2048 }, { 8, 941, 2048 }, { 8, 942, 2048 }, { 9, 943, 2048 }, 
+   { 6, 944, 2048 }, { 7, 945, 2048 }, { 7, 946, 2048 }, { 8, 947, 2048 }, { 7, 948, 2048 }, { 8, 949, 2048 }, { 8, 950, 2048 }, { 9, 951, 2048 }, 
+   { 7, 952, 2048 }, { 8, 953, 2048 }, { 8, 954, 2048 }, { 9, 955, 2048 }, { 8, 956, 2048 }, { 9, 957, 2048 }, { 9, 958, 2048 }, { 10, 959, 2048 }, 
+   { 5, 960, 2048 }, { 6, 961, 2048 }, { 6, 962, 2048 }, { 7, 963, 2048 }, { 6, 964, 2048 }, { 7, 965, 2048 }, { 7, 966, 2048 }, { 8, 967, 2048 }, 
+   { 6, 968, 2048 }, { 7, 969, 2048 }, { 7, 970, 2048 }, { 8, 971, 2048 }, { 7, 972, 2048 }, { 8, 973, 2048 }, { 8, 974, 2048 }, { 9, 975, 2048 }, 
+   { 6, 976, 2048 }, { 7, 977, 2048 }, { 7, 978, 2048 }, { 8, 979, 2048 }, { 7, 980, 2048 }, { 8, 981, 2048 }, { 8, 982, 2048 }, { 9, 983, 2048 }, 
+   { 7, 984, 2048 }, { 8, 985, 2048 }, { 8, 986, 2048 }, { 9, 987, 2048 }, { 8, 988, 2048 }, { 9, 989, 2048 }, { 9, 990, 2048 }, { 10, 991, 2048 }, 
+   { 6, 992, 2048 }, { 7, 993, 2048 }, { 7, 994, 2048 }, { 8, 995, 2048 }, { 7, 996, 2048 }, { 8, 997, 2048 }, { 8, 998, 2048 }, { 9, 999, 2048 }, 
+   { 7, 1000, 2048 }, { 8, 1001, 2048 }, { 8, 1002, 2048 }, { 9, 1003, 2048 }, { 8, 1004, 2048 }, { 9, 1005, 2048 }, { 9, 1006, 2048 }, { 10, 1007, 2048 }, 
+   { 7, 1008, 2048 }, { 8, 1009, 2048 }, { 8, 1010, 2048 }, { 9, 1011, 2048 }, { 8, 1012, 2048 }, { 9, 1013, 2048 }, { 9, 1014, 2048 }, { 10, 1015, 2048 }, 
+   { 8, 1016, 2048 }, { 9, 1017, 2048 }, { 9, 1018, 2048 }, { 10, 1019, 2048 }, { 9, 1020, 2048 }, { 10, 1021, 2048 }, { 10, 1022, 2048 }, { 11, 1023, 2048 }, 
+   { 2, 1024, 2048 }, { 3, 1025, 2048 }, { 3, 1026, 2048 }, { 4, 1027, 2048 }, { 3, 1028, 2048 }, { 4, 1029, 2048 }, { 4, 1030, 2048 }, { 5, 1031, 2048 }, 
+   { 3, 1032, 2048 }, { 4, 1033, 2048 }, { 4, 1034, 2048 }, { 5, 1035, 2048 }, { 4, 1036, 2048 }, { 5, 1037, 2048 }, { 5, 1038, 2048 }, { 6, 1039, 2048 }, 
+   { 3, 1040, 2048 }, { 4, 1041, 2048 }, { 4, 1042, 2048 }, { 5, 1043, 2048 }, { 4, 1044, 2048 }, { 5, 1045, 2048 }, { 5, 1046, 2048 }, { 6, 1047, 2048 }, 
+   { 4, 1048, 2048 }, { 5, 1049, 2048 }, { 5, 1050, 2048 }, { 6, 1051, 2048 }, { 5, 1052, 2048 }, { 6, 1053, 2048 }, { 6, 1054, 2048 }, { 7, 1055, 2048 }, 
+   { 3, 1056, 2048 }, { 4, 1057, 2048 }, { 4, 1058, 2048 }, { 5, 1059, 2048 }, { 4, 1060, 2048 }, { 5, 1061, 2048 }, { 5, 1062, 2048 }, { 6, 1063, 2048 }, 
+   { 4, 1064, 2048 }, { 5, 1065, 2048 }, { 5, 1066, 2048 }, { 6, 1067, 2048 }, { 5, 1068, 2048 }, { 6, 1069, 2048 }, { 6, 1070, 2048 }, { 7, 1071, 2048 }, 
+   { 4, 1072, 2048 }, { 5, 1073, 2048 }, { 5, 1074, 2048 }, { 6, 1075, 2048 }, { 5, 1076, 2048 }, { 6, 1077, 2048 }, { 6, 1078, 2048 }, { 7, 1079, 2048 }, 
+   { 5, 1080, 2048 }, { 6, 1081, 2048 }, { 6, 1082, 2048 }, { 7, 1083, 2048 }, { 6, 1084, 2048 }, { 7, 1085, 2048 }, { 7, 1086, 2048 }, { 8, 1087, 2048 }, 
+   { 3, 1088, 2048 }, { 4, 1089, 2048 }, { 4, 1090, 2048 }, { 5, 1091, 2048 }, { 4, 1092, 2048 }, { 5, 1093, 2048 }, { 5, 1094, 2048 }, { 6, 1095, 2048 }, 
+   { 4, 1096, 2048 }, { 5, 1097, 2048 }, { 5, 1098, 2048 }, { 6, 1099, 2048 }, { 5, 1100, 2048 }, { 6, 1101, 2048 }, { 6, 1102, 2048 }, { 7, 1103, 2048 }, 
+   { 4, 1104, 2048 }, { 5, 1105, 2048 }, { 5, 1106, 2048 }, { 6, 1107, 2048 }, { 5, 1108, 2048 }, { 6, 1109, 2048 }, { 6, 1110, 2048 }, { 7, 1111, 2048 }, 
+   { 5, 1112, 2048 }, { 6, 1113, 2048 }, { 6, 1114, 2048 }, { 7, 1115, 2048 }, { 6, 1116, 2048 }, { 7, 1117, 2048 }, { 7, 1118, 2048 }, { 8, 1119, 2048 }, 
+   { 4, 1120, 2048 }, { 5, 1121, 2048 }, { 5, 1122, 2048 }, { 6, 1123, 2048 }, { 5, 1124, 2048 }, { 6, 1125, 2048 }, { 6, 1126, 2048 }, { 7, 1127, 2048 }, 
+   { 5, 1128, 2048 }, { 6, 1129, 2048 }, { 6, 1130, 2048 }, { 7, 1131, 2048 }, { 6, 1132, 2048 }, { 7, 1133, 2048 }, { 7, 1134, 2048 }, { 8, 1135, 2048 }, 
+   { 5, 1136, 2048 }, { 6, 1137, 2048 }, { 6, 1138, 2048 }, { 7, 1139, 2048 }, { 6, 1140, 2048 }, { 7, 1141, 2048 }, { 7, 1142, 2048 }, { 8, 1143, 2048 }, 
+   { 6, 1144, 2048 }, { 7, 1145, 2048 }, { 7, 1146, 2048 }, { 8, 1147, 2048 }, { 7, 1148, 2048 }, { 8, 1149, 2048 }, { 8, 1150, 2048 }, { 9, 1151, 2048 }, 
+   { 3, 1152, 2048 }, { 4, 1153, 2048 }, { 4, 1154, 2048 }, { 5, 1155, 2048 }, { 4, 1156, 2048 }, { 5, 1157, 2048 }, { 5, 1158, 2048 }, { 6, 1159, 2048 }, 
+   { 4, 1160, 2048 }, { 5, 1161, 2048 }, { 5, 1162, 2048 }, { 6, 1163, 2048 }, { 5, 1164, 2048 }, { 6, 1165, 2048 }, { 6, 1166, 2048 }, { 7, 1167, 2048 }, 
+   { 4, 1168, 2048 }, { 5, 1169, 2048 }, { 5, 1170, 2048 }, { 6, 1171, 2048 }, { 5, 1172, 2048 }, { 6, 1173, 2048 }, { 6, 1174, 2048 }, { 7, 1175, 2048 }, 
+   { 5, 1176, 2048 }, { 6, 1177, 2048 }, { 6, 1178, 2048 }, { 7, 1179, 2048 }, { 6, 1180, 2048 }, { 7, 1181, 2048 }, { 7, 1182, 2048 }, { 8, 1183, 2048 }, 
+   { 4, 1184, 2048 }, { 5, 1185, 2048 }, { 5, 1186, 2048 }, { 6, 1187, 2048 }, { 5, 1188, 2048 }, { 6, 1189, 2048 }, { 6, 1190, 2048 }, { 7, 1191, 2048 }, 
+   { 5, 1192, 2048 }, { 6, 1193, 2048 }, { 6, 1194, 2048 }, { 7, 1195, 2048 }, { 6, 1196, 2048 }, { 7, 1197, 2048 }, { 7, 1198, 2048 }, { 8, 1199, 2048 }, 
+   { 5, 1200, 2048 }, { 6, 1201, 2048 }, { 6, 1202, 2048 }, { 7, 1203, 2048 }, { 6, 1204, 2048 }, { 7, 1205, 2048 }, { 7, 1206, 2048 }, { 8, 1207, 2048 }, 
+   { 6, 1208, 2048 }, { 7, 1209, 2048 }, { 7, 1210, 2048 }, { 8, 1211, 2048 }, { 7, 1212, 2048 }, { 8, 1213, 2048 }, { 8, 1214, 2048 }, { 9, 1215, 2048 }, 
+   { 4, 1216, 2048 }, { 5, 1217, 2048 }, { 5, 1218, 2048 }, { 6, 1219, 2048 }, { 5, 1220, 2048 }, { 6, 1221, 2048 }, { 6, 1222, 2048 }, { 7, 1223, 2048 }, 
+   { 5, 1224, 2048 }, { 6, 1225, 2048 }, { 6, 1226, 2048 }, { 7, 1227, 2048 }, { 6, 1228, 2048 }, { 7, 1229, 2048 }, { 7, 1230, 2048 }, { 8, 1231, 2048 }, 
+   { 5, 1232, 2048 }, { 6, 1233, 2048 }, { 6, 1234, 2048 }, { 7, 1235, 2048 }, { 6, 1236, 2048 }, { 7, 1237, 2048 }, { 7, 1238, 2048 }, { 8, 1239, 2048 }, 
+   { 6, 1240, 2048 }, { 7, 1241, 2048 }, { 7, 1242, 2048 }, { 8, 1243, 2048 }, { 7, 1244, 2048 }, { 8, 1245, 2048 }, { 8, 1246, 2048 }, { 9, 1247, 2048 }, 
+   { 5, 1248, 2048 }, { 6, 1249, 2048 }, { 6, 1250, 2048 }, { 7, 1251, 2048 }, { 6, 1252, 2048 }, { 7, 1253, 2048 }, { 7, 1254, 2048 }, { 8, 1255, 2048 }, 
+   { 6, 1256, 2048 }, { 7, 1257, 2048 }, { 7, 1258, 2048 }, { 8, 1259, 2048 }, { 7, 1260, 2048 }, { 8, 1261, 2048 }, { 8, 1262, 2048 }, { 9, 1263, 2048 }, 
+   { 6, 1264, 2048 }, { 7, 1265, 2048 }, { 7, 1266, 2048 }, { 8, 1267, 2048 }, { 7, 1268, 2048 }, { 8, 1269, 2048 }, { 8, 1270, 2048 }, { 9, 1271, 2048 }, 
+   { 7, 1272, 2048 }, { 8, 1273, 2048 }, { 8, 1274, 2048 }, { 9, 1275, 2048 }, { 8, 1276, 2048 }, { 9, 1277, 2048 }, { 9, 1278, 2048 }, { 10, 1279, 2048 }, 
+   { 3, 1280, 2048 }, { 4, 1281, 2048 }, { 4, 1282, 2048 }, { 5, 1283, 2048 }, { 4, 1284, 2048 }, { 5, 1285, 2048 }, { 5, 1286, 2048 }, { 6, 1287, 2048 }, 
+   { 4, 1288, 2048 }, { 5, 1289, 2048 }, { 5, 1290, 2048 }, { 6, 1291, 2048 }, { 5, 1292, 2048 }, { 6, 1293, 2048 }, { 6, 1294, 2048 }, { 7, 1295, 2048 }, 
+   { 4, 1296, 2048 }, { 5, 1297, 2048 }, { 5, 1298, 2048 }, { 6, 1299, 2048 }, { 5, 1300, 2048 }, { 6, 1301, 2048 }, { 6, 1302, 2048 }, { 7, 1303, 2048 }, 
+   { 5, 1304, 2048 }, { 6, 1305, 2048 }, { 6, 1306, 2048 }, { 7, 1307, 2048 }, { 6, 1308, 2048 }, { 7, 1309, 2048 }, { 7, 1310, 2048 }, { 8, 1311, 2048 }, 
+   { 4, 1312, 2048 }, { 5, 1313, 2048 }, { 5, 1314, 2048 }, { 6, 1315, 2048 }, { 5, 1316, 2048 }, { 6, 1317, 2048 }, { 6, 1318, 2048 }, { 7, 1319, 2048 }, 
+   { 5, 1320, 2048 }, { 6, 1321, 2048 }, { 6, 1322, 2048 }, { 7, 1323, 2048 }, { 6, 1324, 2048 }, { 7, 1325, 2048 }, { 7, 1326, 2048 }, { 8, 1327, 2048 }, 
+   { 5, 1328, 2048 }, { 6, 1329, 2048 }, { 6, 1330, 2048 }, { 7, 1331, 2048 }, { 6, 1332, 2048 }, { 7, 1333, 2048 }, { 7, 1334, 2048 }, { 8, 1335, 2048 }, 
+   { 6, 1336, 2048 }, { 7, 1337, 2048 }, { 7, 1338, 2048 }, { 8, 1339, 2048 }, { 7, 1340, 2048 }, { 8, 1341, 2048 }, { 8, 1342, 2048 }, { 9, 1343, 2048 }, 
+   { 4, 1344, 2048 }, { 5, 1345, 2048 }, { 5, 1346, 2048 }, { 6, 1347, 2048 }, { 5, 1348, 2048 }, { 6, 1349, 2048 }, { 6, 1350, 2048 }, { 7, 1351, 2048 }, 
+   { 5, 1352, 2048 }, { 6, 1353, 2048 }, { 6, 1354, 2048 }, { 7, 1355, 2048 }, { 6, 1356, 2048 }, { 7, 1357, 2048 }, { 7, 1358, 2048 }, { 8, 1359, 2048 }, 
+   { 5, 1360, 2048 }, { 6, 1361, 2048 }, { 6, 1362, 2048 }, { 7, 1363, 2048 }, { 6, 1364, 2048 }, { 7, 1365, 2048 }, { 7, 1366, 2048 }, { 8, 1367, 2048 }, 
+   { 6, 1368, 2048 }, { 7, 1369, 2048 }, { 7, 1370, 2048 }, { 8, 1371, 2048 }, { 7, 1372, 2048 }, { 8, 1373, 2048 }, { 8, 1374, 2048 }, { 9, 1375, 2048 }, 
+   { 5, 1376, 2048 }, { 6, 1377, 2048 }, { 6, 1378, 2048 }, { 7, 1379, 2048 }, { 6, 1380, 2048 }, { 7, 1381, 2048 }, { 7, 1382, 2048 }, { 8, 1383, 2048 }, 
+   { 6, 1384, 2048 }, { 7, 1385, 2048 }, { 7, 1386, 2048 }, { 8, 1387, 2048 }, { 7, 1388, 2048 }, { 8, 1389, 2048 }, { 8, 1390, 2048 }, { 9, 1391, 2048 }, 
+   { 6, 1392, 2048 }, { 7, 1393, 2048 }, { 7, 1394, 2048 }, { 8, 1395, 2048 }, { 7, 1396, 2048 }, { 8, 1397, 2048 }, { 8, 1398, 2048 }, { 9, 1399, 2048 }, 
+   { 7, 1400, 2048 }, { 8, 1401, 2048 }, { 8, 1402, 2048 }, { 9, 1403, 2048 }, { 8, 1404, 2048 }, { 9, 1405, 2048 }, { 9, 1406, 2048 }, { 10, 1407, 2048 }, 
+   { 4, 1408, 2048 }, { 5, 1409, 2048 }, { 5, 1410, 2048 }, { 6, 1411, 2048 }, { 5, 1412, 2048 }, { 6, 1413, 2048 }, { 6, 1414, 2048 }, { 7, 1415, 2048 }, 
+   { 5, 1416, 2048 }, { 6, 1417, 2048 }, { 6, 1418, 2048 }, { 7, 1419, 2048 }, { 6, 1420, 2048 }, { 7, 1421, 2048 }, { 7, 1422, 2048 }, { 8, 1423, 2048 }, 
+   { 5, 1424, 2048 }, { 6, 1425, 2048 }, { 6, 1426, 2048 }, { 7, 1427, 2048 }, { 6, 1428, 2048 }, { 7, 1429, 2048 }, { 7, 1430, 2048 }, { 8, 1431, 2048 }, 
+   { 6, 1432, 2048 }, { 7, 1433, 2048 }, { 7, 1434, 2048 }, { 8, 1435, 2048 }, { 7, 1436, 2048 }, { 8, 1437, 2048 }, { 8, 1438, 2048 }, { 9, 1439, 2048 }, 
+   { 5, 1440, 2048 }, { 6, 1441, 2048 }, { 6, 1442, 2048 }, { 7, 1443, 2048 }, { 6, 1444, 2048 }, { 7, 1445, 2048 }, { 7, 1446, 2048 }, { 8, 1447, 2048 }, 
+   { 6, 1448, 2048 }, { 7, 1449, 2048 }, { 7, 1450, 2048 }, { 8, 1451, 2048 }, { 7, 1452, 2048 }, { 8, 1453, 2048 }, { 8, 1454, 2048 }, { 9, 1455, 2048 }, 
+   { 6, 1456, 2048 }, { 7, 1457, 2048 }, { 7, 1458, 2048 }, { 8, 1459, 2048 }, { 7, 1460, 2048 }, { 8, 1461, 2048 }, { 8, 1462, 2048 }, { 9, 1463, 2048 }, 
+   { 7, 1464, 2048 }, { 8, 1465, 2048 }, { 8, 1466, 2048 }, { 9, 1467, 2048 }, { 8, 1468, 2048 }, { 9, 1469, 2048 }, { 9, 1470, 2048 }, { 10, 1471, 2048 }, 
+   { 5, 1472, 2048 }, { 6, 1473, 2048 }, { 6, 1474, 2048 }, { 7, 1475, 2048 }, { 6, 1476, 2048 }, { 7, 1477, 2048 }, { 7, 1478, 2048 }, { 8, 1479, 2048 }, 
+   { 6, 1480, 2048 }, { 7, 1481, 2048 }, { 7, 1482, 2048 }, { 8, 1483, 2048 }, { 7, 1484, 2048 }, { 8, 1485, 2048 }, { 8, 1486, 2048 }, { 9, 1487, 2048 }, 
+   { 6, 1488, 2048 }, { 7, 1489, 2048 }, { 7, 1490, 2048 }, { 8, 1491, 2048 }, { 7, 1492, 2048 }, { 8, 1493, 2048 }, { 8, 1494, 2048 }, { 9, 1495, 2048 }, 
+   { 7, 1496, 2048 }, { 8, 1497, 2048 }, { 8, 1498, 2048 }, { 9, 1499, 2048 }, { 8, 1500, 2048 }, { 9, 1501, 2048 }, { 9, 1502, 2048 }, { 10, 1503, 2048 }, 
+   { 6, 1504, 2048 }, { 7, 1505, 2048 }, { 7, 1506, 2048 }, { 8, 1507, 2048 }, { 7, 1508, 2048 }, { 8, 1509, 2048 }, { 8, 1510, 2048 }, { 9, 1511, 2048 }, 
+   { 7, 1512, 2048 }, { 8, 1513, 2048 }, { 8, 1514, 2048 }, { 9, 1515, 2048 }, { 8, 1516, 2048 }, { 9, 1517, 2048 }, { 9, 1518, 2048 }, { 10, 1519, 2048 }, 
+   { 7, 1520, 2048 }, { 8, 1521, 2048 }, { 8, 1522, 2048 }, { 9, 1523, 2048 }, { 8, 1524, 2048 }, { 9, 1525, 2048 }, { 9, 1526, 2048 }, { 10, 1527, 2048 }, 
+   { 8, 1528, 2048 }, { 9, 1529, 2048 }, { 9, 1530, 2048 }, { 10, 1531, 2048 }, { 9, 1532, 2048 }, { 10, 1533, 2048 }, { 10, 1534, 2048 }, { 11, 1535, 2048 }, 
+   { 3, 1536, 2048 }, { 4, 1537, 2048 }, { 4, 1538, 2048 }, { 5, 1539, 2048 }, { 4, 1540, 2048 }, { 5, 1541, 2048 }, { 5, 1542, 2048 }, { 6, 1543, 2048 }, 
+   { 4, 1544, 2048 }, { 5, 1545, 2048 }, { 5, 1546, 2048 }, { 6, 1547, 2048 }, { 5, 1548, 2048 }, { 6, 1549, 2048 }, { 6, 1550, 2048 }, { 7, 1551, 2048 }, 
+   { 4, 1552, 2048 }, { 5, 1553, 2048 }, { 5, 1554, 2048 }, { 6, 1555, 2048 }, { 5, 1556, 2048 }, { 6, 1557, 2048 }, { 6, 1558, 2048 }, { 7, 1559, 2048 }, 
+   { 5, 1560, 2048 }, { 6, 1561, 2048 }, { 6, 1562, 2048 }, { 7, 1563, 2048 }, { 6, 1564, 2048 }, { 7, 1565, 2048 }, { 7, 1566, 2048 }, { 8, 1567, 2048 }, 
+   { 4, 1568, 2048 }, { 5, 1569, 2048 }, { 5, 1570, 2048 }, { 6, 1571, 2048 }, { 5, 1572, 2048 }, { 6, 1573, 2048 }, { 6, 1574, 2048 }, { 7, 1575, 2048 }, 
+   { 5, 1576, 2048 }, { 6, 1577, 2048 }, { 6, 1578, 2048 }, { 7, 1579, 2048 }, { 6, 1580, 2048 }, { 7, 1581, 2048 }, { 7, 1582, 2048 }, { 8, 1583, 2048 }, 
+   { 5, 1584, 2048 }, { 6, 1585, 2048 }, { 6, 1586, 2048 }, { 7, 1587, 2048 }, { 6, 1588, 2048 }, { 7, 1589, 2048 }, { 7, 1590, 2048 }, { 8, 1591, 2048 }, 
+   { 6, 1592, 2048 }, { 7, 1593, 2048 }, { 7, 1594, 2048 }, { 8, 1595, 2048 }, { 7, 1596, 2048 }, { 8, 1597, 2048 }, { 8, 1598, 2048 }, { 9, 1599, 2048 }, 
+   { 4, 1600, 2048 }, { 5, 1601, 2048 }, { 5, 1602, 2048 }, { 6, 1603, 2048 }, { 5, 1604, 2048 }, { 6, 1605, 2048 }, { 6, 1606, 2048 }, { 7, 1607, 2048 }, 
+   { 5, 1608, 2048 }, { 6, 1609, 2048 }, { 6, 1610, 2048 }, { 7, 1611, 2048 }, { 6, 1612, 2048 }, { 7, 1613, 2048 }, { 7, 1614, 2048 }, { 8, 1615, 2048 }, 
+   { 5, 1616, 2048 }, { 6, 1617, 2048 }, { 6, 1618, 2048 }, { 7, 1619, 2048 }, { 6, 1620, 2048 }, { 7, 1621, 2048 }, { 7, 1622, 2048 }, { 8, 1623, 2048 }, 
+   { 6, 1624, 2048 }, { 7, 1625, 2048 }, { 7, 1626, 2048 }, { 8, 1627, 2048 }, { 7, 1628, 2048 }, { 8, 1629, 2048 }, { 8, 1630, 2048 }, { 9, 1631, 2048 }, 
+   { 5, 1632, 2048 }, { 6, 1633, 2048 }, { 6, 1634, 2048 }, { 7, 1635, 2048 }, { 6, 1636, 2048 }, { 7, 1637, 2048 }, { 7, 1638, 2048 }, { 8, 1639, 2048 }, 
+   { 6, 1640, 2048 }, { 7, 1641, 2048 }, { 7, 1642, 2048 }, { 8, 1643, 2048 }, { 7, 1644, 2048 }, { 8, 1645, 2048 }, { 8, 1646, 2048 }, { 9, 1647, 2048 }, 
+   { 6, 1648, 2048 }, { 7, 1649, 2048 }, { 7, 1650, 2048 }, { 8, 1651, 2048 }, { 7, 1652, 2048 }, { 8, 1653, 2048 }, { 8, 1654, 2048 }, { 9, 1655, 2048 }, 
+   { 7, 1656, 2048 }, { 8, 1657, 2048 }, { 8, 1658, 2048 }, { 9, 1659, 2048 }, { 8, 1660, 2048 }, { 9, 1661, 2048 }, { 9, 1662, 2048 }, { 10, 1663, 2048 }, 
+   { 4, 1664, 2048 }, { 5, 1665, 2048 }, { 5, 1666, 2048 }, { 6, 1667, 2048 }, { 5, 1668, 2048 }, { 6, 1669, 2048 }, { 6, 1670, 2048 }, { 7, 1671, 2048 }, 
+   { 5, 1672, 2048 }, { 6, 1673, 2048 }, { 6, 1674, 2048 }, { 7, 1675, 2048 }, { 6, 1676, 2048 }, { 7, 1677, 2048 }, { 7, 1678, 2048 }, { 8, 1679, 2048 }, 
+   { 5, 1680, 2048 }, { 6, 1681, 2048 }, { 6, 1682, 2048 }, { 7, 1683, 2048 }, { 6, 1684, 2048 }, { 7, 1685, 2048 }, { 7, 1686, 2048 }, { 8, 1687, 2048 }, 
+   { 6, 1688, 2048 }, { 7, 1689, 2048 }, { 7, 1690, 2048 }, { 8, 1691, 2048 }, { 7, 1692, 2048 }, { 8, 1693, 2048 }, { 8, 1694, 2048 }, { 9, 1695, 2048 }, 
+   { 5, 1696, 2048 }, { 6, 1697, 2048 }, { 6, 1698, 2048 }, { 7, 1699, 2048 }, { 6, 1700, 2048 }, { 7, 1701, 2048 }, { 7, 1702, 2048 }, { 8, 1703, 2048 }, 
+   { 6, 1704, 2048 }, { 7, 1705, 2048 }, { 7, 1706, 2048 }, { 8, 1707, 2048 }, { 7, 1708, 2048 }, { 8, 1709, 2048 }, { 8, 1710, 2048 }, { 9, 1711, 2048 }, 
+   { 6, 1712, 2048 }, { 7, 1713, 2048 }, { 7, 1714, 2048 }, { 8, 1715, 2048 }, { 7, 1716, 2048 }, { 8, 1717, 2048 }, { 8, 1718, 2048 }, { 9, 1719, 2048 }, 
+   { 7, 1720, 2048 }, { 8, 1721, 2048 }, { 8, 1722, 2048 }, { 9, 1723, 2048 }, { 8, 1724, 2048 }, { 9, 1725, 2048 }, { 9, 1726, 2048 }, { 10, 1727, 2048 }, 
+   { 5, 1728, 2048 }, { 6, 1729, 2048 }, { 6, 1730, 2048 }, { 7, 1731, 2048 }, { 6, 1732, 2048 }, { 7, 1733, 2048 }, { 7, 1734, 2048 }, { 8, 1735, 2048 }, 
+   { 6, 1736, 2048 }, { 7, 1737, 2048 }, { 7, 1738, 2048 }, { 8, 1739, 2048 }, { 7, 1740, 2048 }, { 8, 1741, 2048 }, { 8, 1742, 2048 }, { 9, 1743, 2048 }, 
+   { 6, 1744, 2048 }, { 7, 1745, 2048 }, { 7, 1746, 2048 }, { 8, 1747, 2048 }, { 7, 1748, 2048 }, { 8, 1749, 2048 }, { 8, 1750, 2048 }, { 9, 1751, 2048 }, 
+   { 7, 1752, 2048 }, { 8, 1753, 2048 }, { 8, 1754, 2048 }, { 9, 1755, 2048 }, { 8, 1756, 2048 }, { 9, 1757, 2048 }, { 9, 1758, 2048 }, { 10, 1759, 2048 }, 
+   { 6, 1760, 2048 }, { 7, 1761, 2048 }, { 7, 1762, 2048 }, { 8, 1763, 2048 }, { 7, 1764, 2048 }, { 8, 1765, 2048 }, { 8, 1766, 2048 }, { 9, 1767, 2048 }, 
+   { 7, 1768, 2048 }, { 8, 1769, 2048 }, { 8, 1770, 2048 }, { 9, 1771, 2048 }, { 8, 1772, 2048 }, { 9, 1773, 2048 }, { 9, 1774, 2048 }, { 10, 1775, 2048 }, 
+   { 7, 1776, 2048 }, { 8, 1777, 2048 }, { 8, 1778, 2048 }, { 9, 1779, 2048 }, { 8, 1780, 2048 }, { 9, 1781, 2048 }, { 9, 1782, 2048 }, { 10, 1783, 2048 }, 
+   { 8, 1784, 2048 }, { 9, 1785, 2048 }, { 9, 1786, 2048 }, { 10, 1787, 2048 }, { 9, 1788, 2048 }, { 10, 1789, 2048 }, { 10, 1790, 2048 }, { 11, 1791, 2048 }, 
+   { 4, 1792, 2048 }, { 5, 1793, 2048 }, { 5, 1794, 2048 }, { 6, 1795, 2048 }, { 5, 1796, 2048 }, { 6, 1797, 2048 }, { 6, 1798, 2048 }, { 7, 1799, 2048 }, 
+   { 5, 1800, 2048 }, { 6, 1801, 2048 }, { 6, 1802, 2048 }, { 7, 1803, 2048 }, { 6, 1804, 2048 }, { 7, 1805, 2048 }, { 7, 1806, 2048 }, { 8, 1807, 2048 }, 
+   { 5, 1808, 2048 }, { 6, 1809, 2048 }, { 6, 1810, 2048 }, { 7, 1811, 2048 }, { 6, 1812, 2048 }, { 7, 1813, 2048 }, { 7, 1814, 2048 }, { 8, 1815, 2048 }, 
+   { 6, 1816, 2048 }, { 7, 1817, 2048 }, { 7, 1818, 2048 }, { 8, 1819, 2048 }, { 7, 1820, 2048 }, { 8, 1821, 2048 }, { 8, 1822, 2048 }, { 9, 1823, 2048 }, 
+   { 5, 1824, 2048 }, { 6, 1825, 2048 }, { 6, 1826, 2048 }, { 7, 1827, 2048 }, { 6, 1828, 2048 }, { 7, 1829, 2048 }, { 7, 1830, 2048 }, { 8, 1831, 2048 }, 
+   { 6, 1832, 2048 }, { 7, 1833, 2048 }, { 7, 1834, 2048 }, { 8, 1835, 2048 }, { 7, 1836, 2048 }, { 8, 1837, 2048 }, { 8, 1838, 2048 }, { 9, 1839, 2048 }, 
+   { 6, 1840, 2048 }, { 7, 1841, 2048 }, { 7, 1842, 2048 }, { 8, 1843, 2048 }, { 7, 1844, 2048 }, { 8, 1845, 2048 }, { 8, 1846, 2048 }, { 9, 1847, 2048 }, 
+   { 7, 1848, 2048 }, { 8, 1849, 2048 }, { 8, 1850, 2048 }, { 9, 1851, 2048 }, { 8, 1852, 2048 }, { 9, 1853, 2048 }, { 9, 1854, 2048 }, { 10, 1855, 2048 }, 
+   { 5, 1856, 2048 }, { 6, 1857, 2048 }, { 6, 1858, 2048 }, { 7, 1859, 2048 }, { 6, 1860, 2048 }, { 7, 1861, 2048 }, { 7, 1862, 2048 }, { 8, 1863, 2048 }, 
+   { 6, 1864, 2048 }, { 7, 1865, 2048 }, { 7, 1866, 2048 }, { 8, 1867, 2048 }, { 7, 1868, 2048 }, { 8, 1869, 2048 }, { 8, 1870, 2048 }, { 9, 1871, 2048 }, 
+   { 6, 1872, 2048 }, { 7, 1873, 2048 }, { 7, 1874, 2048 }, { 8, 1875, 2048 }, { 7, 1876, 2048 }, { 8, 1877, 2048 }, { 8, 1878, 2048 }, { 9, 1879, 2048 }, 
+   { 7, 1880, 2048 }, { 8, 1881, 2048 }, { 8, 1882, 2048 }, { 9, 1883, 2048 }, { 8, 1884, 2048 }, { 9, 1885, 2048 }, { 9, 1886, 2048 }, { 10, 1887, 2048 }, 
+   { 6, 1888, 2048 }, { 7, 1889, 2048 }, { 7, 1890, 2048 }, { 8, 1891, 2048 }, { 7, 1892, 2048 }, { 8, 1893, 2048 }, { 8, 1894, 2048 }, { 9, 1895, 2048 }, 
+   { 7, 1896, 2048 }, { 8, 1897, 2048 }, { 8, 1898, 2048 }, { 9, 1899, 2048 }, { 8, 1900, 2048 }, { 9, 1901, 2048 }, { 9, 1902, 2048 }, { 10, 1903, 2048 }, 
+   { 7, 1904, 2048 }, { 8, 1905, 2048 }, { 8, 1906, 2048 }, { 9, 1907, 2048 }, { 8, 1908, 2048 }, { 9, 1909, 2048 }, { 9, 1910, 2048 }, { 10, 1911, 2048 }, 
+   { 8, 1912, 2048 }, { 9, 1913, 2048 }, { 9, 1914, 2048 }, { 10, 1915, 2048 }, { 9, 1916, 2048 }, { 10, 1917, 2048 }, { 10, 1918, 2048 }, { 11, 1919, 2048 }, 
+   { 5, 1920, 2048 }, { 6, 1921, 2048 }, { 6, 1922, 2048 }, { 7, 1923, 2048 }, { 6, 1924, 2048 }, { 7, 1925, 2048 }, { 7, 1926, 2048 }, { 8, 1927, 2048 }, 
+   { 6, 1928, 2048 }, { 7, 1929, 2048 }, { 7, 1930, 2048 }, { 8, 1931, 2048 }, { 7, 1932, 2048 }, { 8, 1933, 2048 }, { 8, 1934, 2048 }, { 9, 1935, 2048 }, 
+   { 6, 1936, 2048 }, { 7, 1937, 2048 }, { 7, 1938, 2048 }, { 8, 1939, 2048 }, { 7, 1940, 2048 }, { 8, 1941, 2048 }, { 8, 1942, 2048 }, { 9, 1943, 2048 }, 
+   { 7, 1944, 2048 }, { 8, 1945, 2048 }, { 8, 1946, 2048 }, { 9, 1947, 2048 }, { 8, 1948, 2048 }, { 9, 1949, 2048 }, { 9, 1950, 2048 }, { 10, 1951, 2048 }, 
+   { 6, 1952, 2048 }, { 7, 1953, 2048 }, { 7, 1954, 2048 }, { 8, 1955, 2048 }, { 7, 1956, 2048 }, { 8, 1957, 2048 }, { 8, 1958, 2048 }, { 9, 1959, 2048 }, 
+   { 7, 1960, 2048 }, { 8, 1961, 2048 }, { 8, 1962, 2048 }, { 9, 1963, 2048 }, { 8, 1964, 2048 }, { 9, 1965, 2048 }, { 9, 1966, 2048 }, { 10, 1967, 2048 }, 
+   { 7, 1968, 2048 }, { 8, 1969, 2048 }, { 8, 1970, 2048 }, { 9, 1971, 2048 }, { 8, 1972, 2048 }, { 9, 1973, 2048 }, { 9, 1974, 2048 }, { 10, 1975, 2048 }, 
+   { 8, 1976, 2048 }, { 9, 1977, 2048 }, { 9, 1978, 2048 }, { 10, 1979, 2048 }, { 9, 1980, 2048 }, { 10, 1981, 2048 }, { 10, 1982, 2048 }, { 11, 1983, 2048 }, 
+   { 6, 1984, 2048 }, { 7, 1985, 2048 }, { 7, 1986, 2048 }, { 8, 1987, 2048 }, { 7, 1988, 2048 }, { 8, 1989, 2048 }, { 8, 1990, 2048 }, { 9, 1991, 2048 }, 
+   { 7, 1992, 2048 }, { 8, 1993, 2048 }, { 8, 1994, 2048 }, { 9, 1995, 2048 }, { 8, 1996, 2048 }, { 9, 1997, 2048 }, { 9, 1998, 2048 }, { 10, 1999, 2048 }, 
+   { 7, 2000, 2048 }, { 8, 2001, 2048 }, { 8, 2002, 2048 }, { 9, 2003, 2048 }, { 8, 2004, 2048 }, { 9, 2005, 2048 }, { 9, 2006, 2048 }, { 10, 2007, 2048 }, 
+   { 8, 2008, 2048 }, { 9, 2009, 2048 }, { 9, 2010, 2048 }, { 10, 2011, 2048 }, { 9, 2012, 2048 }, { 10, 2013, 2048 }, { 10, 2014, 2048 }, { 11, 2015, 2048 }, 
+   { 7, 2016, 2048 }, { 8, 2017, 2048 }, { 8, 2018, 2048 }, { 9, 2019, 2048 }, { 8, 2020, 2048 }, { 9, 2021, 2048 }, { 9, 2022, 2048 }, { 10, 2023, 2048 }, 
+   { 8, 2024, 2048 }, { 9, 2025, 2048 }, { 9, 2026, 2048 }, { 10, 2027, 2048 }, { 9, 2028, 2048 }, { 10, 2029, 2048 }, { 10, 2030, 2048 }, { 11, 2031, 2048 }, 
+   { 8, 2032, 2048 }, { 9, 2033, 2048 }, { 9, 2034, 2048 }, { 10, 2035, 2048 }, { 9, 2036, 2048 }, { 10, 2037, 2048 }, { 10, 2038, 2048 }, { 11, 2039, 2048 }, 
+   { 9, 2040, 2048 }, { 10, 2041, 2048 }, { 10, 2042, 2048 }, { 11, 2043, 2048 }, { 10, 2044, 2048 }, { 11, 2045, 2048 }, { 11, 2046, 2048 }, { 12, 2047, 2048 }, 
+#endif
+#endif
+#endif
+#endif
+#endif
+#endif
+};
+
+/* find a hole and free as required, return -1 if no hole found */
+static int find_hole(void)
+{
+   unsigned x;
+   int      y, z;
+   for (z = -1, y = INT_MAX, x = 0; x < FP_ENTRIES; x++) {
+       if (fp_cache[x].lru_count < y && fp_cache[x].lock == 0) {
+          z = x;
+          y = fp_cache[x].lru_count;
+       }
+   }
+
+   /* decrease all */
+   for (x = 0; x < FP_ENTRIES; x++) {
+      if (fp_cache[x].lru_count > 3) {
+         --(fp_cache[x].lru_count);
+      }
+   }
+
+   /* free entry z */
+   if (z >= 0 && fp_cache[z].g) {
+      if (fp_cache[z].mu != NULL) {
+         mp_clear(fp_cache[z].mu);
+         fp_cache[z].mu = NULL;
+      }
+      ltc_ecc_del_point(fp_cache[z].g);
+      fp_cache[z].g  = NULL;
+      for (x = 0; x < (1U<<FP_LUT); x++) {
+         ltc_ecc_del_point(fp_cache[z].LUT[x]);
+         fp_cache[z].LUT[x] = NULL;
+      }
+      fp_cache[z].lru_count = 0;
+   }
+   return z;
+}
+
+/* determine if a base is already in the cache and if so, where */
+static int find_base(ecc_point *g)
+{
+   int x;
+   for (x = 0; x < FP_ENTRIES; x++) {
+      if (fp_cache[x].g != NULL && 
+          mp_cmp(fp_cache[x].g->x, g->x) == LTC_MP_EQ && 
+          mp_cmp(fp_cache[x].g->y, g->y) == LTC_MP_EQ && 
+          mp_cmp(fp_cache[x].g->z, g->z) == LTC_MP_EQ) {
+         break;
+      }
+   }
+   if (x == FP_ENTRIES) {
+      x = -1;
+   }
+   return x;
+}
+
+/* add a new base to the cache */
+static int add_entry(int idx, ecc_point *g)
+{
+   unsigned x, y;
+
+   /* allocate base and LUT */
+   fp_cache[idx].g = ltc_ecc_new_point();
+   if (fp_cache[idx].g == NULL) {
+      return CRYPT_MEM;
+   }
+
+   /* copy x and y */
+   if ((mp_copy(g->x, fp_cache[idx].g->x) != CRYPT_OK) ||
+       (mp_copy(g->y, fp_cache[idx].g->y) != CRYPT_OK) ||
+       (mp_copy(g->z, fp_cache[idx].g->z) != CRYPT_OK)) {
+      ltc_ecc_del_point(fp_cache[idx].g);
+      fp_cache[idx].g = NULL;
+      return CRYPT_MEM;
+   }              
+
+   for (x = 0; x < (1U<<FP_LUT); x++) {
+      fp_cache[idx].LUT[x] = ltc_ecc_new_point();
+      if (fp_cache[idx].LUT[x] == NULL) {
+         for (y = 0; y < x; y++) {
+            ltc_ecc_del_point(fp_cache[idx].LUT[y]);
+            fp_cache[idx].LUT[y] = NULL;
+         }
+         ltc_ecc_del_point(fp_cache[idx].g);
+         fp_cache[idx].g         = NULL;
+         fp_cache[idx].lru_count = 0;
+         return CRYPT_MEM;
+      }
+   }
+   
+   fp_cache[idx].lru_count = 0;
+   return CRYPT_OK;
+}
+
+/* build the LUT by spacing the bits of the input by #modulus/FP_LUT bits apart 
+ * 
+ * The algorithm builds patterns in increasing bit order by first making all 
+ * single bit input patterns, then all two bit input patterns and so on
+ */
+static int build_lut(int idx, void *modulus, void *mp, void *mu)
+{ 
+   unsigned x, y, err, bitlen, lut_gap;
+   void    *tmp;
+
+   tmp = NULL;
+
+   /* sanity check to make sure lut_order table is of correct size, should compile out to a NOP if true */
+   if ((sizeof(lut_orders) / sizeof(lut_orders[0])) < (1U<<FP_LUT)) {
+       err = CRYPT_INVALID_ARG;
+       goto DONE;
+   }       
+
+   /* get bitlen and round up to next multiple of FP_LUT */
+   bitlen  = mp_unsigned_bin_size(modulus) << 3;
+   x       = bitlen % FP_LUT;
+   if (x) {
+      bitlen += FP_LUT - x;
+   }  
+   lut_gap = bitlen / FP_LUT;
+
+   /* init the mu */
+   if ((err = mp_init_copy(&fp_cache[idx].mu, mu)) != CRYPT_OK) {
+      goto ERR;
+   }
+   
+   /* copy base */
+   if ((mp_mulmod(fp_cache[idx].g->x, mu, modulus, fp_cache[idx].LUT[1]->x) != CRYPT_OK) || 
+       (mp_mulmod(fp_cache[idx].g->y, mu, modulus, fp_cache[idx].LUT[1]->y) != CRYPT_OK) || 
+       (mp_mulmod(fp_cache[idx].g->z, mu, modulus, fp_cache[idx].LUT[1]->z) != CRYPT_OK))        { goto ERR; }
+       
+   /* make all single bit entries */
+   for (x = 1; x < FP_LUT; x++) {
+      if ((mp_copy(fp_cache[idx].LUT[1<<(x-1)]->x, fp_cache[idx].LUT[1<<x]->x) != CRYPT_OK) || 
+          (mp_copy(fp_cache[idx].LUT[1<<(x-1)]->y, fp_cache[idx].LUT[1<<x]->y) != CRYPT_OK) || 
+          (mp_copy(fp_cache[idx].LUT[1<<(x-1)]->z, fp_cache[idx].LUT[1<<x]->z) != CRYPT_OK))     { goto ERR; }
+          
+      /* now double it bitlen/FP_LUT times */
+      for (y = 0; y < lut_gap; y++) {
+          if ((err = ltc_mp.ecc_ptdbl(fp_cache[idx].LUT[1<<x], fp_cache[idx].LUT[1<<x], modulus, mp)) != CRYPT_OK) {
+             goto ERR;
+          }
+      }          
+   }
+      
+   /* now make all entries in increase order of hamming weight */
+   for (x = 2; x <= FP_LUT; x++) {
+       for (y = 0; y < (1UL<<FP_LUT); y++) {
+           if (lut_orders[y].ham != (int)x) continue;
+                     
+           /* perform the add */
+           if ((err = ltc_mp.ecc_ptadd(fp_cache[idx].LUT[lut_orders[y].terma], fp_cache[idx].LUT[lut_orders[y].termb], 
+                                       fp_cache[idx].LUT[y], modulus, mp)) != CRYPT_OK) {
+              goto ERR;
+           }
+       }
+   }
+      
+   /* now map all entries back to affine space to make point addition faster */
+   if ((err = mp_init(&tmp)) != CRYPT_OK)                                                                    { goto ERR; }
+   for (x = 1; x < (1UL<<FP_LUT); x++) {
+       /* convert z to normal from montgomery */
+       if ((err = mp_montgomery_reduce(fp_cache[idx].LUT[x]->z, modulus, mp)) != CRYPT_OK)                   { goto ERR; }
+ 
+       /* invert it */
+       if ((err = mp_invmod(fp_cache[idx].LUT[x]->z, modulus, fp_cache[idx].LUT[x]->z)) != CRYPT_OK)         { goto ERR; }
+
+       /* now square it */
+       if ((err = mp_sqrmod(fp_cache[idx].LUT[x]->z, modulus, tmp)) != CRYPT_OK)                             { goto ERR; }
+       
+       /* fix x */
+       if ((err = mp_mulmod(fp_cache[idx].LUT[x]->x, tmp, modulus, fp_cache[idx].LUT[x]->x)) != CRYPT_OK)    { goto ERR; }
+
+       /* get 1/z^3 */
+       if ((err = mp_mulmod(tmp, fp_cache[idx].LUT[x]->z, modulus, tmp)) != CRYPT_OK)                        { goto ERR; }
+
+       /* fix y */
+       if ((err = mp_mulmod(fp_cache[idx].LUT[x]->y, tmp, modulus, fp_cache[idx].LUT[x]->y)) != CRYPT_OK)    { goto ERR; }
+
+       /* free z */
+       mp_clear(fp_cache[idx].LUT[x]->z);
+       fp_cache[idx].LUT[x]->z = NULL;
+   }
+   mp_clear(tmp);
+
+   return CRYPT_OK;                                                                       
+ERR:
+   err = CRYPT_MEM;
+DONE:   
+   for (y = 0; y < (1U<<FP_LUT); y++) {
+      ltc_ecc_del_point(fp_cache[idx].LUT[y]);
+      fp_cache[idx].LUT[y] = NULL;
+   }
+   ltc_ecc_del_point(fp_cache[idx].g);
+   fp_cache[idx].g         = NULL;
+   fp_cache[idx].lru_count = 0;
+   if (fp_cache[idx].mu != NULL) {
+      mp_clear(fp_cache[idx].mu);
+      fp_cache[idx].mu = NULL;
+   }
+   if (tmp != NULL) {
+      mp_clear(tmp);
+   }
+   return err;
+}
+
+/* perform a fixed point ECC mulmod */
+static int accel_fp_mul(int idx, void *k, ecc_point *R, void *modulus, void *mp, int map)
+{
+   unsigned char kb[128];
+   int      x;
+   unsigned y, z, err, bitlen, bitpos, lut_gap, first;
+   void     *tk, *order;
+
+   /* if it's smaller than modulus we fine */
+   if (mp_unsigned_bin_size(k) > mp_unsigned_bin_size(modulus)) {
+      /* find order */
+      y = mp_unsigned_bin_size(modulus);
+      for (x = 0; ltc_ecc_sets[x].size; x++) {
+         if (y <= (unsigned)ltc_ecc_sets[x].size) break;
+      }
+   
+      /* back off if we are on the 521 bit curve */
+      if (y == 66) --x;
+      
+      if ((err = mp_init(&order)) != CRYPT_OK) {
+         return err;
+      }      
+      if ((err = mp_read_radix(order, ltc_ecc_sets[x].order, 16)) != CRYPT_OK) {
+         mp_clear(&order);
+         return err;
+      }
+
+      /* k must be less than modulus */
+      if (mp_cmp(k, order) != LTC_MP_LT) {
+         if ((err = mp_init(&tk)) != CRYPT_OK) {
+            mp_clear(order);
+            return err;
+         }
+         if ((err = mp_mod(k, order, tk)) != CRYPT_OK) {
+            mp_clear(tk);
+            mp_clear(order);
+            return err;
+         }
+      } else {
+         tk = k;
+      }
+      mp_clear(order);
+   } else {
+       tk = k;
+   }       
+   
+   /* get bitlen and round up to next multiple of FP_LUT */
+   bitlen  = mp_unsigned_bin_size(modulus) << 3;
+   x       = bitlen % FP_LUT;
+   if (x) {
+      bitlen += FP_LUT - x;
+   }  
+   lut_gap = bitlen / FP_LUT;
+        
+   /* get the k value */
+   if (mp_unsigned_bin_size(tk) > (sizeof(kb) - 2)) {
+      if (tk != k) {
+         mp_clear(tk);
+      }         
+      return CRYPT_BUFFER_OVERFLOW;
+   }
+   
+   /* store k */
+   zeromem(kb, sizeof(kb));
+   if ((err = mp_to_unsigned_bin(tk, kb)) != CRYPT_OK) {
+      if (tk != k) {
+         mp_clear(tk);
+      }         
+      return err;
+   }
+   
+   /* let's reverse kb so it's little endian */
+   x = 0;
+   y = mp_unsigned_bin_size(tk) - 1;
+   if (tk != k) {
+      mp_clear(tk);
+   }         
+   while ((unsigned)x < y) {
+      z = kb[x]; kb[x] = kb[y]; kb[y] = z;
+      ++x; --y;
+   }      
+   
+   /* at this point we can start, yipee */
+   first = 1;
+   for (x = lut_gap-1; x >= 0; x--) {
+       /* extract FP_LUT bits from kb spread out by lut_gap bits and offset by x bits from the start */
+       bitpos = x;
+       for (y = z = 0; y < FP_LUT; y++) {
+          z |= ((kb[bitpos>>3] >> (bitpos&7)) & 1) << y;
+          bitpos += lut_gap;                               /* it's y*lut_gap + x, but here we can avoid the mult in each loop */
+       }
+              
+       /* double if not first */
+       if (!first) {
+          if ((err = ltc_mp.ecc_ptdbl(R, R, modulus, mp)) != CRYPT_OK) {
+             return err;
+          }
+       }
+       
+       /* add if not first, otherwise copy */          
+       if (!first && z) {
+          if ((err = ltc_mp.ecc_ptadd(R, fp_cache[idx].LUT[z], R, modulus, mp)) != CRYPT_OK) {
+             return err;
+          }
+       } else if (z) {
+          if ((mp_copy(fp_cache[idx].LUT[z]->x, R->x) != CRYPT_OK) || 
+              (mp_copy(fp_cache[idx].LUT[z]->y, R->y) != CRYPT_OK) || 
+              (mp_copy(fp_cache[idx].mu,        R->z) != CRYPT_OK)) { return CRYPT_MEM; }
+              first = 0;              
+       }
+   }     
+   z = 0;
+   zeromem(kb, sizeof(kb));
+   /* map R back from projective space */
+   if (map) {
+      err = ltc_ecc_map(R, modulus, mp);
+   } else {
+      err = CRYPT_OK;
+   }
+   return err;
+}
+
+#ifdef LTC_ECC_SHAMIR
+/* perform a fixed point ECC mulmod */
+static int accel_fp_mul2add(int idx1, int idx2, 
+                            void *kA, void *kB,
+                            ecc_point *R, void *modulus, void *mp)
+{
+   unsigned char kb[2][128];
+   int      x;
+   unsigned y, z, err, bitlen, bitpos, lut_gap, first, zA, zB;
+   void     *tka, *tkb, *order;
+
+   /* if it's smaller than modulus we fine */
+   if (mp_unsigned_bin_size(kA) > mp_unsigned_bin_size(modulus)) {
+      /* find order */
+      y = mp_unsigned_bin_size(modulus);
+      for (x = 0; ltc_ecc_sets[x].size; x++) {
+         if (y <= (unsigned)ltc_ecc_sets[x].size) break;
+      }
+   
+      /* back off if we are on the 521 bit curve */
+      if (y == 66) --x;
+      
+      if ((err = mp_init(&order)) != CRYPT_OK) {
+         return err;
+      }      
+      if ((err = mp_read_radix(order, ltc_ecc_sets[x].order, 16)) != CRYPT_OK) {
+         mp_clear(&order);
+         return err;
+      }
+
+      /* kA must be less than modulus */
+      if (mp_cmp(kA, order) != LTC_MP_LT) {
+         if ((err = mp_init(&tka)) != CRYPT_OK) {
+            mp_clear(order);
+            return err;
+         }
+         if ((err = mp_mod(kA, order, tka)) != CRYPT_OK) {
+            mp_clear(tka);
+            mp_clear(order);
+            return err;
+         }
+      } else {
+         tka = kA;
+      }
+      mp_clear(order);
+   } else {
+      tka = kA;
+   }       
+
+   /* if it's smaller than modulus we fine */
+   if (mp_unsigned_bin_size(kB) > mp_unsigned_bin_size(modulus)) {
+      /* find order */
+      y = mp_unsigned_bin_size(modulus);
+      for (x = 0; ltc_ecc_sets[x].size; x++) {
+         if (y <= (unsigned)ltc_ecc_sets[x].size) break;
+      }
+   
+      /* back off if we are on the 521 bit curve */
+      if (y == 66) --x;
+      
+      if ((err = mp_init(&order)) != CRYPT_OK) {
+         return err;
+      }      
+      if ((err = mp_read_radix(order, ltc_ecc_sets[x].order, 16)) != CRYPT_OK) {
+         mp_clear(&order);
+         return err;
+      }
+
+      /* kB must be less than modulus */
+      if (mp_cmp(kB, order) != LTC_MP_LT) {
+         if ((err = mp_init(&tkb)) != CRYPT_OK) {
+            mp_clear(order);
+            return err;
+         }
+         if ((err = mp_mod(kB, order, tkb)) != CRYPT_OK) {
+            mp_clear(tkb);
+            mp_clear(order);
+            return err;
+         }
+      } else {
+         tkb = kB;
+      }
+      mp_clear(order);
+   } else {
+      tkb = kB;
+   }     
+
+   /* get bitlen and round up to next multiple of FP_LUT */
+   bitlen  = mp_unsigned_bin_size(modulus) << 3;
+   x       = bitlen % FP_LUT;
+   if (x) {
+      bitlen += FP_LUT - x;
+   }  
+   lut_gap = bitlen / FP_LUT;
+        
+   /* get the k value */
+   if ((mp_unsigned_bin_size(tka) > (sizeof(kb[0]) - 2)) || (mp_unsigned_bin_size(tkb) > (sizeof(kb[0]) - 2))  ) {
+      if (tka != kA) {
+         mp_clear(tka);
+      }         
+      if (tkb != kB) {
+         mp_clear(tkb);
+      }         
+      return CRYPT_BUFFER_OVERFLOW;
+   }
+   
+   /* store k */
+   zeromem(kb, sizeof(kb));
+   if ((err = mp_to_unsigned_bin(tka, kb[0])) != CRYPT_OK) {
+      if (tka != kA) {
+         mp_clear(tka);
+      }         
+      if (tkb != kB) {
+         mp_clear(tkb);
+      }         
+      return err;
+   }
+   
+   /* let's reverse kb so it's little endian */
+   x = 0;
+   y = mp_unsigned_bin_size(tka) - 1;
+   if (tka != kA) {
+      mp_clear(tka);
+   }         
+   while ((unsigned)x < y) {
+      z = kb[0][x]; kb[0][x] = kb[0][y]; kb[0][y] = z;
+      ++x; --y;
+   }      
+   
+   /* store b */
+   if ((err = mp_to_unsigned_bin(tkb, kb[1])) != CRYPT_OK) {
+      if (tkb != kB) {
+         mp_clear(tkb);
+      }         
+      return err;
+   }
+
+   x = 0;
+   y = mp_unsigned_bin_size(tkb) - 1;
+   if (tkb != kB) {
+      mp_clear(tkb);
+   }         
+   while ((unsigned)x < y) {
+      z = kb[1][x]; kb[1][x] = kb[1][y]; kb[1][y] = z;
+      ++x; --y;
+   }      
+
+   /* at this point we can start, yipee */
+   first = 1;
+   for (x = lut_gap-1; x >= 0; x--) {
+       /* extract FP_LUT bits from kb spread out by lut_gap bits and offset by x bits from the start */
+       bitpos = x;
+       for (y = zA = zB = 0; y < FP_LUT; y++) {
+          zA |= ((kb[0][bitpos>>3] >> (bitpos&7)) & 1) << y;
+          zB |= ((kb[1][bitpos>>3] >> (bitpos&7)) & 1) << y;
+          bitpos += lut_gap;                               /* it's y*lut_gap + x, but here we can avoid the mult in each loop */
+       }
+              
+       /* double if not first */
+       if (!first) {
+          if ((err = ltc_mp.ecc_ptdbl(R, R, modulus, mp)) != CRYPT_OK) {
+             return err;
+          }
+       }
+       
+       /* add if not first, otherwise copy */          
+       if (!first) {
+          if (zA) {
+             if ((err = ltc_mp.ecc_ptadd(R, fp_cache[idx1].LUT[zA], R, modulus, mp)) != CRYPT_OK) {
+                return err;
+             }
+          }
+          if (zB) {
+             if ((err = ltc_mp.ecc_ptadd(R, fp_cache[idx2].LUT[zB], R, modulus, mp)) != CRYPT_OK) {
+                return err;
+             }
+          }
+       } else {
+          if (zA) {
+              if ((mp_copy(fp_cache[idx1].LUT[zA]->x, R->x) != CRYPT_OK) || 
+                 (mp_copy(fp_cache[idx1].LUT[zA]->y, R->y) != CRYPT_OK) || 
+                 (mp_copy(fp_cache[idx1].mu,        R->z) != CRYPT_OK)) { return CRYPT_MEM; }
+                 first = 0;              
+          }
+          if (zB && first == 0) {
+             if (zB) {
+                if ((err = ltc_mp.ecc_ptadd(R, fp_cache[idx2].LUT[zB], R, modulus, mp)) != CRYPT_OK) {
+                   return err;
+                }
+             }
+          } else if (zB && first == 1) {
+              if ((mp_copy(fp_cache[idx2].LUT[zB]->x, R->x) != CRYPT_OK) || 
+                 (mp_copy(fp_cache[idx2].LUT[zB]->y, R->y) != CRYPT_OK) || 
+                 (mp_copy(fp_cache[idx2].mu,        R->z) != CRYPT_OK)) { return CRYPT_MEM; }
+                 first = 0;              
+          }
+       }
+   }     
+   zeromem(kb, sizeof(kb));
+   return ltc_ecc_map(R, modulus, mp);
+}
+
+/** ECC Fixed Point mulmod global
+  Computes kA*A + kB*B = C using Shamir's Trick
+  @param A        First point to multiply
+  @param kA       What to multiple A by
+  @param B        Second point to multiply
+  @param kB       What to multiple B by
+  @param C        [out] Destination point (can overlap with A or B)
+  @param modulus  Modulus for curve 
+  @return CRYPT_OK on success
+*/ 
+int ltc_ecc_fp_mul2add(ecc_point *A, void *kA,
+                       ecc_point *B, void *kB,
+                       ecc_point *C, void *modulus)
+{
+   int  idx1, idx2, err;
+   void *mp, *mu;
+   
+   mp = NULL;
+   mu = NULL;
+   LTC_MUTEX_LOCK(&ltc_ecc_fp_lock);
+      /* find point */
+      idx1 = find_base(A);
+
+      /* no entry? */
+      if (idx1 == -1) {
+         /* find hole and add it */
+         if ((idx1 = find_hole()) >= 0) {
+            if ((err = add_entry(idx1, A)) != CRYPT_OK) {
+               goto LBL_ERR;
+            }
+         }
+      }
+      if (idx1 != -1) {
+         /* increment LRU */
+         ++(fp_cache[idx1].lru_count);
+      }
+
+      /* find point */
+      idx2 = find_base(B);
+
+      /* no entry? */
+      if (idx2 == -1) {
+         /* find hole and add it */
+         if ((idx2 = find_hole()) >= 0) {
+            if ((err = add_entry(idx2, B)) != CRYPT_OK) {
+               goto LBL_ERR;
+            }
+         }
+      }
+      if (idx2 != -1) {
+         /* increment LRU */
+         ++(fp_cache[idx2].lru_count);
+      }
+
+      /* if it's 2 build the LUT, if it's higher just use the LUT */
+      if (idx1 >= 0 && fp_cache[idx1].lru_count == 2) {
+         /* compute mp */
+         if ((err = mp_montgomery_setup(modulus, &mp)) != CRYPT_OK) { goto LBL_ERR; }
+
+         /* compute mu */
+         if ((err = mp_init(&mu)) != CRYPT_OK) {
+             goto LBL_ERR;
+         }
+         if ((err = mp_montgomery_normalization(mu, modulus)) != CRYPT_OK) {
+            goto LBL_ERR;
+         }            
+                 
+         /* build the LUT */
+         if ((err = build_lut(idx1, modulus, mp, mu)) != CRYPT_OK) {
+             goto LBL_ERR;;
+         }  
+      }
+
+      /* if it's 2 build the LUT, if it's higher just use the LUT */
+      if (idx2 >= 0 && fp_cache[idx2].lru_count == 2) {
+         if (mp == NULL) {
+            /* compute mp */
+            if ((err = mp_montgomery_setup(modulus, &mp)) != CRYPT_OK) { goto LBL_ERR; }
+
+            /* compute mu */
+            if ((err = mp_init(&mu)) != CRYPT_OK) {
+                goto LBL_ERR;
+            }
+            if ((err = mp_montgomery_normalization(mu, modulus)) != CRYPT_OK) {
+               goto LBL_ERR;
+            }            
+         }
+                 
+         /* build the LUT */
+         if ((err = build_lut(idx2, modulus, mp, mu)) != CRYPT_OK) {
+             goto LBL_ERR;;
+         }  
+      }
+
+
+      if (idx1 >=0 && idx2 >= 0 && fp_cache[idx1].lru_count >= 2 && fp_cache[idx2].lru_count >= 2) {
+         if (mp == NULL) {
+            /* compute mp */
+            if ((err = mp_montgomery_setup(modulus, &mp)) != CRYPT_OK) { goto LBL_ERR; }
+         }
+         err = accel_fp_mul2add(idx1, idx2, kA, kB, C, modulus, mp);
+      } else {
+         err = ltc_ecc_mul2add(A, kA, B, kB, C, modulus);
+      }
+LBL_ERR:
+    LTC_MUTEX_UNLOCK(&ltc_ecc_fp_lock);
+    if (mp != NULL) {
+       mp_montgomery_free(mp);
+    }       
+    if (mu != NULL) {
+       mp_clear(mu);
+    }       
+    return err;
+}
+#endif
+
+/** ECC Fixed Point mulmod global
+    @param k        The multiplicand
+    @param G        Base point to multiply
+    @param R        [out] Destination of product
+    @param modulus  The modulus for the curve
+    @param map      [boolean] If non-zero maps the point back to affine co-ordinates, otherwise it's left in jacobian-montgomery form
+    @return CRYPT_OK if successful
+*/   
+int ltc_ecc_fp_mulmod(void *k, ecc_point *G, ecc_point *R, void *modulus, int map)
+{
+   int   idx, err;
+   void *mp, *mu;
+   
+   mp = NULL;
+   mu = NULL;
+   LTC_MUTEX_LOCK(&ltc_ecc_fp_lock);
+      /* find point */
+      idx = find_base(G);
+
+      /* no entry? */
+      if (idx == -1) {
+         /* find hole and add it */
+         idx = find_hole();
+
+         if (idx >= 0) {
+            if ((err = add_entry(idx, G)) != CRYPT_OK) {
+               goto LBL_ERR;
+            }
+         }
+      }
+      if (idx != -1) {
+         /* increment LRU */
+         ++(fp_cache[idx].lru_count);
+      }
+
+ 
+      /* if it's 2 build the LUT, if it's higher just use the LUT */
+      if (idx >= 0 && fp_cache[idx].lru_count == 2) {
+         /* compute mp */
+         if ((err = mp_montgomery_setup(modulus, &mp)) != CRYPT_OK) { goto LBL_ERR; }
+
+         /* compute mu */
+         if ((err = mp_init(&mu)) != CRYPT_OK) {
+             goto LBL_ERR;
+         }
+         if ((err = mp_montgomery_normalization(mu, modulus)) != CRYPT_OK) {
+            goto LBL_ERR;
+         }            
+                 
+         /* build the LUT */
+         if ((err = build_lut(idx, modulus, mp, mu)) != CRYPT_OK) {
+             goto LBL_ERR;;
+         }  
+      }
+
+      if (idx >= 0 && fp_cache[idx].lru_count >= 2) {
+         if (mp == NULL) {
+            /* compute mp */
+            if ((err = mp_montgomery_setup(modulus, &mp)) != CRYPT_OK) { goto LBL_ERR; }
+         }
+         err = accel_fp_mul(idx, k, R, modulus, mp, map);
+      } else {
+         err = ltc_ecc_mulmod(k, G, R, modulus, map);
+      }
+LBL_ERR:
+    LTC_MUTEX_UNLOCK(&ltc_ecc_fp_lock);
+    if (mp != NULL) {
+       mp_montgomery_free(mp);
+    }       
+    if (mu != NULL) {
+       mp_clear(mu);
+    }       
+    return err;
+}
+
+/* helper function for freeing the cache ... must be called with the cache mutex locked */
+static void ltc_ecc_fp_free_cache(void)
+{
+   unsigned x, y;
+   for (x = 0; x < FP_ENTRIES; x++) {
+      if (fp_cache[x].g != NULL) {
+         for (y = 0; y < (1U<<FP_LUT); y++) {
+            ltc_ecc_del_point(fp_cache[x].LUT[y]);
+            fp_cache[x].LUT[y] = NULL;
+         }
+         ltc_ecc_del_point(fp_cache[x].g);
+         fp_cache[x].g         = NULL;
+         if (fp_cache[x].mu != NULL) {
+            mp_clear(fp_cache[x].mu);
+            fp_cache[x].mu     = NULL;
+         }
+         fp_cache[x].lru_count = 0;
+         fp_cache[x].lock = 0;
+      }         
+   }
+}         
+
+/** Free the Fixed Point cache */
+void ltc_ecc_fp_free(void)
+{
+   LTC_MUTEX_LOCK(&ltc_ecc_fp_lock);
+   ltc_ecc_fp_free_cache();
+   LTC_MUTEX_UNLOCK(&ltc_ecc_fp_lock);
+}
+
+/** Add a point to the cache and initialize the LUT
+  @param g        The point to add
+  @param modulus  Modulus for curve 
+  @param lock     Flag to indicate if this entry should be locked into the cache or not
+  @return CRYPT_OK on success
+*/
+int
+ltc_ecc_fp_add_point(ecc_point *g, void *modulus, int lock)
+{
+   int idx;
+   int err;
+   void *mp = NULL;
+   void *mu = NULL;
+
+   LTC_MUTEX_LOCK(&ltc_ecc_fp_lock);
+   if ((idx = find_base(g)) >= 0) {
+      /* it is already in the cache ... just check that the LUT is initialized */
+      if(fp_cache[idx].lru_count >= 2) {
+         LTC_MUTEX_UNLOCK(&ltc_ecc_fp_lock);
+         return CRYPT_OK;
+      }
+   }
+
+   if(idx == -1 && (idx = find_hole()) == -1) {
+      err = CRYPT_BUFFER_OVERFLOW;
+      goto LBL_ERR;
+   }
+   if ((err = add_entry(idx, g)) != CRYPT_OK) {
+      goto LBL_ERR;
+   }
+   /* compute mp */
+   if ((err = mp_montgomery_setup(modulus, &mp)) != CRYPT_OK) {
+      goto LBL_ERR;
+   }
+
+   /* compute mu */
+   if ((err = mp_init(&mu)) != CRYPT_OK) {
+       goto LBL_ERR;
+   }
+   if ((err = mp_montgomery_normalization(mu, modulus)) != CRYPT_OK) {
+      goto LBL_ERR;
+   }            
+	   
+   /* build the LUT */
+   if ((err = build_lut(idx, modulus, mp, mu)) != CRYPT_OK) {
+       goto LBL_ERR;
+   }  
+   fp_cache[idx].lru_count = 2;
+   fp_cache[idx].lock = lock;
+LBL_ERR:
+   LTC_MUTEX_UNLOCK(&ltc_ecc_fp_lock);
+   if (mp != NULL) {
+      mp_montgomery_free(mp);
+   }       
+   if (mu != NULL) {
+      mp_clear(mu);
+   }       
+   return err;
+}
+
+/** Prevent/permit the FP cache from being updated 
+    @param flag        If flag is 0, remove cache lock (unlock), otherwise lock it
+*/
+void ltc_ecc_fp_tablelock(int lock)
+{
+   int i;
+
+   LTC_MUTEX_LOCK(&ltc_ecc_fp_lock);
+   for (i = 0; i < FP_ENTRIES; i++) {
+      fp_cache[i].lock = lock;
+   }
+   LTC_MUTEX_UNLOCK(&ltc_ecc_fp_lock);
+}
+
+/** Export the current cache as a binary packet
+    @param out      [out] pointer to malloc'ed space containing the packet
+    @param outlen   [out] size of exported packet
+    @return CRYPT_OK if successful
+*/
+int ltc_ecc_fp_save_state(unsigned char **out, unsigned long *outlen)
+{
+   ltc_asn1_list *cache_entry;
+   unsigned int   i, j, k;
+   unsigned long  fp_entries, fp_lut, num_entries;
+   int            err;
+
+   LTC_ARGCHK(out    != NULL);
+   LTC_ARGCHK(outlen != NULL);
+
+   fp_entries  = FP_ENTRIES;
+   fp_lut      = FP_LUT;
+   num_entries = 0;
+
+   LTC_MUTEX_LOCK(&ltc_ecc_fp_lock);
+   /*
+    * build the list; 
+      Cache DEFINITIONS ::=
+      BEGIN
+       CacheDump ::= SEQUENCE {
+         numEntries    SHORTINTEGER,
+         maxEntries    SHORTINTEGER,
+         numLUT        SHORTINTEGER,
+         cache         SEQUENCE OF INTEGER
+       }
+      END
+    *      
+    */
+   /*
+    * The cache itself is a point (3 INTEGERS), 
+	* the LUT as pairs of INTEGERS (2 * 1<<FP_LUT), 
+	* and the mu INTEGER
+    */
+   cache_entry = XCALLOC(FP_ENTRIES*(2*(1U<<FP_LUT)+4)+3, sizeof(ltc_asn1_list));
+   if (cache_entry == NULL)
+      return CRYPT_MEM;
+   j = 1;   /* handle the zero'th element later */
+
+   LTC_SET_ASN1(cache_entry, j++, LTC_ASN1_SHORT_INTEGER, &fp_entries, 1);
+   LTC_SET_ASN1(cache_entry, j++, LTC_ASN1_SHORT_INTEGER, &fp_lut, 1);
+
+   for (i = 0; i < FP_ENTRIES; i++) {
+      /*
+       * do not save empty entries, or entries that have not yet had the lut built
+       */
+      if (fp_cache[i].g == NULL || fp_cache[i].lru_count < 2) {
+         continue;
+      }
+      num_entries++;
+      LTC_SET_ASN1(cache_entry, j++, LTC_ASN1_INTEGER, fp_cache[i].g->x, 1);
+      LTC_SET_ASN1(cache_entry, j++, LTC_ASN1_INTEGER, fp_cache[i].g->y, 1);
+      LTC_SET_ASN1(cache_entry, j++, LTC_ASN1_INTEGER, fp_cache[i].g->z, 1);
+      for (k = 0; k < (1U<<FP_LUT); k++) {
+         LTC_SET_ASN1(cache_entry, j++, LTC_ASN1_INTEGER, fp_cache[i].LUT[k]->x, 1);
+         LTC_SET_ASN1(cache_entry, j++, LTC_ASN1_INTEGER, fp_cache[i].LUT[k]->y, 1);
+      }
+      LTC_SET_ASN1(cache_entry, j++, LTC_ASN1_INTEGER, fp_cache[i].mu, 1);
+   }
+   LTC_SET_ASN1(cache_entry, j++, LTC_ASN1_EOL, 0, 0);
+
+   LTC_SET_ASN1(cache_entry, 0, LTC_ASN1_SHORT_INTEGER, &num_entries, 1);
+
+   if ((err = der_length_sequence(cache_entry, j, outlen)) != CRYPT_OK) {
+      goto save_err;
+   }
+   if ((*out = XMALLOC(*outlen)) == NULL) {
+      err = CRYPT_MEM;
+      goto save_err;
+   }
+   err = der_encode_sequence(cache_entry, j, *out, outlen);
+save_err:
+   XFREE(cache_entry);
+   LTC_MUTEX_UNLOCK(&ltc_ecc_fp_lock);
+   return err;
+}
+
+/** Import a binary packet into the current cache
+    @param in      [in] pointer to packet
+    @param inlen   [in] size of packet (bytes)
+    @return CRYPT_OK if successful
+*/
+int ltc_ecc_fp_restore_state(unsigned char *in, unsigned long inlen)
+{
+   int            err;
+   ltc_asn1_list *asn1_list;
+   unsigned long  num_entries, fp_entries, fp_lut;
+   unsigned long  i, j;
+   unsigned int   x;
+
+   LTC_ARGCHK(in != NULL);
+   if (inlen == 0) {
+      return CRYPT_INVALID_ARG;
+   } 
+
+   /* zero indecies */
+   i         = 0;
+   j         = 0;
+   asn1_list = NULL;
+
+   LTC_MUTEX_LOCK(&ltc_ecc_fp_lock);
+   /*
+    * start with an empty cache
+    */
+   ltc_ecc_fp_free_cache();
+
+   /*
+    * decode the input packet: It consists of a sequence with a few
+    * integers (including the FP_ENTRIES and FP_LUT sizes), followed by a
+    * SEQUENCE which is the cache itself.
+	*
+	* use standard decoding for the first part, then flexible for the second
+    */
+   if((err = der_decode_sequence_multi(in, inlen, 
+                                       LTC_ASN1_SHORT_INTEGER, 1, &num_entries,
+                                       LTC_ASN1_SHORT_INTEGER, 1, &fp_entries,
+                                       LTC_ASN1_SHORT_INTEGER, 1, &fp_lut,
+                                       LTC_ASN1_EOL,           0, 0)) != CRYPT_OK) {
+      goto ERR_OUT;
+   }
+   if (fp_entries != FP_ENTRIES || fp_lut != FP_LUT || num_entries > fp_entries) {
+      err = CRYPT_INVALID_PACKET;
+      goto ERR_OUT;
+   }
+   if ((asn1_list = XCALLOC(3+num_entries*(4+2*(1<<FP_LUT))+1, sizeof(ltc_asn1_list))) == NULL) {
+      err = CRYPT_MEM;
+      goto ERR_OUT;
+   }
+   j = 0;
+   LTC_SET_ASN1(asn1_list, j++, LTC_ASN1_SHORT_INTEGER, &num_entries, 1);
+   LTC_SET_ASN1(asn1_list, j++, LTC_ASN1_SHORT_INTEGER, &fp_entries, 1);
+   LTC_SET_ASN1(asn1_list, j++, LTC_ASN1_SHORT_INTEGER, &fp_lut, 1);
+   for (i = 0; i < num_entries; i++) {
+      if((fp_cache[i].g = ltc_ecc_new_point()) == NULL) {
+         err = CRYPT_MEM;
+         goto ERR_OUT;
+      }
+      LTC_SET_ASN1(asn1_list, j++, LTC_ASN1_INTEGER, fp_cache[i].g->x, 1);
+      LTC_SET_ASN1(asn1_list, j++, LTC_ASN1_INTEGER, fp_cache[i].g->y, 1);
+      LTC_SET_ASN1(asn1_list, j++, LTC_ASN1_INTEGER, fp_cache[i].g->z, 1);
+      for (x = 0; x < (1U<<FP_LUT); x++) {
+         /* since we don't store z in the cache, don't use ltc_ecc_new_point() 
+          * (which allocates space for z, only to have to free it later) */
+         ecc_point *p = XCALLOC(1, sizeof(*p));
+
+         if (p == NULL) {
+            err = CRYPT_MEM;
+            goto ERR_OUT;
+         }
+         fp_cache[i].LUT[x] = p;
+         if ((err = mp_init_multi(&p->x, &p->y, NULL)) != CRYPT_OK) {
+            goto ERR_OUT;
+         }
+         p->z = NULL;
+         LTC_SET_ASN1(asn1_list, j++, LTC_ASN1_INTEGER, p->x, 1);
+         LTC_SET_ASN1(asn1_list, j++, LTC_ASN1_INTEGER, p->y, 1);
+      }
+      if((err = mp_init(&fp_cache[i].mu)) != CRYPT_OK) {
+         goto ERR_OUT;
+      }
+      LTC_SET_ASN1(asn1_list, j++, LTC_ASN1_INTEGER, fp_cache[i].mu, 1);
+      fp_cache[i].lru_count = 3;
+      fp_cache[i].lock = 1;
+   }
+
+   if ((err = der_decode_sequence(in, inlen, asn1_list, j)) != CRYPT_OK) {
+      goto ERR_OUT;
+   }
+   XFREE(asn1_list);
+   LTC_MUTEX_UNLOCK(&ltc_ecc_fp_lock);
+   return CRYPT_OK;
+ERR_OUT:
+   if(asn1_list)
+      XFREE(asn1_list);
+   ltc_ecc_fp_free_cache();
+   LTC_MUTEX_UNLOCK(&ltc_ecc_fp_lock);
+   return err;
+}
+
+#endif
+
+
+/* $Source$ */
+/* $Revision$ */
+/* $Date$ */
+
diff --git a/core/lib/libtomcrypt/src/math/fp/sub.mk b/core/lib/libtomcrypt/src/math/fp/sub.mk
new file mode 100644
index 0000000..c3dacea
--- /dev/null
+++ b/core/lib/libtomcrypt/src/math/fp/sub.mk
@@ -0,0 +1 @@
+srcs-$(CFG_CRYPTO_ECC) += ltc_ecc_fp_mulmod.c
diff --git a/core/lib/libtomcrypt/src/math/multi.c b/core/lib/libtomcrypt/src/math/multi.c
new file mode 100644
index 0000000..d2d8f96
--- /dev/null
+++ b/core/lib/libtomcrypt/src/math/multi.c
@@ -0,0 +1,116 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+#ifdef LTC_MPI
+#include <stdarg.h>
+
+int ltc_init_multi(void **a, ...)
+{
+   void    **cur = a;
+   int       np  = 0;
+   va_list   args;
+
+   va_start(args, a);
+   while (cur != NULL) {
+       if (mp_init(cur) != CRYPT_OK) {
+          /* failed */
+          va_list clean_list;
+
+          va_start(clean_list, a);
+          cur = a;
+          while (np--) {
+              mp_clear(*cur);
+              cur = va_arg(clean_list, void**);
+          }
+          va_end(clean_list);
+          return CRYPT_MEM;
+       }
+       ++np;
+       cur = va_arg(args, void**);
+   }
+   va_end(args);
+   return CRYPT_OK;   
+}
+
+int ltc_init_multi_size(int size_bits, void **a, ...)
+{
+   void    **cur = a;
+   int       np  = 0;
+   va_list   args;
+
+   va_start(args, a);
+   while (cur != NULL) {
+       if (mp_init_size(size_bits, cur) != CRYPT_OK) {
+          /* failed */
+          va_list clean_list;
+
+          va_start(clean_list, a);
+          cur = a;
+          while (np--) {
+              mp_clear(*cur);
+              cur = va_arg(clean_list, void**);
+          }
+          va_end(clean_list);
+          return CRYPT_MEM;
+       }
+       ++np;
+       cur = va_arg(args, void**);
+   }
+   va_end(args);
+   return CRYPT_OK;   
+}
+
+void ltc_deinit_multi(void *a, ...)
+{
+   void     *cur = a;
+   va_list   args;
+
+   va_start(args, a);
+   while (cur != NULL) {
+       mp_clear(cur);
+       cur = va_arg(args, void *);
+   }
+   va_end(args);
+}
+
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/math/multi.c,v $ */
+/* $Revision: 1.6 $ */
+/* $Date: 2006/12/28 01:27:23 $ */
diff --git a/core/lib/libtomcrypt/src/math/rand_bn.c b/core/lib/libtomcrypt/src/math/rand_bn.c
new file mode 100755
index 0000000..bd328e0
--- /dev/null
+++ b/core/lib/libtomcrypt/src/math/rand_bn.c
@@ -0,0 +1,71 @@
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ */
+#include "tomcrypt.h"
+
+#ifdef LTC_MDSA
+/**
+  Generate a random number N with given bitlength (note: MSB can be 0)
+*/
+
+int rand_bn_bits(void *N, int bits, prng_state *prng, int wprng)
+{
+   int res, bytes;
+   unsigned char *buf, mask;
+
+   LTC_ARGCHK(N != NULL);
+   LTC_ARGCHK(bits > 1);
+
+   /* check PRNG */
+   if ((res = prng_is_valid(wprng)) != CRYPT_OK) return res;
+
+   bytes = (bits+7) >> 3;
+   mask = 0xff << (8 - bits % 8);
+
+   /* allocate buffer */
+   if ((buf = XCALLOC(1, bytes)) == NULL) return CRYPT_MEM;
+
+   /* generate random bytes */
+   if (prng_descriptor[wprng]->read(buf, bytes, prng) != (unsigned long)bytes) {
+      res = CRYPT_ERROR_READPRNG;
+      goto cleanup;
+   }
+   /* mask bits */
+   buf[0] &= ~mask;
+   /* load value */
+   if ((res = mp_read_unsigned_bin(N, buf, bytes)) != CRYPT_OK) goto cleanup;
+
+   res = CRYPT_OK;
+
+cleanup:
+#ifdef LTC_CLEAN_STACK
+   zeromem(buf, bytes);
+#endif
+   XFREE(buf);
+   return res;
+}
+
+/**
+  Generate a random number N in a range: 0 <= N < limit
+*/
+int rand_bn_range(void *N, void *limit, prng_state *prng, int wprng)
+{
+   int res;
+
+   LTC_ARGCHK(N != NULL);
+   LTC_ARGCHK(limit != NULL);
+
+   do {
+     res = rand_bn_bits(N, mp_count_bits(limit), prng, wprng);
+     if (res != CRYPT_OK) return res;
+   } while (mp_cmp(N, limit) != LTC_MP_LT);
+
+   return CRYPT_OK;
+}
+#endif
diff --git a/core/lib/libtomcrypt/src/math/rand_prime.c b/core/lib/libtomcrypt/src/math/rand_prime.c
new file mode 100644
index 0000000..4b73142
--- /dev/null
+++ b/core/lib/libtomcrypt/src/math/rand_prime.c
@@ -0,0 +1,117 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+#if !defined LTC_NO_MATH && !defined LTC_NO_PRNGS
+
+/**
+  @file rand_prime.c
+  Generate a random prime, Tom St Denis
+*/  
+
+#define USE_BBS 1
+
+int rand_prime(void *N, long len, prng_state *prng, int wprng)
+{
+   int            err, res, type;
+   unsigned char *buf;
+
+   LTC_ARGCHK(N != NULL);
+
+   /* get type */
+   if (len < 0) {
+      type = USE_BBS;
+      len = -len;
+   } else {
+      type = 0;
+   }
+
+   /* allow sizes between 2 and 512 bytes for a prime size */
+   if (len < 2 || len > 512) { 
+      return CRYPT_INVALID_PRIME_SIZE;
+   }
+   
+   /* valid PRNG? Better be! */
+   if ((err = prng_is_valid(wprng)) != CRYPT_OK) {
+      return err; 
+   }
+
+   /* allocate buffer to work with */
+   buf = XCALLOC(1, len);
+   if (buf == NULL) {
+       return CRYPT_MEM;
+   }
+
+   do {
+      /* generate value */
+      if (prng_descriptor[wprng]->read(buf, len, prng) != (unsigned long)len) {
+         XFREE(buf);
+         return CRYPT_ERROR_READPRNG;
+      }
+
+      /* munge bits */
+      buf[0]     |= 0x80 | 0x40;
+      buf[len-1] |= 0x01 | ((type & USE_BBS) ? 0x02 : 0x00);
+ 
+      /* load value */
+      if ((err = mp_read_unsigned_bin(N, buf, len)) != CRYPT_OK) {
+         XFREE(buf);
+         return err;
+      }
+
+      /* test */
+      if ((err = mp_prime_is_prime(N, 8, &res)) != CRYPT_OK) {
+         XFREE(buf);
+         return err;
+      }
+   } while (res == LTC_MP_NO);
+
+#ifdef LTC_CLEAN_STACK
+   zeromem(buf, len);
+#endif
+
+   XFREE(buf);
+   return CRYPT_OK;
+}
+      
+#endif /* LTC_NO_MATH */
+
+
+/* $Source: /cvs/libtom/libtomcrypt/src/math/rand_prime.c,v $ */
+/* $Revision: 1.7 $ */
+/* $Date: 2006/12/28 01:27:23 $ */
diff --git a/core/lib/libtomcrypt/src/math/sub.mk b/core/lib/libtomcrypt/src/math/sub.mk
new file mode 100644
index 0000000..4a38a32
--- /dev/null
+++ b/core/lib/libtomcrypt/src/math/sub.mk
@@ -0,0 +1,4 @@
+subdirs-y += fp
+srcs-y += multi.c
+srcs-y += rand_prime.c
+srcs-y += rand_bn.c
diff --git a/core/lib/libtomcrypt/src/misc/base64/base64_decode.c b/core/lib/libtomcrypt/src/misc/base64/base64_decode.c
new file mode 100644
index 0000000..6ed8530
--- /dev/null
+++ b/core/lib/libtomcrypt/src/misc/base64/base64_decode.c
@@ -0,0 +1,185 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file base64_decode.c
+  Compliant base64 code donated by Wayne Scott (wscott@bitmover.com)
+  base64 URL Safe variant (RFC 4648 section 5) by Karel Miko
+*/
+
+
+#if defined(LTC_BASE64) || defined (LTC_BASE64_URL)
+
+#if defined(LTC_BASE64)
+static const unsigned char map_base64[256] = {
+255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
+255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
+255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
+255, 255, 255, 255, 255, 255, 255,  62, 255, 255, 255,  63,
+ 52,  53,  54,  55,  56,  57,  58,  59,  60,  61, 255, 255,
+255, 254, 255, 255, 255,   0,   1,   2,   3,   4,   5,   6,
+  7,   8,   9,  10,  11,  12,  13,  14,  15,  16,  17,  18,
+ 19,  20,  21,  22,  23,  24,  25, 255, 255, 255, 255, 255,
+255,  26,  27,  28,  29,  30,  31,  32,  33,  34,  35,  36,
+ 37,  38,  39,  40,  41,  42,  43,  44,  45,  46,  47,  48,
+ 49,  50,  51, 255, 255, 255, 255, 255, 255, 255, 255, 255,
+255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
+255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
+255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
+255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
+255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
+255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
+255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
+255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
+255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
+255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
+255, 255, 255, 255 };
+#endif /* LTC_BASE64 */
+
+#if defined(LTC_BASE64_URL)
+static const unsigned char map_base64url[256] = {
+255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
+255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
+255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
+255, 255, 255, 255, 255, 255, 255, 255, 255,  62, 255, 255,
+ 52,  53,  54,  55,  56,  57,  58,  59,  60,  61, 255, 255,
+255, 254, 255, 255, 255,   0,   1,   2,   3,   4,   5,   6,
+  7,   8,   9,  10,  11,  12,  13,  14,  15,  16,  17,  18,
+ 19,  20,  21,  22,  23,  24,  25, 255, 255, 255, 255,  63,
+255,  26,  27,  28,  29,  30,  31,  32,  33,  34,  35,  36,
+ 37,  38,  39,  40,  41,  42,  43,  44,  45,  46,  47,  48,
+ 49,  50,  51, 255, 255, 255, 255, 255, 255, 255, 255, 255,
+255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
+255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
+255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
+255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
+255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
+255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
+255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
+255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
+255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
+255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
+255, 255, 255, 255 };
+#endif /* LTC_BASE64_URL */
+
+static int _base64_decode_internal(const unsigned char *in,  unsigned long inlen,
+                                 unsigned char *out, unsigned long *outlen,
+                           const unsigned char *map)
+{
+   unsigned long t, x, y, z;
+   unsigned char c;
+   int           g;
+
+   LTC_ARGCHK(in     != NULL);
+   LTC_ARGCHK(out    != NULL);
+   LTC_ARGCHK(outlen != NULL);
+
+   g = 3;
+   for (x = y = z = t = 0; x < inlen; x++) {
+       c = map[in[x]&0xFF];
+       if (c == 255) continue;
+       /* the final = symbols are read and used to trim the remaining bytes */
+       if (c == 254) { 
+          c = 0; 
+          /* prevent g < 0 which would potentially allow an overflow later */
+          if (--g < 0) {
+             return CRYPT_INVALID_PACKET;
+          }
+       } else if (g != 3) {
+          /* we only allow = to be at the end */
+          return CRYPT_INVALID_PACKET;
+       }
+
+       t = (t<<6)|c;
+
+       if (++y == 4) {
+          if (z + g > *outlen) { 
+             return CRYPT_BUFFER_OVERFLOW; 
+          }
+          out[z++] = (unsigned char)((t>>16)&255);
+          if (g > 1) out[z++] = (unsigned char)((t>>8)&255);
+          if (g > 2) out[z++] = (unsigned char)(t&255);
+          y = t = 0;
+       }
+   }
+   if (y != 0) {
+       return CRYPT_INVALID_PACKET;
+   }
+   *outlen = z;
+   return CRYPT_OK;
+}
+
+#if defined(LTC_BASE64)
+/**
+   base64 decode a block of memory
+   @param in       The base64 data to decode
+   @param inlen    The length of the base64 data
+   @param out      [out] The destination of the binary decoded data
+   @param outlen   [in/out] The max size and resulting size of the decoded data
+   @return CRYPT_OK if successful
+*/
+int base64_decode(const unsigned char *in,  unsigned long inlen,
+                        unsigned char *out, unsigned long *outlen)
+{
+    return _base64_decode_internal(in, inlen, out, outlen, map_base64);
+}
+#endif /* LTC_BASE64 */
+
+#if defined(LTC_BASE64_URL)
+/**
+   base64 (URL Safe, RFC 4648 section 5) decode a block of memory
+   @param in       The base64 data to decode
+   @param inlen    The length of the base64 data
+   @param out      [out] The destination of the binary decoded data
+   @param outlen   [in/out] The max size and resulting size of the decoded data
+   @return CRYPT_OK if successful
+*/
+int base64url_decode(const unsigned char *in,  unsigned long inlen,
+                           unsigned char *out, unsigned long *outlen)
+{
+    return _base64_decode_internal(in, inlen, out, outlen, map_base64url);
+}
+#endif /* LTC_BASE64_URL */
+
+#endif
+
+
+/* $Source: /cvs/libtom/libtomcrypt/src/misc/base64/base64_decode.c,v $ */
+/* $Revision: 1.6 $ */
+/* $Date: 2007/05/12 14:32:35 $ */
diff --git a/core/lib/libtomcrypt/src/misc/base64/base64_encode.c b/core/lib/libtomcrypt/src/misc/base64/base64_encode.c
new file mode 100644
index 0000000..822c174
--- /dev/null
+++ b/core/lib/libtomcrypt/src/misc/base64/base64_encode.c
@@ -0,0 +1,147 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file base64_encode.c
+  Compliant base64 encoder donated by Wayne Scott (wscott@bitmover.com)
+  base64 URL Safe variant (RFC 4648 section 5) by Karel Miko
+*/
+
+
+#if defined(LTC_BASE64) || defined (LTC_BASE64_URL)
+
+#if defined(LTC_BASE64)
+static const char *codes_base64 =
+"ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz0123456789+/";
+#endif /* LTC_BASE64 */
+
+#if defined(LTC_BASE64_URL)
+static const char *codes_base64url =
+"ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz0123456789-_";
+#endif /* LTC_BASE64_URL */
+
+static int _base64_encode_internal(const unsigned char *in,  unsigned long inlen,
+                                 unsigned char *out, unsigned long *outlen,
+                                 const char *codes, int pad)
+{
+   unsigned long i, len2, leven;
+   unsigned char *p;
+
+   LTC_ARGCHK(in     != NULL);
+   LTC_ARGCHK(out    != NULL);
+   LTC_ARGCHK(outlen != NULL);
+
+   /* valid output size ? */
+   len2 = 4 * ((inlen + 2) / 3);
+   if (*outlen < len2 + 1) {
+      *outlen = len2 + 1;
+      return CRYPT_BUFFER_OVERFLOW;
+   }
+   p = out;
+   leven = 3*(inlen / 3);
+   for (i = 0; i < leven; i += 3) {
+       *p++ = codes[(in[0] >> 2) & 0x3F];
+       *p++ = codes[(((in[0] & 3) << 4) + (in[1] >> 4)) & 0x3F];
+       *p++ = codes[(((in[1] & 0xf) << 2) + (in[2] >> 6)) & 0x3F];
+       *p++ = codes[in[2] & 0x3F];
+       in += 3;
+   }
+   /* Pad it if necessary...  */
+   if (i < inlen) {
+       unsigned a = in[0];
+       unsigned b = (i+1 < inlen) ? in[1] : 0;
+
+       *p++ = codes[(a >> 2) & 0x3F];
+       *p++ = codes[(((a & 3) << 4) + (b >> 4)) & 0x3F];
+       if (pad) {
+         *p++ = (i+1 < inlen) ? codes[(((b & 0xf) << 2)) & 0x3F] : '=';
+         *p++ = '=';
+       }
+       else {
+         if (i+1 < inlen) *p++ = codes[(((b & 0xf) << 2)) & 0x3F];
+       }
+   }
+
+   /* append a NULL byte */
+   *p = '\0';
+
+   /* return ok */
+   *outlen = p - out;
+   return CRYPT_OK;
+}
+
+#if defined(LTC_BASE64)
+/**
+   base64 Encode a buffer (NUL terminated)
+   @param in      The input buffer to encode
+   @param inlen   The length of the input buffer
+   @param out     [out] The destination of the base64 encoded data
+   @param outlen  [in/out] The max size and resulting size
+   @return CRYPT_OK if successful
+*/
+int base64_encode(const unsigned char *in,  unsigned long inlen,
+                        unsigned char *out, unsigned long *outlen)
+{
+    return _base64_encode_internal(in, inlen, out, outlen, codes_base64, 1);
+}
+#endif /* LTC_BASE64 */
+
+
+#if defined(LTC_BASE64_URL)
+/**
+   base64 (URL Safe, RFC 4648 section 5) Encode a buffer (NUL terminated)
+   @param in      The input buffer to encode
+   @param inlen   The length of the input buffer
+   @param out     [out] The destination of the base64 encoded data
+   @param outlen  [in/out] The max size and resulting size
+   @return CRYPT_OK if successful
+*/
+int base64url_encode(const unsigned char *in,  unsigned long inlen,
+                           unsigned char *out, unsigned long *outlen)
+{
+    return _base64_encode_internal(in, inlen, out, outlen, codes_base64url, 0);
+}
+#endif /* LTC_BASE64_URL */
+
+#endif
+
+
+/* $Source: /cvs/libtom/libtomcrypt/src/misc/base64/base64_encode.c,v $ */
+/* $Revision: 1.7 $ */
+/* $Date: 2007/05/12 14:32:35 $ */
diff --git a/core/lib/libtomcrypt/src/misc/base64/sub.mk b/core/lib/libtomcrypt/src/misc/base64/sub.mk
new file mode 100644
index 0000000..9191690
--- /dev/null
+++ b/core/lib/libtomcrypt/src/misc/base64/sub.mk
@@ -0,0 +1,2 @@
+srcs-y += base64_decode.c
+srcs-y += base64_encode.c
diff --git a/core/lib/libtomcrypt/src/misc/burn_stack.c b/core/lib/libtomcrypt/src/misc/burn_stack.c
new file mode 100644
index 0000000..8b903d4
--- /dev/null
+++ b/core/lib/libtomcrypt/src/misc/burn_stack.c
@@ -0,0 +1,61 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+   @file burn_stack.c
+   Burn stack, Tom St Denis
+*/
+
+/**
+   Burn some stack memory
+   @param len amount of stack to burn in bytes
+*/
+void burn_stack(unsigned long len)
+{
+   unsigned char buf[32];
+   zeromem(buf, sizeof(buf));
+   if (len > (unsigned long)sizeof(buf))
+      burn_stack(len - sizeof(buf));
+}
+
+
+
+/* $Source: /cvs/libtom/libtomcrypt/src/misc/burn_stack.c,v $ */
+/* $Revision: 1.5 $ */
+/* $Date: 2006/12/28 01:27:24 $ */
diff --git a/core/lib/libtomcrypt/src/misc/crypt/crypt.c b/core/lib/libtomcrypt/src/misc/crypt/crypt.c
new file mode 100644
index 0000000..369c5ac
--- /dev/null
+++ b/core/lib/libtomcrypt/src/misc/crypt/crypt.c
@@ -0,0 +1,472 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file crypt.c
+  Build strings, Tom St Denis
+*/
+#define NAME_VALUE(s) #s"="NAME(s)
+#define NAME(s) #s
+
+const char *crypt_build_settings =
+   "LibTomCrypt " SCRYPT " (Tom St Denis, tomstdenis@gmail.com)\n"
+   "LibTomCrypt is public domain software.\n"
+#if defined(INCLUDE_BUILD_DATE)
+   "Built on " __DATE__ " at " __TIME__ "\n"
+#endif
+   "\n\nEndianness: "
+#if defined(ENDIAN_NEUTRAL)
+   "neutral\n"
+#else
+#if defined(ENDIAN_LITTLE)
+   "little"
+#elif defined(ENDIAN_BIG)
+   "big"
+#endif
+   #if defined(ENDIAN_32BITWORD)
+   " (32-bit words)\n"
+   #else
+   " (64-bit words)\n"
+   #endif
+#endif
+   "Clean stack: "
+#if defined(LTC_CLEAN_STACK)
+   "enabled\n"
+#else
+   "disabled\n"
+#endif
+   "Ciphers built-in:\n"
+#if defined(LTC_BLOWFISH)
+   "   Blowfish\n"
+#endif
+#if defined(LTC_RC2)
+   "   RC2\n"
+#endif
+#if defined(LTC_RC5)
+   "   RC5\n"
+#endif
+#if defined(LTC_RC6)
+   "   RC6\n"
+#endif
+#if defined(LTC_SAFERP)
+   "   Safer+\n"
+#endif
+#if defined(LTC_SAFER)
+   "   Safer\n"
+#endif
+#if defined(LTC_RIJNDAEL)
+   "   Rijndael\n"
+#endif
+#if defined(LTC_XTEA)
+   "   XTEA\n"
+#endif
+#if defined(LTC_TWOFISH)
+   "   Twofish "
+   #if defined(LTC_TWOFISH_SMALL) && defined(LTC_TWOFISH_TABLES) && defined(LTC_TWOFISH_ALL_TABLES)
+       "(small, tables, all_tables)\n"
+   #elif defined(LTC_TWOFISH_SMALL) && defined(LTC_TWOFISH_TABLES)
+       "(small, tables)\n"
+   #elif defined(LTC_TWOFISH_SMALL) && defined(LTC_TWOFISH_ALL_TABLES)
+       "(small, all_tables)\n"
+   #elif defined(LTC_TWOFISH_TABLES) && defined(LTC_TWOFISH_ALL_TABLES)
+       "(tables, all_tables)\n"
+   #elif defined(LTC_TWOFISH_SMALL)
+       "(small)\n"
+   #elif defined(LTC_TWOFISH_TABLES)
+       "(tables)\n"
+   #elif defined(LTC_TWOFISH_ALL_TABLES)
+       "(all_tables)\n"
+   #else
+       "\n"
+   #endif
+#endif
+#if defined(LTC_DES)
+   "   DES\n"
+#endif
+#if defined(LTC_CAST5)
+   "   CAST5\n"
+#endif
+#if defined(LTC_NOEKEON)
+   "   Noekeon\n"
+#endif
+#if defined(LTC_SKIPJACK)
+   "   Skipjack\n"
+#endif
+#if defined(LTC_KHAZAD)
+   "   Khazad\n"
+#endif
+#if defined(LTC_ANUBIS)
+   "   Anubis "
+#endif
+#if defined(LTC_ANUBIS_TWEAK)
+   " (tweaked)"
+#endif
+   "\n"
+#if defined(LTC_KSEED)
+   "   KSEED\n"
+#endif
+#if defined(LTC_KASUMI)
+   "   KASUMI\n"
+#endif
+#if defined(LTC_MULTI2)
+   "   MULTI2\n"
+#endif
+#if defined(LTC_CAMELLIA)
+   "   Camellia\n"
+#endif
+
+    "\nHashes built-in:\n"
+#if defined(LTC_SHA512)
+   "   SHA-512\n"
+#endif
+#if defined(LTC_SHA384)
+   "   SHA-384\n"
+#endif
+#if defined(LTC_SHA512_256)
+   "   SHA-512/256\n"
+#endif
+#if defined(LTC_SHA256)
+   "   SHA-256\n"
+#endif
+#if defined(LTC_SHA512_224)
+   "   SHA-512/224\n"
+#endif
+#if defined(LTC_SHA224)
+   "   SHA-224\n"
+#endif
+#if defined(LTC_TIGER)
+   "   TIGER\n"
+#endif
+#if defined(LTC_SHA1)
+   "   SHA1\n"
+#endif
+#if defined(LTC_MD5)
+   "   MD5\n"
+#endif
+#if defined(LTC_MD4)
+   "   MD4\n"
+#endif
+#if defined(LTC_MD2)
+   "   MD2\n"
+#endif
+#if defined(LTC_RIPEMD128)
+   "   RIPEMD128\n"
+#endif
+#if defined(LTC_RIPEMD160)
+   "   RIPEMD160\n"
+#endif
+#if defined(LTC_RIPEMD256)
+   "   RIPEMD256\n"
+#endif
+#if defined(LTC_RIPEMD320)
+   "   RIPEMD320\n"
+#endif
+#if defined(LTC_WHIRLPOOL)
+   "   WHIRLPOOL\n"
+#endif
+#if defined(LTC_CHC_HASH)
+   "   CHC_HASH\n"
+#endif
+
+    "\nBlock Chaining Modes:\n"
+#if defined(LTC_CFB_MODE)
+    "   CFB\n"
+#endif
+#if defined(LTC_OFB_MODE)
+    "   OFB\n"
+#endif
+#if defined(LTC_ECB_MODE)
+    "   ECB\n"
+#endif
+#if defined(LTC_CBC_MODE)
+    "   CBC\n"
+#endif
+#if defined(LTC_CTR_MODE)
+    "   CTR\n"
+#endif
+#if defined(LTC_LRW_MODE)
+    "   LRW"
+#if defined(LTC_LRW_TABLES)
+    " (tables) "
+#endif
+    "\n"
+#endif
+#if defined(LTC_F8_MODE)
+    "   F8\n"
+#endif
+#if defined(LTC_XTS_MODE)
+    "   XTS\n"
+#endif
+
+    "\nMACs:\n"
+#if defined(LTC_HMAC)
+    "   HMAC\n"
+#endif
+#if defined(LTC_OMAC)
+    "   OMAC\n"
+#endif
+#if defined(LTC_PMAC)
+    "   PMAC\n"
+#endif
+#if defined(LTC_PELICAN)
+    "   PELICAN\n"
+#endif
+#if defined(LTC_XCBC)
+    "   XCBC\n"
+#endif
+#if defined(LTC_F9_MODE)
+    "   F9\n"
+#endif
+
+    "\nENC + AUTH modes:\n"
+#if defined(LTC_EAX_MODE)
+    "   EAX\n"
+#endif
+#if defined(LTC_OCB_MODE)
+    "   OCB\n"
+#endif
+#if defined(LTC_OCB3_MODE)
+    "   OCB3\n"
+#endif
+#if defined(LTC_CCM_MODE)
+    "   CCM\n"
+#endif
+#if defined(LTC_GCM_MODE)
+    "   GCM"
+#if defined(LTC_GCM_TABLES)
+    " (tables) "
+#endif
+#if defined(LTC_GCM_TABLES_SSE2)
+    " (SSE2) "
+#endif
+   "\n"
+#endif
+
+    "\nPRNG:\n"
+#if defined(LTC_YARROW)
+    "   Yarrow ("NAME_VALUE(LTC_YARROW_AES)")\n"
+#endif
+#if defined(LTC_SPRNG)
+    "   SPRNG\n"
+#endif
+#if defined(LTC_RC4)
+    "   RC4\n"
+#endif
+#if defined(LTC_FORTUNA)
+    "   Fortuna (" NAME_VALUE(LTC_FORTUNA_POOLS) ", " NAME_VALUE(LTC_FORTUNA_WD) ")\n"
+#endif
+#if defined(LTC_SOBER128)
+    "   SOBER128\n"
+#endif
+
+    "\nPK Algs:\n"
+#if defined(LTC_MRSA)
+    "   RSA"
+#if defined(LTC_RSA_BLINDING) && defined(LTC_RSA_CRT_HARDENING)
+    " (with blinding and CRT hardening)"
+#elif defined(LTC_RSA_BLINDING)
+    " (with blinding)"
+#elif defined(LTC_RSA_CRT_HARDENING)
+    " (with CRT hardening)"
+#endif
+    "\n"
+#endif
+#if defined(LTC_MDH)
+    "   DH\n"
+#endif
+#if defined(LTC_MECC)
+    "   ECC"
+#if defined(LTC_ECC_TIMING_RESISTANT)
+    " (with blinding)"
+#endif
+    "\n"
+#endif
+#if defined(LTC_MDSA)
+    "   DSA\n"
+#endif
+#if defined(LTC_MKAT)
+    "   Katja\n"
+#endif
+
+    "\nCompiler:\n"
+#if defined(_WIN64)
+    "   WIN64 platform detected.\n"
+#elif defined(_WIN32)
+    "   WIN32 platform detected.\n"
+#endif
+#if defined(__CYGWIN__)
+    "   CYGWIN Detected.\n"
+#endif
+#if defined(__DJGPP__)
+    "   DJGPP Detected.\n"
+#endif
+#if defined(_MSC_VER)
+    "   MSVC compiler detected.\n"
+#endif
+#if defined(__clang_version__)
+    "   Clang compiler " __clang_version__ ".\n"
+#elif defined(INTEL_CC)
+    "   Intel C Compiler " __VERSION__ ".\n"
+#elif defined(__GNUC__)         /* clang and icc also define __GNUC__ */
+    "   GCC compiler " __VERSION__ ".\n"
+#endif
+
+#if defined(__x86_64__)
+    "   x86-64 detected.\n"
+#endif
+#if defined(LTC_PPC32)
+    "   PPC32 detected.\n"
+#endif
+
+    "\nVarious others: "
+#if defined(LTC_ADLER32)
+    " ADLER32 "
+#endif
+#if defined(LTC_BASE64)
+    " BASE64 "
+#endif
+#if defined(LTC_BASE64_URL)
+    " BASE64-URL-SAFE "
+#endif
+#if defined(LTC_CRC32)
+    " CRC32 "
+#endif
+#if defined(LTC_DER)
+    " DER "
+#endif
+#if defined(LTC_DER_MAX_PUBKEY_SIZE)
+    " " NAME_VALUE(LTC_DER_MAX_PUBKEY_SIZE) " "
+#endif
+#if defined(LTC_PKCS_1)
+    " PKCS#1 "
+#endif
+#if defined(LTC_PKCS_5)
+    " PKCS#5 "
+#endif
+#if defined(LTC_HKDF)
+    " HKDF "
+#endif
+#if defined(LTC_MPI)
+    " MPI "
+#endif
+#if defined(LTC_DEVRANDOM)
+    " LTC_DEVRANDOM "
+#endif
+#if defined(LTC_TRY_URANDOM_FIRST)
+    " LTC_TRY_URANDOM_FIRST "
+#endif
+#if defined(LTC_RNG_GET_BYTES)
+    " LTC_RNG_GET_BYTES "
+#endif
+#if defined(LTC_RNG_MAKE_PRNG)
+    " LTC_RNG_MAKE_PRNG "
+#endif
+#if defined(LTC_HASH_HELPERS)
+    " LTC_HASH_HELPERS "
+#endif
+#if defined(LTC_VALGRIND)
+    " LTC_VALGRIND "
+#endif
+#if defined(LTC_TEST)
+    " LTC_TEST "
+#endif
+#if defined(LTC_TEST_EXT)
+    " LTC_TEST_EXT "
+#endif
+#if defined(LTC_SMALL_CODE)
+    " LTC_SMALL_CODE "
+#endif
+#if defined(LTC_NO_FILE)
+    " LTC_NO_FILE "
+#endif
+#if defined(LTC_FAST)
+    " LTC_FAST "
+#endif
+#if defined(LTC_NO_FAST)
+    " LTC_NO_FAST "
+#endif
+#if defined(LTC_NO_BSWAP)
+    " LTC_NO_BSWAP "
+#endif
+#if defined(LTC_NO_ASM)
+    " LTC_NO_ASM "
+#endif
+#if defined(LTC_ROx_ASM)
+    " LTC_ROx_ASM "
+#if defined(LTC_NO_ROLC)
+    " LTC_NO_ROLC "
+#endif
+#endif
+#if defined(LTC_NO_TEST)
+    " LTC_NO_TEST "
+#endif
+#if defined(LTC_NO_TABLES)
+    " LTC_NO_TABLES "
+#endif
+#if defined(LTC_PTHREAD)
+    " LTC_PTHREAD "
+#endif
+#if defined(LTM_DESC)
+    " LTM_DESC "
+#endif
+#if defined(TFM_DESC)
+    " TFM_DESC "
+#endif
+#if defined(GMP_DESC)
+    " GMP_DESC "
+#endif
+#if defined(LTC_EASY)
+    " LTC_EASY "
+#endif
+#if defined(LTC_MECC_ACCEL)
+    " LTC_MECC_ACCEL "
+#endif
+#if defined(LTC_MECC_FP)
+   " LTC_MECC_FP "
+#endif
+#if defined(LTC_ECC_SHAMIR)
+   " LTC_ECC_SHAMIR "
+#endif
+    "\n"
+    ;
+
+
+/* $Source: /cvs/libtom/libtomcrypt/src/misc/crypt/crypt.c,v $ */
+/* $Revision: 1.36 $ */
+/* $Date: 2007/05/12 14:46:12 $ */
diff --git a/core/lib/libtomcrypt/src/misc/crypt/crypt_argchk.c b/core/lib/libtomcrypt/src/misc/crypt/crypt_argchk.c
new file mode 100644
index 0000000..10591df
--- /dev/null
+++ b/core/lib/libtomcrypt/src/misc/crypt/crypt_argchk.c
@@ -0,0 +1,56 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file crypt_argchk.c
+  Perform argument checking, Tom St Denis
+*/  
+
+#if (ARGTYPE == 0)
+void crypt_argchk(char *v, char *s, int d)
+{
+ fprintf(stderr, "LTC_ARGCHK '%s' failure on line %d of file %s\n",
+         v, d, s);
+ abort();
+}
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/misc/crypt/crypt_argchk.c,v $ */
+/* $Revision: 1.5 $ */
+/* $Date: 2006/12/28 01:27:24 $ */
diff --git a/core/lib/libtomcrypt/src/misc/crypt/crypt_cipher_descriptor.c b/core/lib/libtomcrypt/src/misc/crypt/crypt_cipher_descriptor.c
new file mode 100644
index 0000000..2a10863
--- /dev/null
+++ b/core/lib/libtomcrypt/src/misc/crypt/crypt_cipher_descriptor.c
@@ -0,0 +1,52 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file crypt_cipher_descriptor.c
+  Stores the cipher descriptor table, Tom St Denis
+*/
+
+const struct ltc_cipher_descriptor *cipher_descriptor[TAB_SIZE];
+
+LTC_MUTEX_GLOBAL(ltc_cipher_mutex)
+
+
+/* $Source: /cvs/libtom/libtomcrypt/src/misc/crypt/crypt_cipher_descriptor.c,v $ */
+/* $Revision: 1.13 $ */
+/* $Date: 2006/12/28 01:27:24 $ */
diff --git a/core/lib/libtomcrypt/src/misc/crypt/crypt_cipher_is_valid.c b/core/lib/libtomcrypt/src/misc/crypt/crypt_cipher_is_valid.c
new file mode 100644
index 0000000..1e0ed18
--- /dev/null
+++ b/core/lib/libtomcrypt/src/misc/crypt/crypt_cipher_is_valid.c
@@ -0,0 +1,63 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file crypt_cipher_is_valid.c
+  Determine if cipher is valid, Tom St Denis
+*/
+
+/*
+   Test if a cipher index is valid
+   @param idx   The index of the cipher to search for
+   @return CRYPT_OK if valid
+*/
+int cipher_is_valid(int idx)
+{
+   LTC_MUTEX_LOCK(&ltc_cipher_mutex);
+   if (idx < 0 || idx >= TAB_SIZE || cipher_descriptor[idx] == NULL) {
+      LTC_MUTEX_UNLOCK(&ltc_cipher_mutex);
+      return CRYPT_INVALID_CIPHER;
+   }
+   LTC_MUTEX_UNLOCK(&ltc_cipher_mutex);
+   return CRYPT_OK;
+}
+
+/* $Source: /cvs/libtom/libtomcrypt/src/misc/crypt/crypt_cipher_is_valid.c,v $ */
+/* $Revision: 1.6 $ */
+/* $Date: 2006/12/28 01:27:24 $ */
diff --git a/core/lib/libtomcrypt/src/misc/crypt/crypt_find_cipher.c b/core/lib/libtomcrypt/src/misc/crypt/crypt_find_cipher.c
new file mode 100644
index 0000000..ddafca8
--- /dev/null
+++ b/core/lib/libtomcrypt/src/misc/crypt/crypt_find_cipher.c
@@ -0,0 +1,68 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file crypt_find_cipher.c
+  Find a cipher in the descriptor tables, Tom St Denis
+*/
+
+/**
+   Find a registered cipher by name
+   @param name   The name of the cipher to look for
+   @return >= 0 if found, -1 if not present
+*/
+int find_cipher(const char *name)
+{
+   int x;
+   LTC_ARGCHK(name != NULL);
+   LTC_MUTEX_LOCK(&ltc_cipher_mutex);
+   for (x = 0; x < TAB_SIZE; x++) {
+       if (cipher_descriptor[x] != NULL && !XSTRCMP(cipher_descriptor[x]->name, name)) {
+          LTC_MUTEX_UNLOCK(&ltc_cipher_mutex);
+          return x;
+       }
+   }
+   LTC_MUTEX_UNLOCK(&ltc_cipher_mutex);
+   return -1;
+}
+
+
+/* $Source: /cvs/libtom/libtomcrypt/src/misc/crypt/crypt_find_cipher.c,v $ */
+/* $Revision: 1.7 $ */
+/* $Date: 2006/12/28 01:27:24 $ */
diff --git a/core/lib/libtomcrypt/src/misc/crypt/crypt_find_cipher_any.c b/core/lib/libtomcrypt/src/misc/crypt/crypt_find_cipher_any.c
new file mode 100644
index 0000000..ccc2f9f
--- /dev/null
+++ b/core/lib/libtomcrypt/src/misc/crypt/crypt_find_cipher_any.c
@@ -0,0 +1,77 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file crypt_find_cipher_any.c
+  Find a cipher in the descriptor tables, Tom St Denis
+*/
+
+/**
+   Find a cipher flexibly.  First by name then if not present by block and key size 
+   @param name        The name of the cipher desired
+   @param blocklen    The minimum length of the block cipher desired (octets)
+   @param keylen      The minimum length of the key size desired (octets)
+   @return >= 0 if found, -1 if not present
+*/
+int find_cipher_any(const char *name, int blocklen, int keylen)
+{
+   int x;
+
+   LTC_ARGCHK(name != NULL);
+
+   x = find_cipher(name);
+   if (x != -1) return x;
+
+   LTC_MUTEX_LOCK(&ltc_cipher_mutex);
+   for (x = 0; x < TAB_SIZE; x++) {
+       if (cipher_descriptor[x] == NULL) {
+          continue;
+       }
+       if (blocklen <= (int)cipher_descriptor[x]->block_length && keylen <= (int)cipher_descriptor[x]->max_key_length) {
+          LTC_MUTEX_UNLOCK(&ltc_cipher_mutex);
+          return x;
+       }
+   }
+   LTC_MUTEX_UNLOCK(&ltc_cipher_mutex);
+   return -1;
+}
+
+/* $Source: /cvs/libtom/libtomcrypt/src/misc/crypt/crypt_find_cipher_any.c,v $ */
+/* $Revision: 1.6 $ */
+/* $Date: 2006/12/28 01:27:24 $ */
diff --git a/core/lib/libtomcrypt/src/misc/crypt/crypt_find_cipher_id.c b/core/lib/libtomcrypt/src/misc/crypt/crypt_find_cipher_id.c
new file mode 100644
index 0000000..f4a8cc5
--- /dev/null
+++ b/core/lib/libtomcrypt/src/misc/crypt/crypt_find_cipher_id.c
@@ -0,0 +1,66 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file crypt_find_cipher_id.c
+  Find cipher by ID, Tom St Denis
+*/
+
+/**
+   Find a cipher by ID number
+   @param ID    The ID (not same as index) of the cipher to find
+   @return >= 0 if found, -1 if not present
+*/
+int find_cipher_id(unsigned char ID)
+{
+   int x;
+   LTC_MUTEX_LOCK(&ltc_cipher_mutex);
+   for (x = 0; x < TAB_SIZE && cipher_descriptor[x] != NULL; x++) {
+       if (cipher_descriptor[x]->ID == ID) {
+          LTC_MUTEX_UNLOCK(&ltc_cipher_mutex);
+          return x;
+       }
+   }
+   LTC_MUTEX_UNLOCK(&ltc_cipher_mutex);
+   return -1;
+}
+
+/* $Source: /cvs/libtom/libtomcrypt/src/misc/crypt/crypt_find_cipher_id.c,v $ */
+/* $Revision: 1.6 $ */
+/* $Date: 2006/12/28 01:27:24 $ */
diff --git a/core/lib/libtomcrypt/src/misc/crypt/crypt_find_hash.c b/core/lib/libtomcrypt/src/misc/crypt/crypt_find_hash.c
new file mode 100644
index 0000000..cd51135
--- /dev/null
+++ b/core/lib/libtomcrypt/src/misc/crypt/crypt_find_hash.c
@@ -0,0 +1,67 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file crypt_find_hash.c
+  Find a hash, Tom St Denis
+*/
+
+/**
+   Find a registered hash by name
+   @param name   The name of the hash to look for
+   @return >= 0 if found, -1 if not present
+*/
+int find_hash(const char *name)
+{
+   int x;
+   LTC_ARGCHK(name != NULL);
+   LTC_MUTEX_LOCK(&ltc_hash_mutex);
+   for (x = 0; x < TAB_SIZE; x++) {
+       if (hash_descriptor[x] != NULL && XSTRCMP(hash_descriptor[x]->name, name) == 0) {
+          LTC_MUTEX_UNLOCK(&ltc_hash_mutex);
+          return x;
+       }
+   }
+   LTC_MUTEX_UNLOCK(&ltc_hash_mutex);
+   return -1;
+}
+
+/* $Source: /cvs/libtom/libtomcrypt/src/misc/crypt/crypt_find_hash.c,v $ */
+/* $Revision: 1.7 $ */
+/* $Date: 2006/12/28 01:27:24 $ */
diff --git a/core/lib/libtomcrypt/src/misc/crypt/crypt_find_hash_any.c b/core/lib/libtomcrypt/src/misc/crypt/crypt_find_hash_any.c
new file mode 100644
index 0000000..e6ac21e
--- /dev/null
+++ b/core/lib/libtomcrypt/src/misc/crypt/crypt_find_hash_any.c
@@ -0,0 +1,76 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file crypt_find_hash_any.c
+  Find a hash, Tom St Denis
+*/
+
+/**
+   Find a hash flexibly.  First by name then if not present by digest size 
+   @param name        The name of the hash desired
+   @param digestlen   The minimum length of the digest size (octets)
+   @return >= 0 if found, -1 if not present
+*/int find_hash_any(const char *name, int digestlen)
+{
+   int x, y, z;
+   LTC_ARGCHK(name != NULL);
+
+   x = find_hash(name);
+   if (x != -1) return x;
+
+   LTC_MUTEX_LOCK(&ltc_hash_mutex);
+   y = MAXBLOCKSIZE+1;
+   z = -1;
+   for (x = 0; x < TAB_SIZE; x++) {
+       if (hash_descriptor[x] == NULL) {
+          continue;
+       }
+       if ((int)hash_descriptor[x]->hashsize >= digestlen && (int)hash_descriptor[x]->hashsize < y) {
+          z = x;
+          y = hash_descriptor[x]->hashsize;
+       }
+   }
+   LTC_MUTEX_UNLOCK(&ltc_hash_mutex);
+   return z;
+}
+
+/* $Source: /cvs/libtom/libtomcrypt/src/misc/crypt/crypt_find_hash_any.c,v $ */
+/* $Revision: 1.6 $ */
+/* $Date: 2006/12/28 01:27:24 $ */
diff --git a/core/lib/libtomcrypt/src/misc/crypt/crypt_find_hash_id.c b/core/lib/libtomcrypt/src/misc/crypt/crypt_find_hash_id.c
new file mode 100644
index 0000000..07c81fc
--- /dev/null
+++ b/core/lib/libtomcrypt/src/misc/crypt/crypt_find_hash_id.c
@@ -0,0 +1,66 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file crypt_find_hash_id.c
+  Find hash by ID, Tom St Denis
+*/
+
+/**
+   Find a hash by ID number
+   @param ID    The ID (not same as index) of the hash to find
+   @return >= 0 if found, -1 if not present
+*/
+int find_hash_id(unsigned char ID)
+{
+   int x;
+   LTC_MUTEX_LOCK(&ltc_hash_mutex);
+   for (x = 0; x < TAB_SIZE; x++) {
+      if (hash_descriptor[x] && hash_descriptor[x]->ID == ID) {
+          LTC_MUTEX_UNLOCK(&ltc_hash_mutex);
+          return x;
+      }
+   }
+   LTC_MUTEX_UNLOCK(&ltc_hash_mutex);
+   return -1;
+}
+
+/* $Source: /cvs/libtom/libtomcrypt/src/misc/crypt/crypt_find_hash_id.c,v $ */
+/* $Revision: 1.7 $ */
+/* $Date: 2006/12/28 01:27:24 $ */
diff --git a/core/lib/libtomcrypt/src/misc/crypt/crypt_find_hash_oid.c b/core/lib/libtomcrypt/src/misc/crypt/crypt_find_hash_oid.c
new file mode 100644
index 0000000..5a0fb64
--- /dev/null
+++ b/core/lib/libtomcrypt/src/misc/crypt/crypt_find_hash_oid.c
@@ -0,0 +1,62 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file crypt_find_hash_oid.c
+  Find a hash, Tom St Denis
+*/
+
+int find_hash_oid(const unsigned long *ID, unsigned long IDlen)
+{
+   int x;
+   LTC_ARGCHK(ID != NULL);
+   LTC_MUTEX_LOCK(&ltc_hash_mutex);
+   for (x = 0; x < TAB_SIZE; x++) {
+       if (hash_descriptor[x] != NULL && hash_descriptor[x]->OIDlen == IDlen && !XMEMCMP(hash_descriptor[x]->OID, ID, sizeof(unsigned long) * IDlen)) {
+          LTC_MUTEX_UNLOCK(&ltc_hash_mutex);
+          return x;
+       }
+   }
+   LTC_MUTEX_UNLOCK(&ltc_hash_mutex);
+   return -1;
+}
+
+/* $Source: /cvs/libtom/libtomcrypt/src/misc/crypt/crypt_find_hash_oid.c,v $ */
+/* $Revision: 1.5 $ */
+/* $Date: 2006/12/28 01:27:24 $ */
diff --git a/core/lib/libtomcrypt/src/misc/crypt/crypt_find_prng.c b/core/lib/libtomcrypt/src/misc/crypt/crypt_find_prng.c
new file mode 100644
index 0000000..c4bd507
--- /dev/null
+++ b/core/lib/libtomcrypt/src/misc/crypt/crypt_find_prng.c
@@ -0,0 +1,68 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file crypt_find_prng.c
+  Find a PRNG, Tom St Denis
+*/
+
+/**
+   Find a registered PRNG by name
+   @param name   The name of the PRNG to look for
+   @return >= 0 if found, -1 if not present
+*/
+int find_prng(const char *name)
+{
+   int x;
+   LTC_ARGCHK(name != NULL);
+   LTC_MUTEX_LOCK(&ltc_prng_mutex);
+   for (x = 0; x < TAB_SIZE; x++) {
+       if ((prng_descriptor[x] != NULL) && XSTRCMP(prng_descriptor[x]->name, name) == 0) {
+          LTC_MUTEX_UNLOCK(&ltc_prng_mutex);
+          return x;
+       }
+   }
+   LTC_MUTEX_UNLOCK(&ltc_prng_mutex);
+   return -1;
+}
+
+
+/* $Source: /cvs/libtom/libtomcrypt/src/misc/crypt/crypt_find_prng.c,v $ */
+/* $Revision: 1.7 $ */
+/* $Date: 2006/12/28 01:27:24 $ */
diff --git a/core/lib/libtomcrypt/src/misc/crypt/crypt_fsa.c b/core/lib/libtomcrypt/src/misc/crypt/crypt_fsa.c
new file mode 100644
index 0000000..d912fd0
--- /dev/null
+++ b/core/lib/libtomcrypt/src/misc/crypt/crypt_fsa.c
@@ -0,0 +1,85 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+#include <stdarg.h>
+
+/**
+  @file crypt_fsa.c
+  LibTomCrypt FULL SPEED AHEAD!, Tom St Denis
+*/  
+
+/* format is ltc_mp, cipher_desc, [cipher_desc], NULL, hash_desc, [hash_desc], NULL, prng_desc, [prng_desc], NULL */
+int crypt_fsa(void *mp, ...)
+{
+   va_list  args;
+   void     *p;
+
+   va_start(args, mp);
+   if (mp != NULL) {
+      XMEMCPY(&ltc_mp, mp, sizeof(ltc_mp));
+   }
+   
+   while ((p = va_arg(args, void*)) != NULL) {
+      if (register_cipher(p) == -1) {
+         va_end(args);
+         return CRYPT_INVALID_CIPHER;
+      }
+   }
+
+   while ((p = va_arg(args, void*)) != NULL) {
+      if (register_hash(p) == -1) {
+         va_end(args);
+         return CRYPT_INVALID_HASH;
+      }
+   }
+
+   while ((p = va_arg(args, void*)) != NULL) {
+      if (register_prng(p) == -1) {
+         va_end(args);
+         return CRYPT_INVALID_PRNG;
+      }
+   }
+
+   va_end(args);
+   return CRYPT_OK;   
+}
+
+
+/* $Source: /cvs/libtom/libtomcrypt/src/misc/crypt/crypt_fsa.c,v $ */
+/* $Revision: 1.5 $ */
+/* $Date: 2006/12/28 01:27:24 $ */
diff --git a/core/lib/libtomcrypt/src/misc/crypt/crypt_hash_descriptor.c b/core/lib/libtomcrypt/src/misc/crypt/crypt_hash_descriptor.c
new file mode 100644
index 0000000..875919e
--- /dev/null
+++ b/core/lib/libtomcrypt/src/misc/crypt/crypt_hash_descriptor.c
@@ -0,0 +1,52 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file crypt_hash_descriptor.c
+  Stores the hash descriptor table, Tom St Denis  
+*/
+
+const struct ltc_hash_descriptor *hash_descriptor[TAB_SIZE];
+
+LTC_MUTEX_GLOBAL(ltc_hash_mutex)
+
+
+/* $Source: /cvs/libtom/libtomcrypt/src/misc/crypt/crypt_hash_descriptor.c,v $ */
+/* $Revision: 1.10 $ */
+/* $Date: 2006/12/28 01:27:24 $ */
diff --git a/core/lib/libtomcrypt/src/misc/crypt/crypt_hash_is_valid.c b/core/lib/libtomcrypt/src/misc/crypt/crypt_hash_is_valid.c
new file mode 100644
index 0000000..ca654d6
--- /dev/null
+++ b/core/lib/libtomcrypt/src/misc/crypt/crypt_hash_is_valid.c
@@ -0,0 +1,63 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file crypt_hash_is_valid.c
+  Determine if hash is valid, Tom St Denis
+*/  
+
+/*
+   Test if a hash index is valid
+   @param idx   The index of the hash to search for
+   @return CRYPT_OK if valid
+*/
+int hash_is_valid(int idx)
+{
+   LTC_MUTEX_LOCK(&ltc_hash_mutex);
+   if (idx < 0 || idx >= TAB_SIZE || hash_descriptor[idx] == NULL) {
+      LTC_MUTEX_UNLOCK(&ltc_hash_mutex);
+      return CRYPT_INVALID_HASH;
+   }
+   LTC_MUTEX_UNLOCK(&ltc_hash_mutex);
+   return CRYPT_OK;
+}
+
+/* $Source: /cvs/libtom/libtomcrypt/src/misc/crypt/crypt_hash_is_valid.c,v $ */
+/* $Revision: 1.6 $ */
+/* $Date: 2006/12/28 01:27:24 $ */
diff --git a/core/lib/libtomcrypt/src/misc/crypt/crypt_prng_descriptor.c b/core/lib/libtomcrypt/src/misc/crypt/crypt_prng_descriptor.c
new file mode 100644
index 0000000..17eb1bc
--- /dev/null
+++ b/core/lib/libtomcrypt/src/misc/crypt/crypt_prng_descriptor.c
@@ -0,0 +1,51 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file crypt_prng_descriptor.c
+  Stores the PRNG descriptors, Tom St Denis
+*/  
+const struct ltc_prng_descriptor *prng_descriptor[TAB_SIZE];
+
+LTC_MUTEX_GLOBAL(ltc_prng_mutex)
+
+
+/* $Source: /cvs/libtom/libtomcrypt/src/misc/crypt/crypt_prng_descriptor.c,v $ */
+/* $Revision: 1.8 $ */
+/* $Date: 2006/12/28 01:27:24 $ */
diff --git a/core/lib/libtomcrypt/src/misc/crypt/crypt_prng_is_valid.c b/core/lib/libtomcrypt/src/misc/crypt/crypt_prng_is_valid.c
new file mode 100644
index 0000000..65d1550
--- /dev/null
+++ b/core/lib/libtomcrypt/src/misc/crypt/crypt_prng_is_valid.c
@@ -0,0 +1,63 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file crypt_prng_is_valid.c
+  Determine if PRNG is valid, Tom St Denis
+*/
+
+/*
+   Test if a PRNG index is valid
+   @param idx   The index of the PRNG to search for
+   @return CRYPT_OK if valid
+*/
+int prng_is_valid(int idx)
+{
+   LTC_MUTEX_LOCK(&ltc_prng_mutex);
+   if (idx < 0 || idx >= TAB_SIZE || prng_descriptor[idx] == NULL) {
+      LTC_MUTEX_UNLOCK(&ltc_prng_mutex);
+      return CRYPT_INVALID_PRNG;
+   }
+   LTC_MUTEX_UNLOCK(&ltc_prng_mutex);
+   return CRYPT_OK;
+}
+
+/* $Source: /cvs/libtom/libtomcrypt/src/misc/crypt/crypt_prng_is_valid.c,v $ */
+/* $Revision: 1.6 $ */
+/* $Date: 2006/12/28 01:27:24 $ */
diff --git a/core/lib/libtomcrypt/src/misc/crypt/crypt_register_cipher.c b/core/lib/libtomcrypt/src/misc/crypt/crypt_register_cipher.c
new file mode 100644
index 0000000..27fcde1
--- /dev/null
+++ b/core/lib/libtomcrypt/src/misc/crypt/crypt_register_cipher.c
@@ -0,0 +1,81 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file crypt_register_cipher.c
+  Register a cipher, Tom St Denis
+*/
+
+/**
+   Register a cipher with the descriptor table
+   @param cipher   The cipher you wish to register
+   @return value >= 0 if successfully added (or already present), -1 if unsuccessful
+*/
+int register_cipher(const struct ltc_cipher_descriptor *cipher)
+{
+   int x;
+
+   LTC_ARGCHK(cipher != NULL);
+
+   /* is it already registered? */
+   LTC_MUTEX_LOCK(&ltc_cipher_mutex);
+   for (x = 0; x < TAB_SIZE; x++) {
+       if (cipher_descriptor[x] != NULL && cipher_descriptor[x]->ID == cipher->ID) {
+          LTC_MUTEX_UNLOCK(&ltc_cipher_mutex);
+          return x;
+       }
+   }
+
+   /* find a blank spot */
+   for (x = 0; x < TAB_SIZE; x++) {
+       if (cipher_descriptor[x] == NULL) {
+          cipher_descriptor[x] = cipher;
+          LTC_MUTEX_UNLOCK(&ltc_cipher_mutex);
+          return x;
+       }
+   }
+
+   /* no spot */
+   LTC_MUTEX_UNLOCK(&ltc_cipher_mutex);
+   return -1;
+}
+
+/* $Source: /cvs/libtom/libtomcrypt/src/misc/crypt/crypt_register_cipher.c,v $ */
+/* $Revision: 1.6 $ */
+/* $Date: 2006/12/28 01:27:24 $ */
diff --git a/core/lib/libtomcrypt/src/misc/crypt/crypt_register_hash.c b/core/lib/libtomcrypt/src/misc/crypt/crypt_register_hash.c
new file mode 100644
index 0000000..ad4bc64
--- /dev/null
+++ b/core/lib/libtomcrypt/src/misc/crypt/crypt_register_hash.c
@@ -0,0 +1,81 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file crypt_register_hash.c
+  Register a HASH, Tom St Denis
+*/
+
+/**
+   Register a hash with the descriptor table
+   @param hash   The hash you wish to register
+   @return value >= 0 if successfully added (or already present), -1 if unsuccessful
+*/
+int register_hash(const struct ltc_hash_descriptor *hash)
+{
+   int x;
+
+   LTC_ARGCHK(hash != NULL);
+
+   /* is it already registered? */
+   LTC_MUTEX_LOCK(&ltc_hash_mutex);
+   for (x = 0; x < TAB_SIZE; x++) {
+       if (hash_descriptor[x] == hash) {
+          LTC_MUTEX_UNLOCK(&ltc_hash_mutex);
+          return x;
+       }
+   }
+
+   /* find a blank spot */
+   for (x = 0; x < TAB_SIZE; x++) {
+       if (hash_descriptor[x] == NULL) {
+          hash_descriptor[x] = hash;
+          LTC_MUTEX_UNLOCK(&ltc_hash_mutex);
+          return x;
+       }
+   }
+
+   /* no spot */
+   LTC_MUTEX_UNLOCK(&ltc_hash_mutex);
+   return -1;
+}
+
+/* $Source: /cvs/libtom/libtomcrypt/src/misc/crypt/crypt_register_hash.c,v $ */
+/* $Revision: 1.7 $ */
+/* $Date: 2006/12/28 01:27:24 $ */
diff --git a/core/lib/libtomcrypt/src/misc/crypt/crypt_register_prng.c b/core/lib/libtomcrypt/src/misc/crypt/crypt_register_prng.c
new file mode 100644
index 0000000..16297ce
--- /dev/null
+++ b/core/lib/libtomcrypt/src/misc/crypt/crypt_register_prng.c
@@ -0,0 +1,81 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file crypt_register_prng.c
+  Register a PRNG, Tom St Denis
+*/
+  
+/**
+   Register a PRNG with the descriptor table
+   @param prng   The PRNG you wish to register
+   @return value >= 0 if successfully added (or already present), -1 if unsuccessful
+*/
+int register_prng(const struct ltc_prng_descriptor *prng)
+{
+   int x;
+
+   LTC_ARGCHK(prng != NULL);
+
+   /* is it already registered? */
+   LTC_MUTEX_LOCK(&ltc_prng_mutex);
+   for (x = 0; x < TAB_SIZE; x++) {
+       if (prng_descriptor[x] == prng) {
+          LTC_MUTEX_UNLOCK(&ltc_prng_mutex);
+          return x;
+       }
+   }
+
+   /* find a blank spot */
+   for (x = 0; x < TAB_SIZE; x++) {
+       if (prng_descriptor[x] == NULL) {
+	  prng_descriptor[x] = prng;
+          LTC_MUTEX_UNLOCK(&ltc_prng_mutex);
+          return x;
+       }
+   }
+
+   /* no spot */
+   LTC_MUTEX_UNLOCK(&ltc_prng_mutex);
+   return -1;
+}
+
+/* $Source: /cvs/libtom/libtomcrypt/src/misc/crypt/crypt_register_prng.c,v $ */
+/* $Revision: 1.8 $ */
+/* $Date: 2006/12/28 01:27:24 $ */
diff --git a/core/lib/libtomcrypt/src/misc/crypt/crypt_unregister_cipher.c b/core/lib/libtomcrypt/src/misc/crypt/crypt_unregister_cipher.c
new file mode 100644
index 0000000..111aceb
--- /dev/null
+++ b/core/lib/libtomcrypt/src/misc/crypt/crypt_unregister_cipher.c
@@ -0,0 +1,71 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file crypt_unregister_cipher.c
+  Unregister a cipher, Tom St Denis
+*/
+
+/**
+  Unregister a cipher from the descriptor table
+  @param cipher   The cipher descriptor to remove
+  @return CRYPT_OK on success
+*/
+int unregister_cipher(const struct ltc_cipher_descriptor *cipher)
+{
+   int x;
+
+   LTC_ARGCHK(cipher != NULL);
+
+   /* is it already registered? */
+   LTC_MUTEX_LOCK(&ltc_cipher_mutex);
+   for (x = 0; x < TAB_SIZE; x++) {
+       if (cipher_descriptor[x] == cipher) {
+          cipher_descriptor[x] = NULL;
+          LTC_MUTEX_UNLOCK(&ltc_cipher_mutex);
+          return CRYPT_OK;
+       }
+   }
+   LTC_MUTEX_UNLOCK(&ltc_cipher_mutex);
+   return CRYPT_ERROR;
+}
+
+/* $Source: /cvs/libtom/libtomcrypt/src/misc/crypt/crypt_unregister_cipher.c,v $ */
+/* $Revision: 1.7 $ */
+/* $Date: 2006/12/28 01:27:24 $ */
diff --git a/core/lib/libtomcrypt/src/misc/crypt/crypt_unregister_hash.c b/core/lib/libtomcrypt/src/misc/crypt/crypt_unregister_hash.c
new file mode 100644
index 0000000..77741b1
--- /dev/null
+++ b/core/lib/libtomcrypt/src/misc/crypt/crypt_unregister_hash.c
@@ -0,0 +1,71 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file crypt_unregister_hash.c
+  Unregister a hash, Tom St Denis
+*/
+
+/**
+  Unregister a hash from the descriptor table
+  @param hash   The hash descriptor to remove
+  @return CRYPT_OK on success
+*/
+int unregister_hash(const struct ltc_hash_descriptor *hash)
+{
+   int x;
+
+   LTC_ARGCHK(hash != NULL);
+
+   /* is it already registered? */
+   LTC_MUTEX_LOCK(&ltc_hash_mutex);
+   for (x = 0; x < TAB_SIZE; x++) {
+       if (hash_descriptor[x] == hash) {
+          hash_descriptor[x] = NULL;
+          LTC_MUTEX_UNLOCK(&ltc_hash_mutex);
+          return CRYPT_OK;
+       }
+   }
+   LTC_MUTEX_UNLOCK(&ltc_hash_mutex);
+   return CRYPT_ERROR;
+}
+
+/* $Source: /cvs/libtom/libtomcrypt/src/misc/crypt/crypt_unregister_hash.c,v $ */
+/* $Revision: 1.7 $ */
+/* $Date: 2006/12/28 01:27:24 $ */
diff --git a/core/lib/libtomcrypt/src/misc/crypt/crypt_unregister_prng.c b/core/lib/libtomcrypt/src/misc/crypt/crypt_unregister_prng.c
new file mode 100644
index 0000000..efa9da9
--- /dev/null
+++ b/core/lib/libtomcrypt/src/misc/crypt/crypt_unregister_prng.c
@@ -0,0 +1,71 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file crypt_unregister_prng.c
+  Unregister a PRNG, Tom St Denis
+*/
+
+/**
+  Unregister a PRNG from the descriptor table
+  @param prng   The PRNG descriptor to remove
+  @return CRYPT_OK on success
+*/
+int unregister_prng(const struct ltc_prng_descriptor *prng)
+{
+   int x;
+
+   LTC_ARGCHK(prng != NULL);
+ 
+   /* is it already registered? */
+   LTC_MUTEX_LOCK(&ltc_prng_mutex);
+   for (x = 0; x < TAB_SIZE; x++) {
+       if (prng_descriptor[x] == prng) {
+          prng_descriptor[x] = NULL;
+          LTC_MUTEX_UNLOCK(&ltc_prng_mutex);
+          return CRYPT_OK;
+       }
+   }
+   LTC_MUTEX_UNLOCK(&ltc_prng_mutex);
+   return CRYPT_ERROR;
+}
+
+/* $Source: /cvs/libtom/libtomcrypt/src/misc/crypt/crypt_unregister_prng.c,v $ */
+/* $Revision: 1.7 $ */
+/* $Date: 2006/12/28 01:27:24 $ */
diff --git a/core/lib/libtomcrypt/src/misc/crypt/sub.mk b/core/lib/libtomcrypt/src/misc/crypt/sub.mk
new file mode 100644
index 0000000..9880a9b
--- /dev/null
+++ b/core/lib/libtomcrypt/src/misc/crypt/sub.mk
@@ -0,0 +1,22 @@
+srcs-y += crypt.c
+srcs-y += crypt_cipher_descriptor.c
+srcs-y += crypt_cipher_is_valid.c
+srcs-y += crypt_find_cipher_any.c
+srcs-y += crypt_find_cipher.c
+srcs-y += crypt_find_cipher_id.c
+srcs-y += crypt_find_hash_any.c
+srcs-y += crypt_find_hash.c
+srcs-y += crypt_find_hash_id.c
+srcs-y += crypt_find_hash_oid.c
+srcs-y += crypt_find_prng.c
+srcs-y += crypt_fsa.c
+srcs-y += crypt_hash_descriptor.c
+srcs-y += crypt_hash_is_valid.c
+srcs-y += crypt_prng_descriptor.c
+srcs-y += crypt_prng_is_valid.c
+srcs-y += crypt_register_cipher.c
+srcs-y += crypt_register_hash.c
+srcs-y += crypt_register_prng.c
+srcs-y += crypt_unregister_cipher.c
+srcs-y += crypt_unregister_hash.c
+srcs-y += crypt_unregister_prng.c
diff --git a/core/lib/libtomcrypt/src/misc/error_to_string.c b/core/lib/libtomcrypt/src/misc/error_to_string.c
new file mode 100644
index 0000000..19b7311
--- /dev/null
+++ b/core/lib/libtomcrypt/src/misc/error_to_string.c
@@ -0,0 +1,104 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+
+#include "tomcrypt.h"
+
+/**
+  @file error_to_string.c
+  Convert error codes to ASCII strings, Tom St Denis
+*/
+
+static const char *err_2_str[] =
+{
+   "CRYPT_OK",
+   "CRYPT_ERROR",
+   "Non-fatal 'no-operation' requested.",
+
+   "Invalid keysize for block cipher.",
+   "Invalid number of rounds for block cipher.",
+   "Algorithm failed test vectors.",
+
+   "Buffer overflow.",
+   "Invalid input packet.",
+
+   "Invalid number of bits for a PRNG.",
+   "Error reading the PRNG.",
+
+   "Invalid cipher specified.",
+   "Invalid hash specified.",
+   "Invalid PRNG specified.",
+
+   "Out of memory.",
+
+   "Invalid PK key or key type specified for function.",
+   "A private PK key is required.",
+
+   "Invalid argument provided.",
+   "File Not Found",
+
+   "Invalid PK type.",
+   "Invalid PK system.",
+   "Duplicate PK key found on keyring.",
+   "Key not found in keyring.",
+   "Invalid sized parameter.",
+
+   "Invalid size for prime.",
+
+   "Invalid padding.",
+
+   "Hash applied to too many bits.",
+};
+
+/**
+   Convert an LTC error code to ASCII
+   @param err    The error code
+   @return A pointer to the ASCII NUL terminated string for the error or "Invalid error code." if the err code was not valid.
+*/
+const char *error_to_string(int err)
+{
+   if (err < 0 || err >= (int)(sizeof(err_2_str)/sizeof(err_2_str[0]))) {
+      return "Invalid error code.";
+   } else {
+      return err_2_str[err];
+   }   
+}
+
+
+/* $Source: /cvs/libtom/libtomcrypt/src/misc/error_to_string.c,v $ */
+/* $Revision: 1.5 $ */
+/* $Date: 2006/12/28 01:27:24 $ */
diff --git a/core/lib/libtomcrypt/src/misc/mem_neq.c b/core/lib/libtomcrypt/src/misc/mem_neq.c
new file mode 100644
index 0000000..9953388
--- /dev/null
+++ b/core/lib/libtomcrypt/src/misc/mem_neq.c
@@ -0,0 +1,87 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+   @file mem_neq.c
+   Compare two blocks of memory for inequality.
+   Steffen Jaeckel
+*/
+
+/**
+   Compare two blocks of memory for inequality.
+
+   The usage is similar to that of standard memcmp(), but you can only test
+   if the memory is equal or not - you can not determine by how much the
+   first different byte differs.
+
+   @param a     The first memory region
+   @param b     The second memory region
+   @param len   The length of the area to compare (octets)
+
+   @return 0 when a and b are equal for len bytes, else they are not equal.
+*/
+int mem_neq(const void *a, const void *b, size_t len)
+{
+   unsigned char ret = 0;
+   const unsigned char* pa;
+   const unsigned char* pb;
+
+   LTC_ARGCHK(a != NULL);
+   LTC_ARGCHK(b != NULL);
+
+   pa = a;
+   pb = b;
+
+   while (len-- > 0) {
+      ret |= *pa ^ *pb;
+      ++pa;
+      ++pb;
+   }
+
+   ret |= ret >> 4;
+   ret |= ret >> 2;
+   ret |= ret >> 1;
+   ret &= 1;
+
+   return ret;
+}
+
+/* $Source$ */
+/* $Revision$ */
+/* $Date$ */
diff --git a/core/lib/libtomcrypt/src/misc/pkcs5/pkcs_5_1.c b/core/lib/libtomcrypt/src/misc/pkcs5/pkcs_5_1.c
new file mode 100644
index 0000000..19d07ef
--- /dev/null
+++ b/core/lib/libtomcrypt/src/misc/pkcs5/pkcs_5_1.c
@@ -0,0 +1,133 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include <tomcrypt.h>
+
+/** 
+   @file pkcs_5_1.c
+   LTC_PKCS #5, Algorithm #1, Tom St Denis
+*/
+#ifdef LTC_PKCS_5
+/**
+   Execute LTC_PKCS #5 v1
+   @param password         The password (or key)
+   @param password_len     The length of the password (octet)
+   @param salt             The salt (or nonce) which is 8 octets long
+   @param iteration_count  The LTC_PKCS #5 v1 iteration count
+   @param hash_idx         The index of the hash desired
+   @param out              [out] The destination for this algorithm
+   @param outlen           [in/out] The max size and resulting size of the algorithm output
+   @return CRYPT_OK if successful
+*/
+int pkcs_5_alg1(const unsigned char *password, unsigned long password_len, 
+                const unsigned char *salt, 
+                int iteration_count,  int hash_idx,
+                unsigned char *out,   unsigned long *outlen)
+{
+   int err;
+   unsigned long x;
+   hash_state    *md;
+   unsigned char *buf;
+
+   LTC_ARGCHK(password != NULL);
+   LTC_ARGCHK(salt     != NULL);
+   LTC_ARGCHK(out      != NULL);
+   LTC_ARGCHK(outlen   != NULL);
+
+   /* test hash IDX */
+   if ((err = hash_is_valid(hash_idx)) != CRYPT_OK) {
+      return err;
+   }
+
+   /* allocate memory */
+   md  = XMALLOC(sizeof(hash_state));
+   buf = XMALLOC(MAXBLOCKSIZE);
+   if (md == NULL || buf == NULL) {
+      if (md != NULL) {
+         XFREE(md);
+      }
+      if (buf != NULL) { 
+         XFREE(buf);
+      }
+      return CRYPT_MEM;
+   }        
+
+   /* hash initial password + salt */
+   if ((err = hash_descriptor[hash_idx].init(md)) != CRYPT_OK) {
+       goto LBL_ERR;
+   }
+   if ((err = hash_descriptor[hash_idx].process(md, password, password_len)) != CRYPT_OK) {
+       goto LBL_ERR;
+   }
+   if ((err = hash_descriptor[hash_idx].process(md, salt, 8)) != CRYPT_OK) {
+       goto LBL_ERR;
+   }
+   if ((err = hash_descriptor[hash_idx].done(md, buf)) != CRYPT_OK) {
+       goto LBL_ERR;
+   }
+
+   while (--iteration_count) {
+      /* code goes here. */
+      x = MAXBLOCKSIZE;
+      if ((err = hash_memory(hash_idx, buf, hash_descriptor[hash_idx].hashsize, buf, &x)) != CRYPT_OK) {
+         goto LBL_ERR;
+      }
+   }
+
+   /* copy upto outlen bytes */
+   for (x = 0; x < hash_descriptor[hash_idx].hashsize && x < *outlen; x++) {
+       out[x] = buf[x];
+   }
+   *outlen = x;
+   err = CRYPT_OK;
+LBL_ERR:
+#ifdef LTC_CLEAN_STACK 
+   zeromem(buf, MAXBLOCKSIZE);
+   zeromem(md, sizeof(hash_state));
+#endif
+
+   XFREE(buf);
+   XFREE(md);
+
+   return err;
+}
+
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/misc/pkcs5/pkcs_5_1.c,v $ */
+/* $Revision: 1.7 $ */
+/* $Date: 2007/05/12 14:32:35 $ */
diff --git a/core/lib/libtomcrypt/src/misc/pkcs5/pkcs_5_2.c b/core/lib/libtomcrypt/src/misc/pkcs5/pkcs_5_2.c
new file mode 100644
index 0000000..b969798
--- /dev/null
+++ b/core/lib/libtomcrypt/src/misc/pkcs5/pkcs_5_2.c
@@ -0,0 +1,156 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include <tomcrypt.h>
+
+/** 
+   @file pkcs_5_2.c
+   LTC_PKCS #5, Algorithm #2, Tom St Denis
+*/
+#ifdef LTC_PKCS_5
+
+/**
+   Execute LTC_PKCS #5 v2
+   @param password          The input password (or key)
+   @param password_len      The length of the password (octets)
+   @param salt              The salt (or nonce)
+   @param salt_len          The length of the salt (octets)
+   @param iteration_count   # of iterations desired for LTC_PKCS #5 v2 [read specs for more]
+   @param hash_idx          The index of the hash desired
+   @param out               [out] The destination for this algorithm
+   @param outlen            [in/out] The max size and resulting size of the algorithm output
+   @return CRYPT_OK if successful
+*/
+int pkcs_5_alg2(const unsigned char *password, unsigned long password_len, 
+                const unsigned char *salt,     unsigned long salt_len,
+                int iteration_count,           int hash_idx,
+                unsigned char *out,            unsigned long *outlen)
+{
+   int err, itts;
+   ulong32  blkno;
+   unsigned long stored, left, x, y;
+   unsigned char *buf[2];
+   hmac_state    *hmac;
+
+   LTC_ARGCHK(password != NULL);
+   LTC_ARGCHK(salt     != NULL);
+   LTC_ARGCHK(out      != NULL);
+   LTC_ARGCHK(outlen   != NULL);
+
+   /* test hash IDX */
+   if ((err = hash_is_valid(hash_idx)) != CRYPT_OK) {
+      return err;
+   }
+
+   buf[0] = XMALLOC(MAXBLOCKSIZE * 2);
+   hmac   = XMALLOC(sizeof(hmac_state));
+   if (hmac == NULL || buf[0] == NULL) {
+      if (hmac != NULL) {
+         XFREE(hmac);
+      }
+      if (buf[0] != NULL) {
+         XFREE(buf[0]);
+      }
+      return CRYPT_MEM;
+   }
+   /* buf[1] points to the second block of MAXBLOCKSIZE bytes */
+   buf[1] = buf[0] + MAXBLOCKSIZE;
+
+   left   = *outlen;
+   blkno  = 1;
+   stored = 0;
+   while (left != 0) {
+       /* process block number blkno */
+       zeromem(buf[0], MAXBLOCKSIZE*2);
+       
+       /* store current block number and increment for next pass */
+       STORE32H(blkno, buf[1]);
+       ++blkno;
+
+       /* get PRF(P, S||int(blkno)) */
+       if ((err = hmac_init(hmac, hash_idx, password, password_len)) != CRYPT_OK) { 
+          goto LBL_ERR;
+       }
+       if ((err = hmac_process(hmac, salt, salt_len)) != CRYPT_OK) {
+          goto LBL_ERR;
+       }
+       if ((err = hmac_process(hmac, buf[1], 4)) != CRYPT_OK) {
+          goto LBL_ERR;
+       }
+       x = MAXBLOCKSIZE;
+       if ((err = hmac_done(hmac, buf[0], &x)) != CRYPT_OK) {
+          goto LBL_ERR;
+       }
+
+       /* now compute repeated and XOR it in buf[1] */
+       XMEMCPY(buf[1], buf[0], x);
+       for (itts = 1; itts < iteration_count; ++itts) {
+           if ((err = hmac_memory(hash_idx, password, password_len, buf[0], x, buf[0], &x)) != CRYPT_OK) {
+              goto LBL_ERR;
+           }
+           for (y = 0; y < x; y++) {
+               buf[1][y] ^= buf[0][y];
+           }
+       }
+
+       /* now emit upto x bytes of buf[1] to output */
+       for (y = 0; y < x && left != 0; ++y) {
+           out[stored++] = buf[1][y];
+           --left;
+       }
+   }
+   *outlen = stored;
+
+   err = CRYPT_OK;
+LBL_ERR:
+#ifdef LTC_CLEAN_STACK
+   zeromem(buf[0], MAXBLOCKSIZE*2);
+   zeromem(hmac, sizeof(hmac_state));
+#endif
+
+   XFREE(hmac);
+   XFREE(buf[0]);
+
+   return err;
+}
+
+#endif
+
+
+/* $Source: /cvs/libtom/libtomcrypt/src/misc/pkcs5/pkcs_5_2.c,v $ */
+/* $Revision: 1.7 $ */
+/* $Date: 2007/05/12 14:32:35 $ */
diff --git a/core/lib/libtomcrypt/src/misc/pkcs5/sub.mk b/core/lib/libtomcrypt/src/misc/pkcs5/sub.mk
new file mode 100644
index 0000000..7f8ccfe
--- /dev/null
+++ b/core/lib/libtomcrypt/src/misc/pkcs5/sub.mk
@@ -0,0 +1,2 @@
+srcs-y += pkcs_5_1.c
+srcs-y += pkcs_5_2.c
diff --git a/core/lib/libtomcrypt/src/misc/sub.mk b/core/lib/libtomcrypt/src/misc/sub.mk
new file mode 100644
index 0000000..6b60213
--- /dev/null
+++ b/core/lib/libtomcrypt/src/misc/sub.mk
@@ -0,0 +1,6 @@
+srcs-y += burn_stack.c
+srcs-y += error_to_string.c
+srcs-y += zeromem.c
+subdirs-y += base64
+subdirs-y += crypt
+subdirs-y += pkcs5
diff --git a/core/lib/libtomcrypt/src/misc/zeromem.c b/core/lib/libtomcrypt/src/misc/zeromem.c
new file mode 100644
index 0000000..2afe0dd
--- /dev/null
+++ b/core/lib/libtomcrypt/src/misc/zeromem.c
@@ -0,0 +1,61 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+   @file zeromem.c
+   Zero a block of memory, Tom St Denis
+*/
+
+/**
+   Zero a block of memory
+   @param out    The destination of the area to zero
+   @param outlen The length of the area to zero (octets)
+*/
+void zeromem(volatile void *out, size_t outlen)
+{
+   volatile char *mem = out;
+   LTC_ARGCHKVD(out != NULL);
+   while (outlen-- > 0) {
+      *mem++ = 0;
+   }
+}
+
+/* $Source: /cvs/libtom/libtomcrypt/src/misc/zeromem.c,v $ */
+/* $Revision: 1.7 $ */
+/* $Date: 2006/12/28 01:27:24 $ */
diff --git a/core/lib/libtomcrypt/src/modes/cbc/cbc_decrypt.c b/core/lib/libtomcrypt/src/modes/cbc/cbc_decrypt.c
new file mode 100644
index 0000000..8ab2542
--- /dev/null
+++ b/core/lib/libtomcrypt/src/modes/cbc/cbc_decrypt.c
@@ -0,0 +1,124 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+   @file cbc_decrypt.c
+   CBC implementation, encrypt block, Tom St Denis
+*/
+
+
+#ifdef LTC_CBC_MODE
+
+/**
+  CBC decrypt
+  @param ct     Ciphertext
+  @param pt     [out] Plaintext
+  @param len    The number of bytes to process (must be multiple of block length)
+  @param cbc    CBC state
+  @return CRYPT_OK if successful
+*/
+int cbc_decrypt(const unsigned char *ct, unsigned char *pt, unsigned long len, symmetric_CBC *cbc)
+{
+   int x, err;
+   unsigned char tmp[16];
+#ifdef LTC_FAST
+   LTC_FAST_TYPE tmpy;
+#else
+   unsigned char tmpy;
+#endif         
+
+   LTC_ARGCHK(pt  != NULL);
+   LTC_ARGCHK(ct  != NULL);
+   LTC_ARGCHK(cbc != NULL);
+
+   if ((err = cipher_is_valid(cbc->cipher)) != CRYPT_OK) {
+       return err;
+   }
+   
+   /* is blocklen valid? */
+   if (cbc->blocklen < 1 || cbc->blocklen > (int)sizeof(cbc->IV)) {
+      return CRYPT_INVALID_ARG;
+   }    
+
+   if (len % cbc->blocklen) {
+      return CRYPT_INVALID_ARG;
+   }
+#ifdef LTC_FAST
+   if (cbc->blocklen % sizeof(LTC_FAST_TYPE)) {   
+      return CRYPT_INVALID_ARG;
+   }
+#endif
+   
+   if (cipher_descriptor[cbc->cipher]->accel_cbc_decrypt != NULL) {
+      return cipher_descriptor[cbc->cipher]->accel_cbc_decrypt(ct, pt, len / cbc->blocklen, cbc->IV, &cbc->key);
+   } else {
+      while (len) {
+         /* decrypt */
+         if ((err = cipher_descriptor[cbc->cipher]->ecb_decrypt(ct, tmp, &cbc->key)) != CRYPT_OK) {
+            return err;
+         }
+
+         /* xor IV against plaintext */
+         #if defined(LTC_FAST)
+        for (x = 0; x < cbc->blocklen; x += sizeof(LTC_FAST_TYPE)) {
+            tmpy = *((LTC_FAST_TYPE*)((unsigned char *)cbc->IV + x)) ^ *((LTC_FAST_TYPE*)((unsigned char *)tmp + x));
+       *((LTC_FAST_TYPE*)((unsigned char *)cbc->IV + x)) = *((LTC_FAST_TYPE*)((unsigned char *)ct + x));
+       *((LTC_FAST_TYPE*)((unsigned char *)pt + x)) = tmpy;
+        }
+    #else 
+            for (x = 0; x < cbc->blocklen; x++) {
+               tmpy       = tmp[x] ^ cbc->IV[x];
+               cbc->IV[x] = ct[x];
+               pt[x]      = tmpy;
+            }
+    #endif
+       
+         ct  += cbc->blocklen;
+         pt  += cbc->blocklen;
+         len -= cbc->blocklen;
+      }
+   }
+   return CRYPT_OK;
+}
+
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/modes/cbc/cbc_decrypt.c,v $ */
+/* $Revision: 1.16 $ */
+/* $Date: 2006/12/28 01:27:24 $ */
diff --git a/core/lib/libtomcrypt/src/modes/cbc/cbc_done.c b/core/lib/libtomcrypt/src/modes/cbc/cbc_done.c
new file mode 100644
index 0000000..a2664c1
--- /dev/null
+++ b/core/lib/libtomcrypt/src/modes/cbc/cbc_done.c
@@ -0,0 +1,69 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+   @file cbc_done.c
+   CBC implementation, finish chain, Tom St Denis
+*/
+
+#ifdef LTC_CBC_MODE
+
+/** Terminate the chain
+  @param cbc    The CBC chain to terminate
+  @return CRYPT_OK on success
+*/
+int cbc_done(symmetric_CBC *cbc)
+{
+   int err;
+   LTC_ARGCHK(cbc != NULL);
+
+   if ((err = cipher_is_valid(cbc->cipher)) != CRYPT_OK) {
+      return err;
+   }
+   cipher_descriptor[cbc->cipher]->done(&cbc->key);
+   return CRYPT_OK;
+}
+
+   
+
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/modes/cbc/cbc_done.c,v $ */
+/* $Revision: 1.7 $ */
+/* $Date: 2006/12/28 01:27:24 $ */
diff --git a/core/lib/libtomcrypt/src/modes/cbc/cbc_encrypt.c b/core/lib/libtomcrypt/src/modes/cbc/cbc_encrypt.c
new file mode 100644
index 0000000..2b138b4
--- /dev/null
+++ b/core/lib/libtomcrypt/src/modes/cbc/cbc_encrypt.c
@@ -0,0 +1,125 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+   @file cbc_encrypt.c
+   CBC implementation, encrypt block, Tom St Denis
+*/
+
+
+#ifdef LTC_CBC_MODE
+
+/**
+  CBC encrypt
+  @param pt     Plaintext
+  @param ct     [out] Ciphertext
+  @param len    The number of bytes to process (must be multiple of block length)
+  @param cbc    CBC state
+  @return CRYPT_OK if successful
+*/
+int cbc_encrypt(const unsigned char *pt, unsigned char *ct, unsigned long len, symmetric_CBC *cbc)
+{
+   int x, err;
+
+   LTC_ARGCHK(pt != NULL);
+   LTC_ARGCHK(ct != NULL);
+   LTC_ARGCHK(cbc != NULL);
+
+   if ((err = cipher_is_valid(cbc->cipher)) != CRYPT_OK) {
+       return err;
+   }
+   
+   /* is blocklen valid? */
+   if (cbc->blocklen < 1 || cbc->blocklen > (int)sizeof(cbc->IV)) {
+      return CRYPT_INVALID_ARG;
+   }    
+
+   if (len % cbc->blocklen) {
+      return CRYPT_INVALID_ARG;
+   }
+#ifdef LTC_FAST
+   if (cbc->blocklen % sizeof(LTC_FAST_TYPE)) {   
+      return CRYPT_INVALID_ARG;
+   }
+#endif
+
+   if (cipher_descriptor[cbc->cipher]->accel_cbc_encrypt != NULL) {
+      return cipher_descriptor[cbc->cipher]->accel_cbc_encrypt(pt, ct, len / cbc->blocklen, cbc->IV, &cbc->key);
+   } else {
+      while (len) {
+         /* xor IV against plaintext */
+         #if defined(LTC_FAST)
+        for (x = 0; x < cbc->blocklen; x += sizeof(LTC_FAST_TYPE)) {
+            *((LTC_FAST_TYPE*)((unsigned char *)cbc->IV + x)) ^= *((LTC_FAST_TYPE*)((unsigned char *)pt + x));
+        }
+    #else 
+            for (x = 0; x < cbc->blocklen; x++) {
+               cbc->IV[x] ^= pt[x];
+            }
+    #endif
+
+         /* encrypt */
+         if ((err = cipher_descriptor[cbc->cipher]->ecb_encrypt(cbc->IV, ct, &cbc->key)) != CRYPT_OK) {
+            return err;
+         }
+
+        /* store IV [ciphertext] for a future block */
+         #if defined(LTC_FAST)
+        for (x = 0; x < cbc->blocklen; x += sizeof(LTC_FAST_TYPE)) {
+            *((LTC_FAST_TYPE*)((unsigned char *)cbc->IV + x)) = *((LTC_FAST_TYPE*)((unsigned char *)ct + x));
+        }
+    #else 
+             for (x = 0; x < cbc->blocklen; x++) {
+                cbc->IV[x] = ct[x];
+             }
+    #endif
+        
+        ct  += cbc->blocklen;
+        pt  += cbc->blocklen;
+        len -= cbc->blocklen;
+     }
+   }
+   return CRYPT_OK;
+}
+
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/modes/cbc/cbc_encrypt.c,v $ */
+/* $Revision: 1.14 $ */
+/* $Date: 2006/12/28 01:27:24 $ */
diff --git a/core/lib/libtomcrypt/src/modes/cbc/cbc_getiv.c b/core/lib/libtomcrypt/src/modes/cbc/cbc_getiv.c
new file mode 100644
index 0000000..799ac96
--- /dev/null
+++ b/core/lib/libtomcrypt/src/modes/cbc/cbc_getiv.c
@@ -0,0 +1,73 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+   @file cbc_getiv.c
+   CBC implementation, get IV, Tom St Denis
+*/
+
+#ifdef LTC_CBC_MODE
+
+/**
+   Get the current initial vector
+   @param IV   [out] The destination of the initial vector
+   @param len  [in/out]  The max size and resulting size of the initial vector
+   @param cbc  The CBC state
+   @return CRYPT_OK if successful
+*/
+int cbc_getiv(unsigned char *IV, unsigned long *len, symmetric_CBC *cbc)
+{
+   LTC_ARGCHK(IV  != NULL);
+   LTC_ARGCHK(len != NULL);
+   LTC_ARGCHK(cbc != NULL);
+   if ((unsigned long)cbc->blocklen > *len) {
+      *len = cbc->blocklen;
+      return CRYPT_BUFFER_OVERFLOW;
+   }
+   XMEMCPY(IV, cbc->IV, cbc->blocklen);
+   *len = cbc->blocklen;
+
+   return CRYPT_OK;
+}
+
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/modes/cbc/cbc_getiv.c,v $ */
+/* $Revision: 1.7 $ */
+/* $Date: 2006/12/28 01:27:24 $ */
diff --git a/core/lib/libtomcrypt/src/modes/cbc/cbc_setiv.c b/core/lib/libtomcrypt/src/modes/cbc/cbc_setiv.c
new file mode 100644
index 0000000..05e3ee9
--- /dev/null
+++ b/core/lib/libtomcrypt/src/modes/cbc/cbc_setiv.c
@@ -0,0 +1,71 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+   @file cbc_setiv.c
+   CBC implementation, set IV, Tom St Denis
+*/
+
+
+#ifdef LTC_CBC_MODE
+
+/**
+   Set an initial vector
+   @param IV   The initial vector
+   @param len  The length of the vector (in octets)
+   @param cbc  The CBC state
+   @return CRYPT_OK if successful
+*/
+int cbc_setiv(const unsigned char *IV, unsigned long len, symmetric_CBC *cbc)
+{
+   LTC_ARGCHK(IV  != NULL);
+   LTC_ARGCHK(cbc != NULL);
+   if (len != (unsigned long)cbc->blocklen) {
+      return CRYPT_INVALID_ARG;
+   }
+   XMEMCPY(cbc->IV, IV, len);
+   return CRYPT_OK;
+}
+
+#endif 
+
+
+/* $Source: /cvs/libtom/libtomcrypt/src/modes/cbc/cbc_setiv.c,v $ */
+/* $Revision: 1.6 $ */
+/* $Date: 2006/12/28 01:27:24 $ */
diff --git a/core/lib/libtomcrypt/src/modes/cbc/cbc_start.c b/core/lib/libtomcrypt/src/modes/cbc/cbc_start.c
new file mode 100644
index 0000000..f0c0218
--- /dev/null
+++ b/core/lib/libtomcrypt/src/modes/cbc/cbc_start.c
@@ -0,0 +1,89 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+   @file cbc_start.c
+   CBC implementation, start chain, Tom St Denis
+*/
+
+#ifdef LTC_CBC_MODE
+
+/**
+   Initialize a CBC context
+   @param cipher      The index of the cipher desired
+   @param IV          The initial vector
+   @param key         The secret key 
+   @param keylen      The length of the secret key (octets)
+   @param num_rounds  Number of rounds in the cipher desired (0 for default)
+   @param cbc         The CBC state to initialize
+   @return CRYPT_OK if successful
+*/
+int cbc_start(int cipher, const unsigned char *IV, const unsigned char *key, 
+              int keylen, int num_rounds, symmetric_CBC *cbc)
+{
+   int x, err;
+ 
+   LTC_ARGCHK(IV != NULL);
+   LTC_ARGCHK(key != NULL);
+   LTC_ARGCHK(cbc != NULL);
+
+   /* bad param? */
+   if ((err = cipher_is_valid(cipher)) != CRYPT_OK) {
+      return err;
+   }
+
+   /* setup cipher */
+   if ((err = cipher_descriptor[cipher]->setup(key, keylen, num_rounds, &cbc->key)) != CRYPT_OK) {
+      return err;
+   }
+
+   /* copy IV */
+   cbc->blocklen = cipher_descriptor[cipher]->block_length;
+   cbc->cipher   = cipher;
+   for (x = 0; x < cbc->blocklen; x++) {
+       cbc->IV[x] = IV[x];
+   }
+   return CRYPT_OK;
+}
+
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/modes/cbc/cbc_start.c,v $ */
+/* $Revision: 1.6 $ */
+/* $Date: 2006/12/28 01:27:24 $ */
diff --git a/core/lib/libtomcrypt/src/modes/cbc/sub.mk b/core/lib/libtomcrypt/src/modes/cbc/sub.mk
new file mode 100644
index 0000000..1ce3e77
--- /dev/null
+++ b/core/lib/libtomcrypt/src/modes/cbc/sub.mk
@@ -0,0 +1,6 @@
+srcs-y += cbc_decrypt.c
+srcs-y += cbc_done.c
+srcs-y += cbc_encrypt.c
+srcs-y += cbc_getiv.c
+srcs-y += cbc_setiv.c
+srcs-y += cbc_start.c
diff --git a/core/lib/libtomcrypt/src/modes/cfb/cfb_decrypt.c b/core/lib/libtomcrypt/src/modes/cfb/cfb_decrypt.c
new file mode 100644
index 0000000..7ca938f
--- /dev/null
+++ b/core/lib/libtomcrypt/src/modes/cfb/cfb_decrypt.c
@@ -0,0 +1,94 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file cfb_decrypt.c
+  CFB implementation, decrypt data, Tom St Denis
+*/
+
+#ifdef LTC_CFB_MODE
+
+/**
+   CFB decrypt
+   @param ct      Ciphertext
+   @param pt      [out] Plaintext
+   @param len     Length of ciphertext (octets)
+   @param cfb     CFB state
+   @return CRYPT_OK if successful
+*/
+int cfb_decrypt(const unsigned char *ct, unsigned char *pt, unsigned long len, symmetric_CFB *cfb)
+{
+   int err;
+
+   LTC_ARGCHK(pt != NULL);
+   LTC_ARGCHK(ct != NULL);
+   LTC_ARGCHK(cfb != NULL);
+
+   if ((err = cipher_is_valid(cfb->cipher)) != CRYPT_OK) {
+       return err;
+   }
+
+   /* is blocklen/padlen valid? */
+   if (cfb->blocklen < 0 || cfb->blocklen > (int)sizeof(cfb->IV) ||
+       cfb->padlen   < 0 || cfb->padlen   > (int)sizeof(cfb->pad)) {
+      return CRYPT_INVALID_ARG;
+   }
+
+   while (len-- > 0) {
+       if (cfb->padlen == cfb->blocklen) {
+          if ((err = cipher_descriptor[cfb->cipher].ecb_encrypt(cfb->pad, cfb->IV, &cfb->key)) != CRYPT_OK) {
+             return err;
+          }
+          cfb->padlen = 0;
+       }
+       cfb->pad[cfb->padlen] = *ct;
+       *pt = *ct ^ cfb->IV[cfb->padlen];
+       ++pt; 
+       ++ct;
+       ++(cfb->padlen);
+   }
+   return CRYPT_OK;
+}
+
+#endif
+
+
+/* $Source: /cvs/libtom/libtomcrypt/src/modes/cfb/cfb_decrypt.c,v $ */
+/* $Revision: 1.8 $ */
+/* $Date: 2006/12/28 01:27:24 $ */
diff --git a/core/lib/libtomcrypt/src/modes/cfb/cfb_done.c b/core/lib/libtomcrypt/src/modes/cfb/cfb_done.c
new file mode 100644
index 0000000..9f84727
--- /dev/null
+++ b/core/lib/libtomcrypt/src/modes/cfb/cfb_done.c
@@ -0,0 +1,69 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+   @file cfb_done.c
+   CFB implementation, finish chain, Tom St Denis
+*/
+
+#ifdef LTC_CFB_MODE
+
+/** Terminate the chain
+  @param cfb    The CFB chain to terminate
+  @return CRYPT_OK on success
+*/
+int cfb_done(symmetric_CFB *cfb)
+{
+   int err;
+   LTC_ARGCHK(cfb != NULL);
+
+   if ((err = cipher_is_valid(cfb->cipher)) != CRYPT_OK) {
+      return err;
+   }
+   cipher_descriptor[cfb->cipher].done(&cfb->key);
+   return CRYPT_OK;
+}
+
+   
+
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/modes/cfb/cfb_done.c,v $ */
+/* $Revision: 1.7 $ */
+/* $Date: 2006/12/28 01:27:24 $ */
diff --git a/core/lib/libtomcrypt/src/modes/cfb/cfb_encrypt.c b/core/lib/libtomcrypt/src/modes/cfb/cfb_encrypt.c
new file mode 100644
index 0000000..4acb8f1
--- /dev/null
+++ b/core/lib/libtomcrypt/src/modes/cfb/cfb_encrypt.c
@@ -0,0 +1,92 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file cfb_encrypt.c
+  CFB implementation, encrypt data, Tom St Denis
+*/
+
+#ifdef LTC_CFB_MODE
+
+/**
+  CFB encrypt
+  @param pt     Plaintext
+  @param ct     [out] Ciphertext
+  @param len    Length of plaintext (octets)
+  @param cfb    CFB state
+  @return CRYPT_OK if successful
+*/
+int cfb_encrypt(const unsigned char *pt, unsigned char *ct, unsigned long len, symmetric_CFB *cfb)
+{
+   int err;
+
+   LTC_ARGCHK(pt != NULL);
+   LTC_ARGCHK(ct != NULL);
+   LTC_ARGCHK(cfb != NULL);
+
+   if ((err = cipher_is_valid(cfb->cipher)) != CRYPT_OK) {
+       return err;
+   }
+
+   /* is blocklen/padlen valid? */
+   if (cfb->blocklen < 0 || cfb->blocklen > (int)sizeof(cfb->IV) ||
+       cfb->padlen   < 0 || cfb->padlen   > (int)sizeof(cfb->pad)) {
+      return CRYPT_INVALID_ARG;
+   }
+
+   while (len-- > 0) {
+       if (cfb->padlen == cfb->blocklen) {
+          if ((err = cipher_descriptor[cfb->cipher].ecb_encrypt(cfb->pad, cfb->IV, &cfb->key)) != CRYPT_OK) {
+             return err;
+          }
+          cfb->padlen = 0;
+       }
+       cfb->pad[cfb->padlen] = (*ct = *pt ^ cfb->IV[cfb->padlen]);
+       ++pt; 
+       ++ct;
+       ++(cfb->padlen);
+   }
+   return CRYPT_OK;
+}
+
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/modes/cfb/cfb_encrypt.c,v $ */
+/* $Revision: 1.8 $ */
+/* $Date: 2006/12/28 01:27:24 $ */
diff --git a/core/lib/libtomcrypt/src/modes/cfb/cfb_getiv.c b/core/lib/libtomcrypt/src/modes/cfb/cfb_getiv.c
new file mode 100644
index 0000000..0adb1f6
--- /dev/null
+++ b/core/lib/libtomcrypt/src/modes/cfb/cfb_getiv.c
@@ -0,0 +1,73 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+   @file cfb_getiv.c
+   CFB implementation, get IV, Tom St Denis
+*/
+
+#ifdef LTC_CFB_MODE
+
+/**
+   Get the current initial vector
+   @param IV   [out] The destination of the initial vector
+   @param len  [in/out]  The max size and resulting size of the initial vector
+   @param cfb  The CFB state
+   @return CRYPT_OK if successful
+*/
+int cfb_getiv(unsigned char *IV, unsigned long *len, symmetric_CFB *cfb)
+{
+   LTC_ARGCHK(IV  != NULL);
+   LTC_ARGCHK(len != NULL);
+   LTC_ARGCHK(cfb != NULL);
+   if ((unsigned long)cfb->blocklen > *len) {
+      *len = cfb->blocklen;
+      return CRYPT_BUFFER_OVERFLOW;
+   }
+   XMEMCPY(IV, cfb->IV, cfb->blocklen);
+   *len = cfb->blocklen;
+
+   return CRYPT_OK;
+}
+
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/modes/cfb/cfb_getiv.c,v $ */
+/* $Revision: 1.7 $ */
+/* $Date: 2006/12/28 01:27:24 $ */
diff --git a/core/lib/libtomcrypt/src/modes/cfb/cfb_setiv.c b/core/lib/libtomcrypt/src/modes/cfb/cfb_setiv.c
new file mode 100644
index 0000000..26cc81b
--- /dev/null
+++ b/core/lib/libtomcrypt/src/modes/cfb/cfb_setiv.c
@@ -0,0 +1,79 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file cfb_setiv.c
+  CFB implementation, set IV, Tom St Denis
+*/  
+
+#ifdef LTC_CFB_MODE
+
+/**
+   Set an initial vector
+   @param IV   The initial vector
+   @param len  The length of the vector (in octets)
+   @param cfb  The CFB state
+   @return CRYPT_OK if successful
+*/
+int cfb_setiv(const unsigned char *IV, unsigned long len, symmetric_CFB *cfb)
+{
+   int err;
+   
+   LTC_ARGCHK(IV  != NULL);
+   LTC_ARGCHK(cfb != NULL);
+
+   if ((err = cipher_is_valid(cfb->cipher)) != CRYPT_OK) {
+       return err;
+   }
+   
+   if (len != (unsigned long)cfb->blocklen) {
+      return CRYPT_INVALID_ARG;
+   }
+      
+   /* force next block */
+   cfb->padlen = 0;
+   return cipher_descriptor[cfb->cipher].ecb_encrypt(IV, cfb->IV, &cfb->key);
+}
+
+#endif 
+
+
+/* $Source: /cvs/libtom/libtomcrypt/src/modes/cfb/cfb_setiv.c,v $ */
+/* $Revision: 1.7 $ */
+/* $Date: 2006/12/28 01:27:24 $ */
diff --git a/core/lib/libtomcrypt/src/modes/cfb/cfb_start.c b/core/lib/libtomcrypt/src/modes/cfb/cfb_start.c
new file mode 100644
index 0000000..3a691ec
--- /dev/null
+++ b/core/lib/libtomcrypt/src/modes/cfb/cfb_start.c
@@ -0,0 +1,92 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+   @file cfb_start.c
+   CFB implementation, start chain, Tom St Denis
+*/
+
+
+#ifdef LTC_CFB_MODE
+
+/**
+   Initialize a CFB context
+   @param cipher      The index of the cipher desired
+   @param IV          The initial vector
+   @param key         The secret key 
+   @param keylen      The length of the secret key (octets)
+   @param num_rounds  Number of rounds in the cipher desired (0 for default)
+   @param cfb         The CFB state to initialize
+   @return CRYPT_OK if successful
+*/
+int cfb_start(int cipher, const unsigned char *IV, const unsigned char *key, 
+              int keylen, int num_rounds, symmetric_CFB *cfb)
+{
+   int x, err;
+
+   LTC_ARGCHK(IV != NULL);
+   LTC_ARGCHK(key != NULL);
+   LTC_ARGCHK(cfb != NULL);
+
+   if ((err = cipher_is_valid(cipher)) != CRYPT_OK) {
+      return err;
+   }
+   
+
+   /* copy data */
+   cfb->cipher = cipher;
+   cfb->blocklen = cipher_descriptor[cipher].block_length;
+   for (x = 0; x < cfb->blocklen; x++)
+       cfb->IV[x] = IV[x];
+
+   /* init the cipher */
+   if ((err = cipher_descriptor[cipher].setup(key, keylen, num_rounds, &cfb->key)) != CRYPT_OK) {
+      return err;
+   }
+
+   /* encrypt the IV */
+   cfb->padlen = 0;
+   return cipher_descriptor[cfb->cipher].ecb_encrypt(cfb->IV, cfb->IV, &cfb->key);
+}
+
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/modes/cfb/cfb_start.c,v $ */
+/* $Revision: 1.7 $ */
+/* $Date: 2006/12/28 01:27:24 $ */
diff --git a/core/lib/libtomcrypt/src/modes/cfb/sub.mk b/core/lib/libtomcrypt/src/modes/cfb/sub.mk
new file mode 100644
index 0000000..7a92b01
--- /dev/null
+++ b/core/lib/libtomcrypt/src/modes/cfb/sub.mk
@@ -0,0 +1,6 @@
+srcs-y += cfb_decrypt.c
+srcs-y += cfb_done.c
+srcs-y += cfb_encrypt.c
+srcs-y += cfb_getiv.c
+srcs-y += cfb_setiv.c
+srcs-y += cfb_start.c
diff --git a/core/lib/libtomcrypt/src/modes/ctr/ctr_decrypt.c b/core/lib/libtomcrypt/src/modes/ctr/ctr_decrypt.c
new file mode 100644
index 0000000..834df8a
--- /dev/null
+++ b/core/lib/libtomcrypt/src/modes/ctr/ctr_decrypt.c
@@ -0,0 +1,69 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file ctr_decrypt.c
+  CTR implementation, decrypt data, Tom St Denis
+*/
+
+#ifdef LTC_CTR_MODE
+
+/**
+   CTR decrypt
+   @param ct      Ciphertext
+   @param pt      [out] Plaintext
+   @param len     Length of ciphertext (octets)
+   @param ctr     CTR state
+   @return CRYPT_OK if successful
+*/
+int ctr_decrypt(const unsigned char *ct, unsigned char *pt, unsigned long len, symmetric_CTR *ctr)
+{
+   LTC_ARGCHK(pt != NULL);
+   LTC_ARGCHK(ct != NULL);
+   LTC_ARGCHK(ctr != NULL);
+
+   return ctr_encrypt(ct, pt, len, ctr);
+}
+
+#endif
+
+
+/* $Source: /cvs/libtom/libtomcrypt/src/modes/ctr/ctr_decrypt.c,v $ */
+/* $Revision: 1.6 $ */
+/* $Date: 2006/12/28 01:27:24 $ */
diff --git a/core/lib/libtomcrypt/src/modes/ctr/ctr_done.c b/core/lib/libtomcrypt/src/modes/ctr/ctr_done.c
new file mode 100644
index 0000000..9b475f5
--- /dev/null
+++ b/core/lib/libtomcrypt/src/modes/ctr/ctr_done.c
@@ -0,0 +1,69 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+   @file ctr_done.c
+   CTR implementation, finish chain, Tom St Denis
+*/
+
+#ifdef LTC_CTR_MODE
+
+/** Terminate the chain
+  @param ctr    The CTR chain to terminate
+  @return CRYPT_OK on success
+*/
+int ctr_done(symmetric_CTR *ctr)
+{
+   int err;
+   LTC_ARGCHK(ctr != NULL);
+
+   if ((err = cipher_is_valid(ctr->cipher)) != CRYPT_OK) {
+      return err;
+   }
+   cipher_descriptor[ctr->cipher]->done(&ctr->key);
+   return CRYPT_OK;
+}
+
+   
+
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/modes/ctr/ctr_done.c,v $ */
+/* $Revision: 1.7 $ */
+/* $Date: 2006/12/28 01:27:24 $ */
diff --git a/core/lib/libtomcrypt/src/modes/ctr/ctr_encrypt.c b/core/lib/libtomcrypt/src/modes/ctr/ctr_encrypt.c
new file mode 100644
index 0000000..0c6b2d7
--- /dev/null
+++ b/core/lib/libtomcrypt/src/modes/ctr/ctr_encrypt.c
@@ -0,0 +1,139 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file ctr_encrypt.c
+  CTR implementation, encrypt data, Tom St Denis
+*/
+
+
+#ifdef LTC_CTR_MODE
+
+/**
+  CTR encrypt
+  @param pt     Plaintext
+  @param ct     [out] Ciphertext
+  @param len    Length of plaintext (octets)
+  @param ctr    CTR state
+  @return CRYPT_OK if successful
+*/
+int ctr_encrypt(const unsigned char *pt, unsigned char *ct, unsigned long len, symmetric_CTR *ctr)
+{
+   int x, err;
+
+   LTC_ARGCHK(pt != NULL);
+   LTC_ARGCHK(ct != NULL);
+   LTC_ARGCHK(ctr != NULL);
+
+   if ((err = cipher_is_valid(ctr->cipher)) != CRYPT_OK) {
+       return err;
+   }
+   
+   /* is blocklen/padlen valid? */
+   if (ctr->blocklen < 1 || ctr->blocklen > (int)sizeof(ctr->ctr) ||
+       ctr->padlen   < 0 || ctr->padlen   > (int)sizeof(ctr->pad)) {
+      return CRYPT_INVALID_ARG;
+   }
+
+#ifdef LTC_FAST
+   if (ctr->blocklen % sizeof(LTC_FAST_TYPE)) {
+      return CRYPT_INVALID_ARG;
+   }
+#endif
+   
+   /* handle acceleration only if pad is empty, accelerator is present and length is >= a block size */
+   if ((ctr->padlen == ctr->blocklen) && cipher_descriptor[ctr->cipher]->accel_ctr_encrypt != NULL && (len >= (unsigned long)ctr->blocklen)) {
+      if ((err = cipher_descriptor[ctr->cipher]->accel_ctr_encrypt(pt, ct, len/ctr->blocklen, ctr->ctr, ctr->mode, &ctr->key)) != CRYPT_OK) {
+         return err;
+      }
+      len %= ctr->blocklen;
+   }
+
+   while (len) {
+      /* is the pad empty? */
+      if (ctr->padlen == ctr->blocklen) {
+         /* increment counter */
+         if (ctr->mode == CTR_COUNTER_LITTLE_ENDIAN) {
+            /* little-endian */
+            for (x = 0; x < ctr->ctrlen; x++) {
+               ctr->ctr[x] = (ctr->ctr[x] + (unsigned char)1) & (unsigned char)255;
+               if (ctr->ctr[x] != (unsigned char)0) {
+                  break;
+               }
+            }
+         } else {
+            /* big-endian */
+            for (x = ctr->blocklen-1; x >= ctr->ctrlen; x--) {
+               ctr->ctr[x] = (ctr->ctr[x] + (unsigned char)1) & (unsigned char)255;
+               if (ctr->ctr[x] != (unsigned char)0) {
+                  break;
+               }
+            }
+         }
+
+         /* encrypt it */
+         if ((err = cipher_descriptor[ctr->cipher]->ecb_encrypt(ctr->ctr, ctr->pad, &ctr->key)) != CRYPT_OK) {
+            return err;
+         }
+         ctr->padlen = 0;
+      }
+#ifdef LTC_FAST
+      if (ctr->padlen == 0 && len >= (unsigned long)ctr->blocklen) {
+         for (x = 0; x < ctr->blocklen; x += sizeof(LTC_FAST_TYPE)) {
+            *((LTC_FAST_TYPE*)((unsigned char *)ct + x)) = *((LTC_FAST_TYPE*)((unsigned char *)pt + x)) ^
+                                                           *((LTC_FAST_TYPE*)((unsigned char *)ctr->pad + x));
+         }
+       pt         += ctr->blocklen;
+       ct         += ctr->blocklen;
+       len        -= ctr->blocklen;
+       ctr->padlen = ctr->blocklen;
+       continue;
+      }
+#endif    
+      *ct++ = *pt++ ^ ctr->pad[ctr->padlen++];
+      --len;
+   }
+   return CRYPT_OK;
+}
+
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/modes/ctr/ctr_encrypt.c,v $ */
+/* $Revision: 1.22 $ */
+/* $Date: 2007/02/22 20:26:05 $ */
diff --git a/core/lib/libtomcrypt/src/modes/ctr/ctr_getiv.c b/core/lib/libtomcrypt/src/modes/ctr/ctr_getiv.c
new file mode 100644
index 0000000..0ccb2fe
--- /dev/null
+++ b/core/lib/libtomcrypt/src/modes/ctr/ctr_getiv.c
@@ -0,0 +1,73 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+   @file ctr_getiv.c
+   CTR implementation, get IV, Tom St Denis
+*/
+
+#ifdef LTC_CTR_MODE
+
+/**
+   Get the current initial vector
+   @param IV   [out] The destination of the initial vector
+   @param len  [in/out]  The max size and resulting size of the initial vector
+   @param ctr  The CTR state
+   @return CRYPT_OK if successful
+*/
+int ctr_getiv(unsigned char *IV, unsigned long *len, symmetric_CTR *ctr)
+{
+   LTC_ARGCHK(IV  != NULL);
+   LTC_ARGCHK(len != NULL);
+   LTC_ARGCHK(ctr != NULL);
+   if ((unsigned long)ctr->blocklen > *len) {
+      *len = ctr->blocklen;
+      return CRYPT_BUFFER_OVERFLOW;
+   }
+   XMEMCPY(IV, ctr->ctr, ctr->blocklen);
+   *len = ctr->blocklen;
+
+   return CRYPT_OK;
+}
+
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/modes/ctr/ctr_getiv.c,v $ */
+/* $Revision: 1.7 $ */
+/* $Date: 2006/12/28 01:27:24 $ */
diff --git a/core/lib/libtomcrypt/src/modes/ctr/ctr_setiv.c b/core/lib/libtomcrypt/src/modes/ctr/ctr_setiv.c
new file mode 100644
index 0000000..cbc4c4e
--- /dev/null
+++ b/core/lib/libtomcrypt/src/modes/ctr/ctr_setiv.c
@@ -0,0 +1,83 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file ctr_setiv.c
+  CTR implementation, set IV, Tom St Denis
+*/
+  
+#ifdef LTC_CTR_MODE
+
+/**
+   Set an initial vector
+   @param IV   The initial vector
+   @param len  The length of the vector (in octets)
+   @param ctr  The CTR state
+   @return CRYPT_OK if successful
+*/
+int ctr_setiv(const unsigned char *IV, unsigned long len, symmetric_CTR *ctr)
+{
+   int err;
+   
+   LTC_ARGCHK(IV  != NULL);
+   LTC_ARGCHK(ctr != NULL);
+
+   /* bad param? */
+   if ((err = cipher_is_valid(ctr->cipher)) != CRYPT_OK) {
+      return err;
+   }
+   
+   if (len != (unsigned long)ctr->blocklen) {
+      return CRYPT_INVALID_ARG;
+   }
+
+   /* set IV */
+   XMEMCPY(ctr->ctr, IV, len);
+   
+   /* force next block */
+   ctr->padlen = 0;
+   return cipher_descriptor[ctr->cipher]->ecb_encrypt(IV, ctr->pad, &ctr->key);
+}
+
+#endif 
+
+
+/* $Source: /cvs/libtom/libtomcrypt/src/modes/ctr/ctr_setiv.c,v $ */
+/* $Revision: 1.7 $ */
+/* $Date: 2006/12/28 01:27:24 $ */
diff --git a/core/lib/libtomcrypt/src/modes/ctr/ctr_start.c b/core/lib/libtomcrypt/src/modes/ctr/ctr_start.c
new file mode 100644
index 0000000..337c221
--- /dev/null
+++ b/core/lib/libtomcrypt/src/modes/ctr/ctr_start.c
@@ -0,0 +1,128 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+   @file ctr_start.c
+   CTR implementation, start chain, Tom St Denis
+*/
+
+
+#ifdef LTC_CTR_MODE
+
+/**
+   Initialize a CTR context
+   @param cipher      The index of the cipher desired
+   @param IV          The initial vector
+   @param key         The secret key 
+   @param keylen      The length of the secret key (octets)
+   @param num_rounds  Number of rounds in the cipher desired (0 for default)
+   @param ctr_mode    The counter mode (CTR_COUNTER_LITTLE_ENDIAN or CTR_COUNTER_BIG_ENDIAN)
+   @param ctr         The CTR state to initialize
+   @return CRYPT_OK if successful
+*/
+int ctr_start(               int   cipher, 
+              const unsigned char *IV, 
+              const unsigned char *key,       int keylen, 
+                             int  num_rounds, int ctr_mode,
+                   symmetric_CTR *ctr)
+{
+   int x, err;
+
+   LTC_ARGCHK(IV  != NULL);
+   LTC_ARGCHK(key != NULL);
+   LTC_ARGCHK(ctr != NULL);
+
+   /* bad param? */
+   if ((err = cipher_is_valid(cipher)) != CRYPT_OK) {
+      return err;
+   }
+
+   /* ctrlen == counter width */
+   ctr->ctrlen   = (ctr_mode & 255) ? (ctr_mode & 255) : cipher_descriptor[cipher]->block_length;
+   if (ctr->ctrlen > cipher_descriptor[cipher]->block_length) {
+      return CRYPT_INVALID_ARG;
+   }
+
+   if ((ctr_mode & 0x1000) == CTR_COUNTER_BIG_ENDIAN) {
+      ctr->ctrlen = cipher_descriptor[cipher]->block_length - ctr->ctrlen;
+   }
+
+   /* setup cipher */
+   if ((err = cipher_descriptor[cipher]->setup(key, keylen, num_rounds, &ctr->key)) != CRYPT_OK) {
+      return err;
+   }
+
+   /* copy ctr */
+   ctr->blocklen = cipher_descriptor[cipher]->block_length;
+   ctr->cipher   = cipher;
+   ctr->padlen   = 0;
+   ctr->mode     = ctr_mode & 0x1000;
+   for (x = 0; x < ctr->blocklen; x++) {
+       ctr->ctr[x] = IV[x];
+   }
+
+   if (ctr_mode & LTC_CTR_RFC3686) {
+      /* increment the IV as per RFC 3686 */
+      if (ctr->mode == CTR_COUNTER_LITTLE_ENDIAN) {
+         /* little-endian */
+         for (x = 0; x < ctr->ctrlen; x++) {
+             ctr->ctr[x] = (ctr->ctr[x] + (unsigned char)1) & (unsigned char)255;
+             if (ctr->ctr[x] != (unsigned char)0) {
+                break;
+             }
+         }
+      } else {
+         /* big-endian */
+         for (x = ctr->blocklen-1; x >= ctr->ctrlen; x--) {
+             ctr->ctr[x] = (ctr->ctr[x] + (unsigned char)1) & (unsigned char)255;
+             if (ctr->ctr[x] != (unsigned char)0) {
+                break;
+             }
+         }
+      }
+   }
+
+   return cipher_descriptor[ctr->cipher]->ecb_encrypt(ctr->ctr, ctr->pad, &ctr->key); 
+}
+
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/modes/ctr/ctr_start.c,v $ */
+/* $Revision: 1.15 $ */
+/* $Date: 2007/02/23 14:18:37 $ */
diff --git a/core/lib/libtomcrypt/src/modes/ctr/sub.mk b/core/lib/libtomcrypt/src/modes/ctr/sub.mk
new file mode 100644
index 0000000..e6f9c92
--- /dev/null
+++ b/core/lib/libtomcrypt/src/modes/ctr/sub.mk
@@ -0,0 +1,7 @@
+srcs-y += ctr_decrypt.c
+srcs-y += ctr_done.c
+srcs-y += ctr_encrypt.c
+srcs-y += ctr_getiv.c
+srcs-y += ctr_setiv.c
+srcs-y += ctr_start.c
+# srcs-y += ctr_test.c
diff --git a/core/lib/libtomcrypt/src/modes/ecb/ecb_decrypt.c b/core/lib/libtomcrypt/src/modes/ecb/ecb_decrypt.c
new file mode 100644
index 0000000..7d9d9a9
--- /dev/null
+++ b/core/lib/libtomcrypt/src/modes/ecb/ecb_decrypt.c
@@ -0,0 +1,88 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file ecb_decrypt.c
+  ECB implementation, decrypt a block, Tom St Denis
+*/
+
+#ifdef LTC_ECB_MODE
+
+/**
+  ECB decrypt
+  @param ct     Ciphertext
+  @param pt     [out] Plaintext
+  @param len    The number of octets to process (must be multiple of the cipher block size)
+  @param ecb    ECB state
+  @return CRYPT_OK if successful
+*/
+int ecb_decrypt(const unsigned char *ct, unsigned char *pt, unsigned long len, symmetric_ECB *ecb)
+{
+   int err;
+   LTC_ARGCHK(pt != NULL);
+   LTC_ARGCHK(ct != NULL);
+   LTC_ARGCHK(ecb != NULL);
+   if ((err = cipher_is_valid(ecb->cipher)) != CRYPT_OK) {
+       return err;
+   }
+   if (len % cipher_descriptor[ecb->cipher]->block_length) {
+      return CRYPT_INVALID_ARG;
+   }
+
+   /* check for accel */
+   if (cipher_descriptor[ecb->cipher]->accel_ecb_decrypt != NULL) {
+      return cipher_descriptor[ecb->cipher]->accel_ecb_decrypt(ct, pt, len / cipher_descriptor[ecb->cipher]->block_length, &ecb->key);
+   } else {
+      while (len) {
+         if ((err = cipher_descriptor[ecb->cipher]->ecb_decrypt(ct, pt, &ecb->key)) != CRYPT_OK) {
+            return err;
+         }
+         pt  += cipher_descriptor[ecb->cipher]->block_length;
+         ct  += cipher_descriptor[ecb->cipher]->block_length;
+         len -= cipher_descriptor[ecb->cipher]->block_length;
+      }
+   }
+   return CRYPT_OK;
+}
+
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/modes/ecb/ecb_decrypt.c,v $ */
+/* $Revision: 1.10 $ */
+/* $Date: 2006/12/28 01:27:24 $ */
diff --git a/core/lib/libtomcrypt/src/modes/ecb/ecb_done.c b/core/lib/libtomcrypt/src/modes/ecb/ecb_done.c
new file mode 100644
index 0000000..1660366
--- /dev/null
+++ b/core/lib/libtomcrypt/src/modes/ecb/ecb_done.c
@@ -0,0 +1,69 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+   @file ecb_done.c
+   ECB implementation, finish chain, Tom St Denis
+*/
+
+#ifdef LTC_ECB_MODE
+
+/** Terminate the chain
+  @param ecb    The ECB chain to terminate
+  @return CRYPT_OK on success
+*/
+int ecb_done(symmetric_ECB *ecb)
+{
+   int err;
+   LTC_ARGCHK(ecb != NULL);
+
+   if ((err = cipher_is_valid(ecb->cipher)) != CRYPT_OK) {
+      return err;
+   }
+   cipher_descriptor[ecb->cipher]->done(&ecb->key);
+   return CRYPT_OK;
+}
+
+   
+
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/modes/ecb/ecb_done.c,v $ */
+/* $Revision: 1.8 $ */
+/* $Date: 2006/12/28 01:27:24 $ */
diff --git a/core/lib/libtomcrypt/src/modes/ecb/ecb_encrypt.c b/core/lib/libtomcrypt/src/modes/ecb/ecb_encrypt.c
new file mode 100644
index 0000000..78f6b92
--- /dev/null
+++ b/core/lib/libtomcrypt/src/modes/ecb/ecb_encrypt.c
@@ -0,0 +1,88 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file ecb_encrypt.c
+  ECB implementation, encrypt a block, Tom St Denis
+*/
+
+#ifdef LTC_ECB_MODE
+
+/**
+  ECB encrypt
+  @param pt     Plaintext
+  @param ct     [out] Ciphertext
+  @param len    The number of octets to process (must be multiple of the cipher block size)
+  @param ecb    ECB state
+  @return CRYPT_OK if successful
+*/
+int ecb_encrypt(const unsigned char *pt, unsigned char *ct, unsigned long len, symmetric_ECB *ecb)
+{
+   int err;
+   LTC_ARGCHK(pt != NULL);
+   LTC_ARGCHK(ct != NULL);
+   LTC_ARGCHK(ecb != NULL);
+   if ((err = cipher_is_valid(ecb->cipher)) != CRYPT_OK) {
+       return err;
+   }
+   if (len % cipher_descriptor[ecb->cipher]->block_length) {
+      return CRYPT_INVALID_ARG;
+   }
+
+   /* check for accel */
+   if (cipher_descriptor[ecb->cipher]->accel_ecb_encrypt != NULL) {
+      return cipher_descriptor[ecb->cipher]->accel_ecb_encrypt(pt, ct, len / cipher_descriptor[ecb->cipher]->block_length, &ecb->key);
+   } else {
+      while (len) {
+         if ((err = cipher_descriptor[ecb->cipher]->ecb_encrypt(pt, ct, &ecb->key)) != CRYPT_OK) {
+            return err;
+         }
+         pt  += cipher_descriptor[ecb->cipher]->block_length;
+         ct  += cipher_descriptor[ecb->cipher]->block_length;
+         len -= cipher_descriptor[ecb->cipher]->block_length;
+      }
+   }
+   return CRYPT_OK;
+}
+
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/modes/ecb/ecb_encrypt.c,v $ */
+/* $Revision: 1.10 $ */
+/* $Date: 2006/12/28 01:27:24 $ */
diff --git a/core/lib/libtomcrypt/src/modes/ecb/ecb_start.c b/core/lib/libtomcrypt/src/modes/ecb/ecb_start.c
new file mode 100644
index 0000000..f57abb2
--- /dev/null
+++ b/core/lib/libtomcrypt/src/modes/ecb/ecb_start.c
@@ -0,0 +1,75 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+   @file ecb_start.c
+   ECB implementation, start chain, Tom St Denis
+*/
+
+
+#ifdef LTC_ECB_MODE
+
+/**
+   Initialize a ECB context
+   @param cipher      The index of the cipher desired
+   @param key         The secret key 
+   @param keylen      The length of the secret key (octets)
+   @param num_rounds  Number of rounds in the cipher desired (0 for default)
+   @param ecb         The ECB state to initialize
+   @return CRYPT_OK if successful
+*/
+int ecb_start(int cipher, const unsigned char *key, int keylen, int num_rounds, symmetric_ECB *ecb)
+{
+   int err;
+   LTC_ARGCHK(key != NULL);
+   LTC_ARGCHK(ecb != NULL);
+
+   if ((err = cipher_is_valid(cipher)) != CRYPT_OK) {
+      return err;
+   }
+   ecb->cipher = cipher;
+   ecb->blocklen = cipher_descriptor[cipher]->block_length;
+   return cipher_descriptor[cipher]->setup(key, keylen, num_rounds, &ecb->key);
+}
+
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/modes/ecb/ecb_start.c,v $ */
+/* $Revision: 1.6 $ */
+/* $Date: 2006/12/28 01:27:24 $ */
diff --git a/core/lib/libtomcrypt/src/modes/ecb/sub.mk b/core/lib/libtomcrypt/src/modes/ecb/sub.mk
new file mode 100644
index 0000000..c47c061
--- /dev/null
+++ b/core/lib/libtomcrypt/src/modes/ecb/sub.mk
@@ -0,0 +1,4 @@
+srcs-y += ecb_decrypt.c
+srcs-y += ecb_done.c
+srcs-y += ecb_encrypt.c
+srcs-y += ecb_start.c
diff --git a/core/lib/libtomcrypt/src/modes/f8/f8_decrypt.c b/core/lib/libtomcrypt/src/modes/f8/f8_decrypt.c
new file mode 100644
index 0000000..2f4591f
--- /dev/null
+++ b/core/lib/libtomcrypt/src/modes/f8/f8_decrypt.c
@@ -0,0 +1,70 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file f8_decrypt.c
+  F8 implementation, decrypt data, Tom St Denis
+*/
+
+#ifdef LTC_F8_MODE
+
+/**
+   F8 decrypt
+   @param ct      Ciphertext
+   @param pt      [out] Plaintext
+   @param len     Length of ciphertext (octets)
+   @param f8      F8 state
+   @return CRYPT_OK if successful
+*/
+int f8_decrypt(const unsigned char *ct, unsigned char *pt, unsigned long len, symmetric_F8 *f8)
+{
+   LTC_ARGCHK(pt != NULL);
+   LTC_ARGCHK(ct != NULL);
+   LTC_ARGCHK(f8 != NULL);
+   return f8_encrypt(ct, pt, len, f8);
+}
+
+
+#endif
+
+ 
+
+/* $Source: /cvs/libtom/libtomcrypt/src/modes/f8/f8_decrypt.c,v $ */
+/* $Revision: 1.3 $ */
+/* $Date: 2006/12/28 01:27:24 $ */
diff --git a/core/lib/libtomcrypt/src/modes/f8/f8_done.c b/core/lib/libtomcrypt/src/modes/f8/f8_done.c
new file mode 100644
index 0000000..91b5ddc
--- /dev/null
+++ b/core/lib/libtomcrypt/src/modes/f8/f8_done.c
@@ -0,0 +1,69 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+   @file f8_done.c
+   F8 implementation, finish chain, Tom St Denis
+*/
+
+#ifdef LTC_F8_MODE
+
+/** Terminate the chain
+  @param f8    The F8 chain to terminate
+  @return CRYPT_OK on success
+*/
+int f8_done(symmetric_F8 *f8)
+{
+   int err;
+   LTC_ARGCHK(f8 != NULL);
+
+   if ((err = cipher_is_valid(f8->cipher)) != CRYPT_OK) {
+      return err;
+   }
+   cipher_descriptor[f8->cipher].done(&f8->key);
+   return CRYPT_OK;
+}
+
+   
+
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/modes/f8/f8_done.c,v $ */
+/* $Revision: 1.3 $ */
+/* $Date: 2006/12/28 01:27:24 $ */
diff --git a/core/lib/libtomcrypt/src/modes/f8/f8_encrypt.c b/core/lib/libtomcrypt/src/modes/f8/f8_encrypt.c
new file mode 100644
index 0000000..7a0039c
--- /dev/null
+++ b/core/lib/libtomcrypt/src/modes/f8/f8_encrypt.c
@@ -0,0 +1,130 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file f8_encrypt.c
+  F8 implementation, encrypt data, Tom St Denis
+*/
+
+#ifdef LTC_F8_MODE
+
+/**
+  F8 encrypt
+  @param pt     Plaintext
+  @param ct     [out] Ciphertext
+  @param len    Length of plaintext (octets)
+  @param f8     F8 state
+  @return CRYPT_OK if successful
+*/
+int f8_encrypt(const unsigned char *pt, unsigned char *ct, unsigned long len, symmetric_F8 *f8)
+{
+   int           err, x;
+   unsigned char buf[MAXBLOCKSIZE];
+   LTC_ARGCHK(pt != NULL);
+   LTC_ARGCHK(ct != NULL);
+   LTC_ARGCHK(f8 != NULL);
+   if ((err = cipher_is_valid(f8->cipher)) != CRYPT_OK) {
+       return err;
+   }
+   
+   /* is blocklen/padlen valid? */
+   if (f8->blocklen < 0 || f8->blocklen > (int)sizeof(f8->IV) ||
+       f8->padlen   < 0 || f8->padlen   > (int)sizeof(f8->IV)) {
+      return CRYPT_INVALID_ARG;
+   }
+   
+   zeromem(buf, sizeof(buf));
+
+   /* make sure the pad is empty */
+   if (f8->padlen == f8->blocklen) {
+      /* xor of IV, MIV and blockcnt == what goes into cipher */
+      STORE32H(f8->blockcnt, (buf+(f8->blocklen-4)));
+      ++(f8->blockcnt);
+      for (x = 0; x < f8->blocklen; x++) {
+          f8->IV[x] ^= f8->MIV[x] ^ buf[x];
+      }
+      if ((err = cipher_descriptor[f8->cipher].ecb_encrypt(f8->IV, f8->IV, &f8->key)) != CRYPT_OK) {
+         return err;
+      }
+      f8->padlen = 0;
+   }
+
+#ifdef LTC_FAST
+   if (f8->padlen == 0) {
+      while (len >= (unsigned long)f8->blocklen) {
+         STORE32H(f8->blockcnt, (buf+(f8->blocklen-4)));
+         ++(f8->blockcnt);
+         for (x = 0; x < f8->blocklen; x += sizeof(LTC_FAST_TYPE)) {
+             *((LTC_FAST_TYPE*)(&ct[x])) = *((LTC_FAST_TYPE*)(&pt[x])) ^ *((LTC_FAST_TYPE*)(&f8->IV[x]));
+             *((LTC_FAST_TYPE*)(&f8->IV[x])) ^= *((LTC_FAST_TYPE*)(&f8->MIV[x])) ^ *((LTC_FAST_TYPE*)(&buf[x]));
+         }
+         if ((err = cipher_descriptor[f8->cipher].ecb_encrypt(f8->IV, f8->IV, &f8->key)) != CRYPT_OK) {
+            return err;
+         }
+         len -= x;
+         pt  += x;
+         ct  += x;
+      }
+   }
+#endif             
+
+   while (len > 0) {
+       if (f8->padlen == f8->blocklen) {
+          /* xor of IV, MIV and blockcnt == what goes into cipher */
+          STORE32H(f8->blockcnt, (buf+(f8->blocklen-4)));
+          ++(f8->blockcnt);
+          for (x = 0; x < f8->blocklen; x++) {
+              f8->IV[x] ^= f8->MIV[x] ^ buf[x];
+          }
+          if ((err = cipher_descriptor[f8->cipher].ecb_encrypt(f8->IV, f8->IV, &f8->key)) != CRYPT_OK) {
+             return err;
+          }
+          f8->padlen = 0;
+       }
+       *ct++ = *pt++ ^ f8->IV[f8->padlen++];
+       --len;
+   }
+   return CRYPT_OK;
+}
+
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/modes/f8/f8_encrypt.c,v $ */
+/* $Revision: 1.7 $ */
+/* $Date: 2006/12/28 01:27:24 $ */
diff --git a/core/lib/libtomcrypt/src/modes/f8/f8_getiv.c b/core/lib/libtomcrypt/src/modes/f8/f8_getiv.c
new file mode 100644
index 0000000..425a420
--- /dev/null
+++ b/core/lib/libtomcrypt/src/modes/f8/f8_getiv.c
@@ -0,0 +1,73 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+   @file ofb_getiv.c
+   F8 implementation, get IV, Tom St Denis
+*/
+
+#ifdef LTC_F8_MODE
+
+/**
+   Get the current initial vector
+   @param IV   [out] The destination of the initial vector
+   @param len  [in/out]  The max size and resulting size of the initial vector
+   @param f8   The F8 state
+   @return CRYPT_OK if successful
+*/
+int f8_getiv(unsigned char *IV, unsigned long *len, symmetric_F8 *f8)
+{
+   LTC_ARGCHK(IV  != NULL);
+   LTC_ARGCHK(len != NULL);
+   LTC_ARGCHK(f8  != NULL);
+   if ((unsigned long)f8->blocklen > *len) {
+      *len = f8->blocklen;
+      return CRYPT_BUFFER_OVERFLOW;
+   }
+   XMEMCPY(IV, f8->IV, f8->blocklen);
+   *len = f8->blocklen;
+
+   return CRYPT_OK;
+}
+
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/modes/f8/f8_getiv.c,v $ */
+/* $Revision: 1.3 $ */
+/* $Date: 2006/12/28 01:27:24 $ */
diff --git a/core/lib/libtomcrypt/src/modes/f8/f8_setiv.c b/core/lib/libtomcrypt/src/modes/f8/f8_setiv.c
new file mode 100644
index 0000000..b0bce2a
--- /dev/null
+++ b/core/lib/libtomcrypt/src/modes/f8/f8_setiv.c
@@ -0,0 +1,79 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+   @file f8_setiv.c
+   F8 implementation, set IV, Tom St Denis
+*/
+
+#ifdef LTC_F8_MODE
+
+/**
+   Set an initial vector
+   @param IV   The initial vector
+   @param len  The length of the vector (in octets)
+   @param f8   The F8 state
+   @return CRYPT_OK if successful
+*/
+int f8_setiv(const unsigned char *IV, unsigned long len, symmetric_F8 *f8)
+{
+   int err;
+
+   LTC_ARGCHK(IV != NULL);
+   LTC_ARGCHK(f8 != NULL);
+
+   if ((err = cipher_is_valid(f8->cipher)) != CRYPT_OK) {
+       return err;
+   }
+
+   if (len != (unsigned long)f8->blocklen) {
+      return CRYPT_INVALID_ARG;
+   }
+
+   /* force next block */
+   f8->padlen = 0;
+   return cipher_descriptor[f8->cipher].ecb_encrypt(IV, f8->IV, &f8->key);
+}
+
+#endif 
+
+
+/* $Source: /cvs/libtom/libtomcrypt/src/modes/f8/f8_setiv.c,v $ */
+/* $Revision: 1.3 $ */
+/* $Date: 2006/12/28 01:27:24 $ */
diff --git a/core/lib/libtomcrypt/src/modes/f8/f8_start.c b/core/lib/libtomcrypt/src/modes/f8/f8_start.c
new file mode 100644
index 0000000..d7296e7
--- /dev/null
+++ b/core/lib/libtomcrypt/src/modes/f8/f8_start.c
@@ -0,0 +1,125 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+   @file f8_start.c
+   F8 implementation, start chain, Tom St Denis
+*/
+
+
+#ifdef LTC_F8_MODE
+
+/**
+   Initialize an F8 context
+   @param cipher      The index of the cipher desired
+   @param IV          The initial vector
+   @param key         The secret key 
+   @param keylen      The length of the secret key (octets)
+   @param salt_key    The salting key for the IV
+   @param skeylen     The length of the salting key (octets)
+   @param num_rounds  Number of rounds in the cipher desired (0 for default)
+   @param f8          The F8 state to initialize
+   @return CRYPT_OK if successful
+*/
+int f8_start(                int  cipher, const unsigned char *IV, 
+             const unsigned char *key,                    int  keylen, 
+             const unsigned char *salt_key,               int  skeylen,
+                             int  num_rounds,   symmetric_F8  *f8)
+{
+   int           x, err;
+   unsigned char tkey[MAXBLOCKSIZE];
+
+   LTC_ARGCHK(IV       != NULL);
+   LTC_ARGCHK(key      != NULL);
+   LTC_ARGCHK(salt_key != NULL);
+   LTC_ARGCHK(f8       != NULL);
+
+   if ((err = cipher_is_valid(cipher)) != CRYPT_OK) {
+      return err;
+   }
+
+#ifdef LTC_FAST
+   if (cipher_descriptor[cipher].block_length % sizeof(LTC_FAST_TYPE)) {
+      return CRYPT_INVALID_ARG;
+   }
+#endif
+
+   /* copy details */
+   f8->blockcnt = 0;
+   f8->cipher   = cipher;
+   f8->blocklen = cipher_descriptor[cipher].block_length;
+   f8->padlen   = f8->blocklen;
+   
+   /* now get key ^ salt_key [extend salt_ket with 0x55 as required to match length] */
+   zeromem(tkey, sizeof(tkey));
+   for (x = 0; x < keylen && x < (int)sizeof(tkey); x++) {
+       tkey[x] = key[x];
+   }
+   for (x = 0; x < skeylen && x < (int)sizeof(tkey); x++) {
+       tkey[x] ^= salt_key[x];
+   }       
+   for (; x < keylen && x < (int)sizeof(tkey); x++) {
+       tkey[x] ^= 0x55;
+   }
+   
+   /* now encrypt with tkey[0..keylen-1] the IV and use that as the IV */
+   if ((err = cipher_descriptor[cipher].setup(tkey, keylen, num_rounds, &f8->key)) != CRYPT_OK) {
+      return err;
+   }
+   
+   /* encrypt IV */
+   if ((err = cipher_descriptor[f8->cipher].ecb_encrypt(IV, f8->MIV, &f8->key)) != CRYPT_OK) {
+      cipher_descriptor[f8->cipher].done(&f8->key);
+      return err;
+   }
+   zeromem(tkey, sizeof(tkey));
+   zeromem(f8->IV, sizeof(f8->IV));
+   
+   /* terminate this cipher */
+   cipher_descriptor[f8->cipher].done(&f8->key);
+   
+   /* init the cipher */
+   return cipher_descriptor[cipher].setup(key, keylen, num_rounds, &f8->key);
+}
+
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/modes/f8/f8_start.c,v $ */
+/* $Revision: 1.8 $ */
+/* $Date: 2006/12/28 01:27:24 $ */
diff --git a/core/lib/libtomcrypt/src/modes/f8/sub.mk b/core/lib/libtomcrypt/src/modes/f8/sub.mk
new file mode 100644
index 0000000..32e1381
--- /dev/null
+++ b/core/lib/libtomcrypt/src/modes/f8/sub.mk
@@ -0,0 +1,6 @@
+srcs-y += f8_decrypt.c
+srcs-y += f8_done.c
+srcs-y += f8_encrypt.c
+srcs-y += f8_getiv.c
+srcs-y += f8_setiv.c
+srcs-y += f8_start.c
diff --git a/core/lib/libtomcrypt/src/modes/lrw/lrw_decrypt.c b/core/lib/libtomcrypt/src/modes/lrw/lrw_decrypt.c
new file mode 100644
index 0000000..be7447d
--- /dev/null
+++ b/core/lib/libtomcrypt/src/modes/lrw/lrw_decrypt.c
@@ -0,0 +1,78 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+   @file lrw_decrypt.c
+   LRW_MODE implementation, Decrypt blocks, Tom St Denis
+*/
+
+#ifdef LTC_LRW_MODE
+
+/**
+  LRW decrypt blocks
+  @param ct     The ciphertext
+  @param pt     [out] The plaintext
+  @param len    The length in octets, must be a multiple of 16
+  @param lrw    The LRW state
+*/
+int lrw_decrypt(const unsigned char *ct, unsigned char *pt, unsigned long len, symmetric_LRW *lrw)
+{
+   int err;
+
+   LTC_ARGCHK(pt  != NULL);
+   LTC_ARGCHK(ct  != NULL);
+   LTC_ARGCHK(lrw != NULL);
+
+   if ((err = cipher_is_valid(lrw->cipher)) != CRYPT_OK) {
+      return err;
+   }
+
+   if (cipher_descriptor[lrw->cipher].accel_lrw_decrypt != NULL) {
+      return cipher_descriptor[lrw->cipher].accel_lrw_decrypt(ct, pt, len, lrw->IV, lrw->tweak, &lrw->key);
+   }
+
+   return lrw_process(ct, pt, len, LRW_DECRYPT, lrw);
+}
+
+
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/modes/lrw/lrw_decrypt.c,v $ */
+/* $Revision: 1.9 $ */
+/* $Date: 2006/12/28 01:27:24 $ */
diff --git a/core/lib/libtomcrypt/src/modes/lrw/lrw_done.c b/core/lib/libtomcrypt/src/modes/lrw/lrw_done.c
new file mode 100644
index 0000000..0a44bad
--- /dev/null
+++ b/core/lib/libtomcrypt/src/modes/lrw/lrw_done.c
@@ -0,0 +1,69 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+   @file lrw_done.c
+   LRW_MODE implementation, Free resources, Tom St Denis
+*/
+
+#ifdef LTC_LRW_MODE
+
+/**
+  Terminate a LRW state
+  @param lrw   The state to terminate
+  @return CRYPT_OK if successful
+*/
+int lrw_done(symmetric_LRW *lrw) 
+{
+   int err;
+
+   LTC_ARGCHK(lrw != NULL);
+ 
+   if ((err = cipher_is_valid(lrw->cipher)) != CRYPT_OK) {
+      return err;
+   }
+   cipher_descriptor[lrw->cipher].done(&lrw->key);
+
+   return CRYPT_OK;
+}
+
+#endif
+/* $Source: /cvs/libtom/libtomcrypt/src/modes/lrw/lrw_done.c,v $ */
+/* $Revision: 1.7 $ */
+/* $Date: 2006/12/28 01:27:24 $ */
diff --git a/core/lib/libtomcrypt/src/modes/lrw/lrw_encrypt.c b/core/lib/libtomcrypt/src/modes/lrw/lrw_encrypt.c
new file mode 100644
index 0000000..0032aab
--- /dev/null
+++ b/core/lib/libtomcrypt/src/modes/lrw/lrw_encrypt.c
@@ -0,0 +1,77 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+   @file lrw_encrypt.c
+   LRW_MODE implementation, Encrypt blocks, Tom St Denis
+*/
+
+#ifdef LTC_LRW_MODE
+ 
+/**
+  LRW encrypt blocks
+  @param pt     The plaintext
+  @param ct     [out] The ciphertext
+  @param len    The length in octets, must be a multiple of 16
+  @param lrw    The LRW state
+*/
+int lrw_encrypt(const unsigned char *pt, unsigned char *ct, unsigned long len, symmetric_LRW *lrw)
+{
+   int err;
+
+   LTC_ARGCHK(pt  != NULL);
+   LTC_ARGCHK(ct  != NULL);
+   LTC_ARGCHK(lrw != NULL);
+
+   if ((err = cipher_is_valid(lrw->cipher)) != CRYPT_OK) {
+      return err;
+   }
+
+   if (cipher_descriptor[lrw->cipher].accel_lrw_encrypt != NULL) {
+      return cipher_descriptor[lrw->cipher].accel_lrw_encrypt(pt, ct, len, lrw->IV, lrw->tweak, &lrw->key);
+   }
+
+   return lrw_process(pt, ct, len, LRW_ENCRYPT, lrw);
+}
+
+
+#endif
+/* $Source: /cvs/libtom/libtomcrypt/src/modes/lrw/lrw_encrypt.c,v $ */
+/* $Revision: 1.10 $ */
+/* $Date: 2006/12/28 01:27:24 $ */
diff --git a/core/lib/libtomcrypt/src/modes/lrw/lrw_getiv.c b/core/lib/libtomcrypt/src/modes/lrw/lrw_getiv.c
new file mode 100644
index 0000000..0297b3c
--- /dev/null
+++ b/core/lib/libtomcrypt/src/modes/lrw/lrw_getiv.c
@@ -0,0 +1,72 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+   @file lrw_getiv.c
+   LRW_MODE implementation, Retrieve the current IV, Tom St Denis
+*/
+
+#ifdef LTC_LRW_MODE
+
+/**
+  Get the IV for LRW
+  @param IV      [out] The IV, must be 16 octets
+  @param len     Length ... must be at least 16 :-)
+  @param lrw     The LRW state to read
+  @return CRYPT_OK if successful
+*/
+int lrw_getiv(unsigned char *IV, unsigned long *len, symmetric_LRW *lrw)
+{
+   LTC_ARGCHK(IV != NULL);
+   LTC_ARGCHK(len != NULL);
+   LTC_ARGCHK(lrw != NULL);
+   if (*len < 16) {
+       *len = 16;
+       return CRYPT_BUFFER_OVERFLOW;
+   }
+
+   XMEMCPY(IV, lrw->IV, 16);
+   *len = 16;
+   return CRYPT_OK;
+}
+
+#endif
+/* $Source: /cvs/libtom/libtomcrypt/src/modes/lrw/lrw_getiv.c,v $ */
+/* $Revision: 1.10 $ */
+/* $Date: 2006/12/28 01:27:24 $ */
diff --git a/core/lib/libtomcrypt/src/modes/lrw/lrw_process.c b/core/lib/libtomcrypt/src/modes/lrw/lrw_process.c
new file mode 100644
index 0000000..8830185
--- /dev/null
+++ b/core/lib/libtomcrypt/src/modes/lrw/lrw_process.c
@@ -0,0 +1,147 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+   @file lrw_process.c
+   LRW_MODE implementation, Encrypt/decrypt blocks, Tom St Denis
+*/
+
+#ifdef LTC_LRW_MODE
+
+/**
+  Process blocks with LRW, since decrypt/encrypt are largely the same they share this code.
+  @param pt        The "input" data
+  @param ct        [out] The "output" data
+  @param len       The length of the input, must be a multiple of 128-bits (16 octets)
+  @param mode      LRW_ENCRYPT or LRW_DECRYPT
+  @param lrw       The LRW state
+  @return  CRYPT_OK if successful
+*/
+int lrw_process(const unsigned char *pt, unsigned char *ct, unsigned long len, int mode, symmetric_LRW *lrw)
+{
+   unsigned char prod[16];
+   int           x, err;
+#ifdef LTC_LRW_TABLES
+   int           y;
+#endif
+
+   LTC_ARGCHK(pt  != NULL);
+   LTC_ARGCHK(ct  != NULL);
+   LTC_ARGCHK(lrw != NULL);
+
+   if (len & 15) {
+      return CRYPT_INVALID_ARG;
+   }
+
+   while (len) {
+      /* copy pad */
+      XMEMCPY(prod, lrw->pad, 16);
+
+      /* increment IV */
+      for (x = 15; x >= 0; x--) {
+          lrw->IV[x] = (lrw->IV[x] + 1) & 255;
+          if (lrw->IV[x]) { 
+              break;
+          }
+      }
+
+      /* update pad */
+#ifdef LTC_LRW_TABLES
+      /* for each byte changed we undo it's affect on the pad then add the new product */
+      for (; x < 16; x++) {
+#ifdef LTC_FAST
+          for (y = 0; y < 16; y += sizeof(LTC_FAST_TYPE)) {
+              *((LTC_FAST_TYPE *)(lrw->pad + y)) ^= *((LTC_FAST_TYPE *)(&lrw->PC[x][lrw->IV[x]][y])) ^ *((LTC_FAST_TYPE *)(&lrw->PC[x][(lrw->IV[x]-1)&255][y]));
+          }
+#else
+          for (y = 0; y < 16; y++) {
+              lrw->pad[y] ^= lrw->PC[x][lrw->IV[x]][y] ^ lrw->PC[x][(lrw->IV[x]-1)&255][y];
+          }
+#endif
+      }
+#else
+      gcm_gf_mult(lrw->tweak, lrw->IV, lrw->pad);
+#endif
+
+      /* xor prod */
+#ifdef LTC_FAST
+      for (x = 0; x < 16; x += sizeof(LTC_FAST_TYPE)) {
+           *((LTC_FAST_TYPE *)(ct + x)) = *((LTC_FAST_TYPE *)(pt + x)) ^ *((LTC_FAST_TYPE *)(prod + x));
+      }
+#else
+      for (x = 0; x < 16; x++) {
+         ct[x] = pt[x] ^ prod[x];
+      }
+#endif
+
+      /* send through cipher */
+      if (mode == LRW_ENCRYPT) {
+         if ((err = cipher_descriptor[lrw->cipher].ecb_encrypt(ct, ct, &lrw->key)) != CRYPT_OK) {
+            return err;
+         }
+      } else {
+         if ((err = cipher_descriptor[lrw->cipher].ecb_decrypt(ct, ct, &lrw->key)) != CRYPT_OK) {
+            return err;
+         }
+      }               
+
+      /* xor prod */
+#ifdef LTC_FAST
+      for (x = 0; x < 16; x += sizeof(LTC_FAST_TYPE)) {
+           *((LTC_FAST_TYPE *)(ct + x)) = *((LTC_FAST_TYPE *)(ct + x)) ^ *((LTC_FAST_TYPE *)(prod + x));
+      }
+#else
+      for (x = 0; x < 16; x++) {
+         ct[x] = ct[x] ^ prod[x];
+      }
+#endif
+   
+      /* move to next */
+      pt  += 16;
+      ct  += 16;
+      len -= 16;
+   }
+
+   return CRYPT_OK;
+}
+      
+#endif
+/* $Source: /cvs/libtom/libtomcrypt/src/modes/lrw/lrw_process.c,v $ */
+/* $Revision: 1.11 $ */
+/* $Date: 2006/12/28 01:27:24 $ */
diff --git a/core/lib/libtomcrypt/src/modes/lrw/lrw_setiv.c b/core/lib/libtomcrypt/src/modes/lrw/lrw_setiv.c
new file mode 100644
index 0000000..3cc6a11
--- /dev/null
+++ b/core/lib/libtomcrypt/src/modes/lrw/lrw_setiv.c
@@ -0,0 +1,106 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+   @file lrw_setiv.c
+   LRW_MODE implementation, Set the current IV, Tom St Denis
+*/
+
+#ifdef LTC_LRW_MODE
+
+/**
+  Set the IV for LRW
+  @param IV      The IV, must be 16 octets
+  @param len     Length ... must be 16 :-)
+  @param lrw     The LRW state to update
+  @return CRYPT_OK if successful
+*/
+int lrw_setiv(const unsigned char *IV, unsigned long len, symmetric_LRW *lrw)
+{
+   int           err;
+#ifdef LTC_LRW_TABLES
+   unsigned char T[16];
+   int           x, y;
+#endif
+   LTC_ARGCHK(IV != NULL);
+   LTC_ARGCHK(lrw != NULL);
+
+   if (len != 16) {
+      return CRYPT_INVALID_ARG;
+   }
+
+   if ((err = cipher_is_valid(lrw->cipher)) != CRYPT_OK) {
+      return err;
+   }
+
+   /* copy the IV */
+   XMEMCPY(lrw->IV, IV, 16);
+
+   /* check if we have to actually do work */
+   if (cipher_descriptor[lrw->cipher].accel_lrw_encrypt != NULL && cipher_descriptor[lrw->cipher].accel_lrw_decrypt != NULL) {
+       /* we have accelerators, let's bail since they don't use lrw->pad anyways */
+       return CRYPT_OK;
+   }
+
+#ifdef LTC_LRW_TABLES
+   XMEMCPY(T, &lrw->PC[0][IV[0]][0], 16);
+   for (x = 1; x < 16; x++) {
+#ifdef LTC_FAST
+       for (y = 0; y < 16; y += sizeof(LTC_FAST_TYPE)) {
+           *((LTC_FAST_TYPE *)(T + y)) ^= *((LTC_FAST_TYPE *)(&lrw->PC[x][IV[x]][y]));
+       }
+#else
+       for (y = 0; y < 16; y++) {
+           T[y] ^= lrw->PC[x][IV[x]][y];
+       }
+#endif
+   }
+   XMEMCPY(lrw->pad, T, 16);
+#else     
+   gcm_gf_mult(lrw->tweak, IV, lrw->pad); 
+#endif
+
+   return CRYPT_OK;
+}
+
+
+#endif
+/* $Source: /cvs/libtom/libtomcrypt/src/modes/lrw/lrw_setiv.c,v $ */
+/* $Revision: 1.13 $ */
+/* $Date: 2006/12/28 01:27:24 $ */
diff --git a/core/lib/libtomcrypt/src/modes/lrw/lrw_start.c b/core/lib/libtomcrypt/src/modes/lrw/lrw_start.c
new file mode 100644
index 0000000..4924ebf
--- /dev/null
+++ b/core/lib/libtomcrypt/src/modes/lrw/lrw_start.c
@@ -0,0 +1,130 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+   @file lrw_start.c
+   LRW_MODE implementation, start mode, Tom St Denis
+*/
+
+#ifdef LTC_LRW_MODE
+
+/**
+  Initialize the LRW context
+  @param cipher        The cipher desired, must be a 128-bit block cipher 
+  @param IV            The index value, must be 128-bits
+  @param key           The cipher key 
+  @param keylen        The length of the cipher key in octets
+  @param tweak         The tweak value (second key), must be 128-bits
+  @param num_rounds    The number of rounds for the cipher (0 == default)
+  @param lrw           [out] The LRW state
+  @return CRYPT_OK on success.
+*/
+int lrw_start(               int   cipher,
+              const unsigned char *IV,
+              const unsigned char *key,       int keylen,
+              const unsigned char *tweak,
+                             int  num_rounds, 
+                   symmetric_LRW *lrw)
+{
+   int           err;
+#ifdef LTC_LRW_TABLES
+   unsigned char B[16];
+   int           x, y, z, t;
+#endif
+
+  LTC_ARGCHK(IV    != NULL);
+  LTC_ARGCHK(key   != NULL);
+  LTC_ARGCHK(tweak != NULL);
+  LTC_ARGCHK(lrw   != NULL);
+
+#ifdef LTC_FAST
+   if (16 % sizeof(LTC_FAST_TYPE)) {
+      return CRYPT_INVALID_ARG;
+   }
+#endif
+
+   /* is cipher valid? */
+   if ((err = cipher_is_valid(cipher)) != CRYPT_OK) {
+      return err;
+   }
+   if (cipher_descriptor[cipher].block_length != 16) {
+      return CRYPT_INVALID_CIPHER;
+   }
+
+   /* schedule key */
+   if ((err = cipher_descriptor[cipher].setup(key, keylen, num_rounds, &lrw->key)) != CRYPT_OK) {
+      return err;
+   }
+   lrw->cipher = cipher;
+
+   /* copy the IV and tweak */
+   XMEMCPY(lrw->tweak, tweak, 16);
+
+#ifdef LTC_LRW_TABLES
+   /* setup tables */
+   /* generate the first table as it has no shifting (from which we make the other tables) */
+   zeromem(B, 16);
+   for (y = 0; y < 256; y++) {
+        B[0] = y;
+        gcm_gf_mult(tweak, B, &lrw->PC[0][y][0]);
+   }
+
+   /* now generate the rest of the tables based the previous table */
+   for (x = 1; x < 16; x++) {
+      for (y = 0; y < 256; y++) {
+         /* now shift it right by 8 bits */
+         t = lrw->PC[x-1][y][15];
+         for (z = 15; z > 0; z--) {
+             lrw->PC[x][y][z] = lrw->PC[x-1][y][z-1];
+         }
+         lrw->PC[x][y][0]  = gcm_shift_table[t<<1];
+         lrw->PC[x][y][1] ^= gcm_shift_table[(t<<1)+1];
+     }
+  }
+#endif
+
+   /* generate first pad */
+   return lrw_setiv(IV, 16, lrw);
+}
+
+
+#endif
+/* $Source: /cvs/libtom/libtomcrypt/src/modes/lrw/lrw_start.c,v $ */
+/* $Revision: 1.12 $ */
+/* $Date: 2006/12/28 01:27:24 $ */
diff --git a/core/lib/libtomcrypt/src/modes/lrw/sub.mk b/core/lib/libtomcrypt/src/modes/lrw/sub.mk
new file mode 100644
index 0000000..cd256f7
--- /dev/null
+++ b/core/lib/libtomcrypt/src/modes/lrw/sub.mk
@@ -0,0 +1,8 @@
+srcs-y += lrw_decrypt.c
+srcs-y += lrw_done.c
+srcs-y += lrw_encrypt.c
+srcs-y += lrw_getiv.c
+srcs-y += lrw_process.c
+srcs-y += lrw_setiv.c
+srcs-y += lrw_start.c
+# srcs-y += lrw_test.c
diff --git a/core/lib/libtomcrypt/src/modes/ofb/ofb_decrypt.c b/core/lib/libtomcrypt/src/modes/ofb/ofb_decrypt.c
new file mode 100644
index 0000000..d034bbe
--- /dev/null
+++ b/core/lib/libtomcrypt/src/modes/ofb/ofb_decrypt.c
@@ -0,0 +1,70 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file ofb_decrypt.c
+  OFB implementation, decrypt data, Tom St Denis
+*/
+
+#ifdef LTC_OFB_MODE
+
+/**
+   OFB decrypt
+   @param ct      Ciphertext
+   @param pt      [out] Plaintext
+   @param len     Length of ciphertext (octets)
+   @param ofb     OFB state
+   @return CRYPT_OK if successful
+*/
+int ofb_decrypt(const unsigned char *ct, unsigned char *pt, unsigned long len, symmetric_OFB *ofb)
+{
+   LTC_ARGCHK(pt != NULL);
+   LTC_ARGCHK(ct != NULL);
+   LTC_ARGCHK(ofb != NULL);
+   return ofb_encrypt(ct, pt, len, ofb);
+}
+
+
+#endif
+
+ 
+
+/* $Source: /cvs/libtom/libtomcrypt/src/modes/ofb/ofb_decrypt.c,v $ */
+/* $Revision: 1.6 $ */
+/* $Date: 2006/12/28 01:27:24 $ */
diff --git a/core/lib/libtomcrypt/src/modes/ofb/ofb_done.c b/core/lib/libtomcrypt/src/modes/ofb/ofb_done.c
new file mode 100644
index 0000000..72bfb09
--- /dev/null
+++ b/core/lib/libtomcrypt/src/modes/ofb/ofb_done.c
@@ -0,0 +1,69 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+   @file ofb_done.c
+   OFB implementation, finish chain, Tom St Denis
+*/
+
+#ifdef LTC_OFB_MODE
+
+/** Terminate the chain
+  @param ofb    The OFB chain to terminate
+  @return CRYPT_OK on success
+*/
+int ofb_done(symmetric_OFB *ofb)
+{
+   int err;
+   LTC_ARGCHK(ofb != NULL);
+
+   if ((err = cipher_is_valid(ofb->cipher)) != CRYPT_OK) {
+      return err;
+   }
+   cipher_descriptor[ofb->cipher].done(&ofb->key);
+   return CRYPT_OK;
+}
+
+   
+
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/modes/ofb/ofb_done.c,v $ */
+/* $Revision: 1.7 $ */
+/* $Date: 2006/12/28 01:27:24 $ */
diff --git a/core/lib/libtomcrypt/src/modes/ofb/ofb_encrypt.c b/core/lib/libtomcrypt/src/modes/ofb/ofb_encrypt.c
new file mode 100644
index 0000000..943d9ce
--- /dev/null
+++ b/core/lib/libtomcrypt/src/modes/ofb/ofb_encrypt.c
@@ -0,0 +1,87 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file ofb_encrypt.c
+  OFB implementation, encrypt data, Tom St Denis
+*/
+
+#ifdef LTC_OFB_MODE
+
+/**
+  OFB encrypt
+  @param pt     Plaintext
+  @param ct     [out] Ciphertext
+  @param len    Length of plaintext (octets)
+  @param ofb    OFB state
+  @return CRYPT_OK if successful
+*/
+int ofb_encrypt(const unsigned char *pt, unsigned char *ct, unsigned long len, symmetric_OFB *ofb)
+{
+   int err;
+   LTC_ARGCHK(pt != NULL);
+   LTC_ARGCHK(ct != NULL);
+   LTC_ARGCHK(ofb != NULL);
+   if ((err = cipher_is_valid(ofb->cipher)) != CRYPT_OK) {
+       return err;
+   }
+   
+   /* is blocklen/padlen valid? */
+   if (ofb->blocklen < 0 || ofb->blocklen > (int)sizeof(ofb->IV) ||
+       ofb->padlen   < 0 || ofb->padlen   > (int)sizeof(ofb->IV)) {
+      return CRYPT_INVALID_ARG;
+   }
+   
+   while (len-- > 0) {
+       if (ofb->padlen == ofb->blocklen) {
+          if ((err = cipher_descriptor[ofb->cipher].ecb_encrypt(ofb->IV, ofb->IV, &ofb->key)) != CRYPT_OK) {
+             return err;
+          }
+          ofb->padlen = 0;
+       }
+       *ct++ = *pt++ ^ ofb->IV[(ofb->padlen)++];
+   }
+   return CRYPT_OK;
+}
+
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/modes/ofb/ofb_encrypt.c,v $ */
+/* $Revision: 1.8 $ */
+/* $Date: 2006/12/28 01:27:24 $ */
diff --git a/core/lib/libtomcrypt/src/modes/ofb/ofb_getiv.c b/core/lib/libtomcrypt/src/modes/ofb/ofb_getiv.c
new file mode 100644
index 0000000..ee2d73d
--- /dev/null
+++ b/core/lib/libtomcrypt/src/modes/ofb/ofb_getiv.c
@@ -0,0 +1,73 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+   @file ofb_getiv.c
+   OFB implementation, get IV, Tom St Denis
+*/
+
+#ifdef LTC_OFB_MODE
+
+/**
+   Get the current initial vector
+   @param IV   [out] The destination of the initial vector
+   @param len  [in/out]  The max size and resulting size of the initial vector
+   @param ofb  The OFB state
+   @return CRYPT_OK if successful
+*/
+int ofb_getiv(unsigned char *IV, unsigned long *len, symmetric_OFB *ofb)
+{
+   LTC_ARGCHK(IV  != NULL);
+   LTC_ARGCHK(len != NULL);
+   LTC_ARGCHK(ofb != NULL);
+   if ((unsigned long)ofb->blocklen > *len) {
+      *len = ofb->blocklen;
+      return CRYPT_BUFFER_OVERFLOW;
+   }
+   XMEMCPY(IV, ofb->IV, ofb->blocklen);
+   *len = ofb->blocklen;
+
+   return CRYPT_OK;
+}
+
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/modes/ofb/ofb_getiv.c,v $ */
+/* $Revision: 1.7 $ */
+/* $Date: 2006/12/28 01:27:24 $ */
diff --git a/core/lib/libtomcrypt/src/modes/ofb/ofb_setiv.c b/core/lib/libtomcrypt/src/modes/ofb/ofb_setiv.c
new file mode 100644
index 0000000..efb66c8
--- /dev/null
+++ b/core/lib/libtomcrypt/src/modes/ofb/ofb_setiv.c
@@ -0,0 +1,79 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+   @file ofb_setiv.c
+   OFB implementation, set IV, Tom St Denis
+*/
+
+#ifdef LTC_OFB_MODE
+
+/**
+   Set an initial vector
+   @param IV   The initial vector
+   @param len  The length of the vector (in octets)
+   @param ofb  The OFB state
+   @return CRYPT_OK if successful
+*/
+int ofb_setiv(const unsigned char *IV, unsigned long len, symmetric_OFB *ofb)
+{
+   int err;
+
+   LTC_ARGCHK(IV  != NULL);
+   LTC_ARGCHK(ofb != NULL);
+
+   if ((err = cipher_is_valid(ofb->cipher)) != CRYPT_OK) {
+       return err;
+   }
+
+   if (len != (unsigned long)ofb->blocklen) {
+      return CRYPT_INVALID_ARG;
+   }
+
+   /* force next block */
+   ofb->padlen = 0;
+   return cipher_descriptor[ofb->cipher].ecb_encrypt(IV, ofb->IV, &ofb->key);
+}
+
+#endif 
+
+
+/* $Source: /cvs/libtom/libtomcrypt/src/modes/ofb/ofb_setiv.c,v $ */
+/* $Revision: 1.7 $ */
+/* $Date: 2006/12/28 01:27:24 $ */
diff --git a/core/lib/libtomcrypt/src/modes/ofb/ofb_start.c b/core/lib/libtomcrypt/src/modes/ofb/ofb_start.c
new file mode 100644
index 0000000..9633614
--- /dev/null
+++ b/core/lib/libtomcrypt/src/modes/ofb/ofb_start.c
@@ -0,0 +1,87 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+   @file ofb_start.c
+   OFB implementation, start chain, Tom St Denis
+*/
+
+
+#ifdef LTC_OFB_MODE
+
+/**
+   Initialize a OFB context
+   @param cipher      The index of the cipher desired
+   @param IV          The initial vector
+   @param key         The secret key 
+   @param keylen      The length of the secret key (octets)
+   @param num_rounds  Number of rounds in the cipher desired (0 for default)
+   @param ofb         The OFB state to initialize
+   @return CRYPT_OK if successful
+*/
+int ofb_start(int cipher, const unsigned char *IV, const unsigned char *key, 
+              int keylen, int num_rounds, symmetric_OFB *ofb)
+{
+   int x, err;
+
+   LTC_ARGCHK(IV != NULL);
+   LTC_ARGCHK(key != NULL);
+   LTC_ARGCHK(ofb != NULL);
+
+   if ((err = cipher_is_valid(cipher)) != CRYPT_OK) {
+      return err;
+   }
+
+   /* copy details */
+   ofb->cipher = cipher;
+   ofb->blocklen = cipher_descriptor[cipher].block_length;
+   for (x = 0; x < ofb->blocklen; x++) {
+       ofb->IV[x] = IV[x];
+   }
+
+   /* init the cipher */
+   ofb->padlen = ofb->blocklen;
+   return cipher_descriptor[cipher].setup(key, keylen, num_rounds, &ofb->key);
+}
+
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/modes/ofb/ofb_start.c,v $ */
+/* $Revision: 1.6 $ */
+/* $Date: 2006/12/28 01:27:24 $ */
diff --git a/core/lib/libtomcrypt/src/modes/ofb/sub.mk b/core/lib/libtomcrypt/src/modes/ofb/sub.mk
new file mode 100644
index 0000000..f1fceeb
--- /dev/null
+++ b/core/lib/libtomcrypt/src/modes/ofb/sub.mk
@@ -0,0 +1,6 @@
+srcs-y += ofb_decrypt.c
+srcs-y += ofb_done.c
+srcs-y += ofb_encrypt.c
+srcs-y += ofb_getiv.c
+srcs-y += ofb_setiv.c
+srcs-y += ofb_start.c
diff --git a/core/lib/libtomcrypt/src/modes/sub.mk b/core/lib/libtomcrypt/src/modes/sub.mk
new file mode 100644
index 0000000..9177622
--- /dev/null
+++ b/core/lib/libtomcrypt/src/modes/sub.mk
@@ -0,0 +1,4 @@
+subdirs-$(_CFG_CRYPTO_WITH_CBC) += cbc
+subdirs-$(CFG_CRYPTO_CTR) += ctr
+subdirs-$(CFG_CRYPTO_ECB) += ecb
+subdirs-$(CFG_CRYPTO_XTS) += xts
diff --git a/core/lib/libtomcrypt/src/modes/xts/sub.mk b/core/lib/libtomcrypt/src/modes/xts/sub.mk
new file mode 100644
index 0000000..255a20d
--- /dev/null
+++ b/core/lib/libtomcrypt/src/modes/xts/sub.mk
@@ -0,0 +1,6 @@
+srcs-y += xts_decrypt.c
+srcs-y += xts_done.c
+srcs-y += xts_encrypt.c
+srcs-y += xts_init.c
+srcs-y += xts_mult_x.c
+# srcs-y += xts_test.c
diff --git a/core/lib/libtomcrypt/src/modes/xts/xts_decrypt.c b/core/lib/libtomcrypt/src/modes/xts/xts_decrypt.c
new file mode 100644
index 0000000..7e729a9
--- /dev/null
+++ b/core/lib/libtomcrypt/src/modes/xts/xts_decrypt.c
@@ -0,0 +1,187 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/** 
+  Source donated by Elliptic Semiconductor Inc (www.ellipticsemi.com) to the LibTom Projects
+*/
+
+#ifdef LTC_XTS_MODE
+
+static int tweak_uncrypt(const unsigned char *C, unsigned char *P, unsigned char *T, symmetric_xts *xts)
+{
+   unsigned long x;
+   int err;
+
+   /* tweak encrypt block i */
+#ifdef LTC_FAST
+   for (x = 0; x < 16; x += sizeof(LTC_FAST_TYPE)) {
+      *((LTC_FAST_TYPE*)&P[x]) = *((LTC_FAST_TYPE*)&C[x]) ^ *((LTC_FAST_TYPE*)&T[x]);
+   }
+#else
+   for (x = 0; x < 16; x++) {
+       P[x] = C[x] ^ T[x];
+   }
+#endif
+     
+   err = cipher_descriptor[xts->cipher]->ecb_decrypt(P, P, &xts->key1);  
+
+#ifdef LTC_FAST
+   for (x = 0; x < 16; x += sizeof(LTC_FAST_TYPE)) {
+      *((LTC_FAST_TYPE*)&P[x]) ^=  *((LTC_FAST_TYPE*)&T[x]);
+   }
+#else
+   for (x = 0; x < 16; x++) {
+       P[x] = P[x] ^ T[x];
+   }
+#endif
+
+   /* LFSR the tweak */
+   xts_mult_x(T);
+
+   return err;
+}   
+
+/** XTS Decryption
+  @param ct     [in] Ciphertext
+  @param ptlen  Length of plaintext (and ciphertext)
+  @param pt     [out]  Plaintext
+  @param tweak  [in] The 128--bit encryption tweak (e.g. sector number)
+  @param xts    The XTS structure
+  Returns CRYPT_OK upon success
+*/
+int xts_decrypt(const unsigned char *ct, unsigned long ptlen, unsigned char *pt, unsigned char *tweak,
+         symmetric_xts *xts)
+{
+   const struct ltc_cipher_descriptor *desc;
+   unsigned char PP[16], CC[16], T[16];
+   unsigned long i, m, mo, lim;
+   int           err;
+
+   /* check inputs */
+   LTC_ARGCHK(pt    != NULL);
+   LTC_ARGCHK(ct    != NULL);
+   LTC_ARGCHK(tweak != NULL);
+   LTC_ARGCHK(xts   != NULL);
+
+   /* check if valid */
+   if ((err = cipher_is_valid(xts->cipher)) != CRYPT_OK) {
+      return err;
+   }
+
+   /* get number of blocks */
+   m  = ptlen >> 4;
+   mo = ptlen & 15;
+
+   /* must have at least one full block */
+   if (m == 0) {
+      return CRYPT_INVALID_ARG;
+   }
+
+   if (mo == 0) {
+      lim = m;
+   } else {
+      lim = m - 1;
+   }
+
+   desc = cipher_descriptor[xts->cipher];
+
+   if (desc->accel_xts_decrypt && lim > 0) {
+
+	   /* use accelerated decryption for whole blocks */
+	   if ((err = desc->accel_xts_decrypt(ct, pt, lim, tweak, &xts->key1,
+					      &xts->key2) != CRYPT_OK)) {
+	      return err;
+	   }
+	   ct += lim * 16;
+	   pt += lim * 16;
+
+	   /* tweak is encrypted on output */
+	   XMEMCPY(T, tweak, sizeof(T));
+   } else {
+      /* encrypt the tweak */
+      if ((err = desc->ecb_encrypt(tweak, T, &xts->key2)) != CRYPT_OK) {
+	 return err;
+      }
+
+      for (i = 0; i < lim; i++) {
+         if ((err = tweak_uncrypt(ct, pt, T, xts)) != CRYPT_OK) {
+            return err;
+         }
+         ct += 16;
+         pt += 16;
+      }
+   }
+
+   /* if ptlen not divide 16 then */
+   if (mo > 0) {
+      XMEMCPY(CC, T, 16);
+      xts_mult_x(CC);
+
+      /* PP = tweak decrypt block m-1 */
+      if ((err = tweak_uncrypt(ct, PP, CC, xts)) != CRYPT_OK) {
+         return err;
+      }
+
+      /* Pm = first ptlen % 16 bytes of PP */
+      for (i = 0; i < mo; i++) {
+          CC[i]    = ct[16+i];
+          pt[16+i] = PP[i];
+      }
+      for (; i < 16; i++) {
+          CC[i] = PP[i];
+      }
+
+      /* Pm-1 = Tweak uncrypt CC */
+      if ((err = tweak_uncrypt(CC, pt, T, xts)) != CRYPT_OK) {
+         return err;
+      }
+   }
+   /* Decrypt the tweak back */
+   if ((err = desc->ecb_decrypt(T, tweak, &xts->key2)) != CRYPT_OK) {
+      return err;
+   }
+
+   return CRYPT_OK;
+}
+
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/modes/xts/xts_decrypt.c,v $ */
+/* $Revision: 1.5 $ */
+/* $Date: 2007/05/12 14:05:56 $ */
diff --git a/core/lib/libtomcrypt/src/modes/xts/xts_done.c b/core/lib/libtomcrypt/src/modes/xts/xts_done.c
new file mode 100644
index 0000000..6d24fbe
--- /dev/null
+++ b/core/lib/libtomcrypt/src/modes/xts/xts_done.c
@@ -0,0 +1,61 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/** 
+  Source donated by Elliptic Semiconductor Inc (www.ellipticsemi.com) to the LibTom Projects
+*/
+
+#ifdef LTC_XTS_MODE
+
+/** Terminate XTS state 
+   @param XTS    The state to terminate
+*/
+void xts_done(symmetric_xts *xts)
+{
+   LTC_ARGCHKVD(xts != NULL);
+   cipher_descriptor[xts->cipher]->done(&xts->key1);
+   cipher_descriptor[xts->cipher]->done(&xts->key2);
+}
+
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/modes/xts/xts_done.c,v $ */
+/* $Revision: 1.4 $ */
+/* $Date: 2007/03/10 23:59:09 $ */
+
diff --git a/core/lib/libtomcrypt/src/modes/xts/xts_encrypt.c b/core/lib/libtomcrypt/src/modes/xts/xts_encrypt.c
new file mode 100644
index 0000000..3448383
--- /dev/null
+++ b/core/lib/libtomcrypt/src/modes/xts/xts_encrypt.c
@@ -0,0 +1,189 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/** 
+  Source donated by Elliptic Semiconductor Inc (www.ellipticsemi.com) to the LibTom Projects
+*/
+
+#ifdef LTC_XTS_MODE
+
+static int tweak_crypt(const unsigned char *P, unsigned char *C, unsigned char *T, symmetric_xts *xts)
+{
+   unsigned long x;
+   int err;
+
+   /* tweak encrypt block i */
+#ifdef LTC_FAST
+   for (x = 0; x < 16; x += sizeof(LTC_FAST_TYPE)) {
+      *((LTC_FAST_TYPE*)&C[x]) = *((LTC_FAST_TYPE*)&P[x]) ^ *((LTC_FAST_TYPE*)&T[x]);
+   }
+#else
+   for (x = 0; x < 16; x++) {
+      C[x] = P[x] ^ T[x];
+   }
+#endif
+     
+   if ((err = cipher_descriptor[xts->cipher]->ecb_encrypt(C, C, &xts->key1)) != CRYPT_OK) {
+      return err;
+   }
+
+#ifdef LTC_FAST
+   for (x = 0; x < 16; x += sizeof(LTC_FAST_TYPE)) {
+      *((LTC_FAST_TYPE*)&C[x]) ^= *((LTC_FAST_TYPE*)&T[x]);
+   }
+#else
+   for (x = 0; x < 16; x++) {
+       C[x] = C[x] ^ T[x];
+   }
+#endif
+
+   /* LFSR the tweak */
+   xts_mult_x(T);
+
+   return CRYPT_OK;
+}   
+
+/** XTS Encryption
+  @param pt     [in]  Plaintext
+  @param ptlen  Length of plaintext (and ciphertext)
+  @param ct     [out] Ciphertext
+  @param tweak  [in] The 128--bit encryption tweak (e.g. sector number)
+  @param xts    The XTS structure
+  Returns CRYPT_OK upon success
+*/
+int xts_encrypt(const unsigned char *pt, unsigned long ptlen, unsigned char *ct, unsigned char *tweak,
+         symmetric_xts *xts)
+{
+   const struct ltc_cipher_descriptor *desc;
+   unsigned char PP[16], CC[16], T[16];
+   unsigned long i, m, mo, lim;
+   int           err;
+
+   /* check inputs */
+   LTC_ARGCHK(pt    != NULL);
+   LTC_ARGCHK(ct    != NULL);
+   LTC_ARGCHK(tweak != NULL);
+   LTC_ARGCHK(xts   != NULL);
+
+   /* check if valid */
+   if ((err = cipher_is_valid(xts->cipher)) != CRYPT_OK) {
+      return err;
+   }
+
+   /* get number of blocks */
+   m  = ptlen >> 4;
+   mo = ptlen & 15;
+
+   /* must have at least one full block */
+   if (m == 0) {
+      return CRYPT_INVALID_ARG;
+   }
+
+   if (mo == 0) {
+      lim = m;
+   } else {
+      lim = m - 1;
+   }
+
+   desc = cipher_descriptor[xts->cipher];
+
+   if (desc->accel_xts_encrypt && lim > 0) {
+
+      /* use accelerated encryption for whole blocks */
+      if ((err = desc->accel_xts_encrypt(pt, ct, lim, tweak, &xts->key1,
+					 &xts->key2) != CRYPT_OK)) {
+	 return err;
+      }
+      ct += lim * 16;
+      pt += lim * 16;
+
+      /* tweak is encrypted on output */
+      XMEMCPY(T, tweak, sizeof(T));
+   } else {
+
+      /* encrypt the tweak */
+      if ((err = desc->ecb_encrypt(tweak, T, &xts->key2)) != CRYPT_OK) {
+	 return err;
+      }
+
+      for (i = 0; i < lim; i++) {
+         if ((err = tweak_crypt(pt, ct, T, xts)) != CRYPT_OK) {
+            return err;
+         }
+         ct += 16;
+         pt += 16;
+      }
+   }
+
+   /* if ptlen not divide 16 then */
+   if (mo > 0) {
+      /* CC = tweak encrypt block m-1 */
+      if ((err = tweak_crypt(pt, CC, T, xts)) != CRYPT_OK) {
+         return err;
+      }
+
+      /* Cm = first ptlen % 16 bytes of CC */
+      for (i = 0; i < mo; i++) {
+          PP[i] = pt[16+i];
+          ct[16+i] = CC[i];
+      }
+
+      for (; i < 16; i++) {
+          PP[i] = CC[i];
+      }
+
+      /* Cm-1 = Tweak encrypt PP */
+      if ((err = tweak_crypt(PP, ct, T, xts)) != CRYPT_OK) {
+         return err;
+      }
+   }
+
+   /* Decrypt the tweak back */
+   if ((err = cipher_descriptor[xts->cipher]->ecb_decrypt(T, tweak, &xts->key2)) != CRYPT_OK) {
+      return err;
+   }
+
+   return err;
+}
+
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/modes/xts/xts_encrypt.c,v $ */
+/* $Revision: 1.5 $ */
+/* $Date: 2007/05/12 14:05:56 $ */
diff --git a/core/lib/libtomcrypt/src/modes/xts/xts_init.c b/core/lib/libtomcrypt/src/modes/xts/xts_init.c
new file mode 100644
index 0000000..318d381
--- /dev/null
+++ b/core/lib/libtomcrypt/src/modes/xts/xts_init.c
@@ -0,0 +1,95 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/** 
+  Source donated by Elliptic Semiconductor Inc (www.ellipticsemi.com) to the LibTom Projects
+*/
+
+#ifdef LTC_XTS_MODE
+
+
+/** Start XTS mode
+   @param cipher      The index of the cipher to use
+   @param key1        The encrypt key
+   @param key2        The tweak encrypt key
+   @param keylen      The length of the keys (each) in octets
+   @param num_rounds  The number of rounds for the cipher (0 == default)
+   @param xts         [out] XTS structure
+   Returns CRYPT_OK upon success.
+*/
+int xts_start(                int  cipher,
+              const unsigned char *key1, 
+              const unsigned char *key2, 
+                    unsigned long  keylen,
+                              int  num_rounds, 
+                    symmetric_xts *xts)
+{
+   int err;
+
+   /* check inputs */
+   LTC_ARGCHK(key1  != NULL);
+   LTC_ARGCHK(key2  != NULL);
+   LTC_ARGCHK(xts   != NULL);
+
+   /* check if valid */
+   if ((err = cipher_is_valid(cipher)) != CRYPT_OK) {
+      return err;
+   }
+
+   if (cipher_descriptor[cipher]->block_length != 16) {
+      return CRYPT_INVALID_ARG;
+   }
+
+   /* schedule the two ciphers */
+   if ((err = cipher_descriptor[cipher]->setup(key1, keylen, num_rounds, &xts->key1)) != CRYPT_OK) {
+      return err;
+   }
+   if ((err = cipher_descriptor[cipher]->setup(key2, keylen, num_rounds, &xts->key2)) != CRYPT_OK) {
+      return err;
+   }
+   xts->cipher = cipher;
+
+   return err;
+}
+
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/modes/xts/xts_init.c,v $ */
+/* $Revision: 1.4 $ */
+/* $Date: 2007/03/10 23:59:09 $ */
diff --git a/core/lib/libtomcrypt/src/modes/xts/xts_mult_x.c b/core/lib/libtomcrypt/src/modes/xts/xts_mult_x.c
new file mode 100644
index 0000000..1c3478b
--- /dev/null
+++ b/core/lib/libtomcrypt/src/modes/xts/xts_mult_x.c
@@ -0,0 +1,68 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/** 
+  Source donated by Elliptic Semiconductor Inc (www.ellipticsemi.com) to the LibTom Projects
+*/
+
+#ifdef LTC_XTS_MODE
+
+/** multiply by x 
+  @param I      The value to multiply by x (LFSR shift)
+*/
+void xts_mult_x(unsigned char *I)
+{
+  int x;
+  unsigned char t, tt;
+
+  for (x = t = 0; x < 16; x++) {
+     tt   = I[x] >> 7;
+     I[x] = ((I[x] << 1) | t) & 0xFF;
+     t    = tt;
+  }
+  if (tt) {
+     I[0] ^= 0x87;
+  } 
+}
+
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/modes/xts/xts_mult_x.c,v $ */
+/* $Revision: 1.4 $ */
+/* $Date: 2007/03/10 23:59:09 $ */
diff --git a/core/lib/libtomcrypt/src/mpa_desc.c b/core/lib/libtomcrypt/src/mpa_desc.c
new file mode 100644
index 0000000..ed7c3e2
--- /dev/null
+++ b/core/lib/libtomcrypt/src/mpa_desc.c
@@ -0,0 +1,663 @@
+/*
+ * 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 "tomcrypt_mpa.h"
+#include <mpa.h>
+
+mpa_scratch_mem external_mem_pool;
+
+void init_mpa_tomcrypt(const mpa_scratch_mem pool)
+{
+	external_mem_pool = pool;
+}
+
+static int init_mpanum(mpanum *a)
+{
+	LTC_ARGCHK(a != NULL);
+	if (!mpa_alloc_static_temp_var(a, external_mem_pool))
+		return CRYPT_MEM;
+	mpa_set_S32(*a, 0);
+	return CRYPT_OK;
+}
+
+static int init(void **a)
+{
+	mpanum n;
+	int ret;
+
+	ret = init_mpanum(&n);
+	*a = n;
+	return ret;
+}
+
+static int init_size(int size_bits, void **a)
+{
+	LTC_ARGCHK(a != NULL);
+	if (!mpa_alloc_static_temp_var_size(size_bits, (mpanum *)a,
+					    external_mem_pool))
+		return CRYPT_MEM;
+	mpa_set_S32(*a, 0);
+	return CRYPT_OK;
+}
+
+static void deinit_mpanum(mpanum a)
+{
+	LTC_ARGCHKVD(a != NULL);
+
+	mpa_free_static_temp_var(&a, external_mem_pool);
+}
+
+static void deinit(void *a)
+{
+	deinit_mpanum(a);
+}
+
+static int neg(void *a, void *b)
+{
+	LTC_ARGCHK(a != NULL);
+	LTC_ARGCHK(b != NULL);
+	mpa_neg((mpanum)b, (const mpanum)a);
+	return CRYPT_OK;
+}
+
+static int copy(void *a, void *b)
+{
+	LTC_ARGCHK(a != NULL);
+	LTC_ARGCHK(b != NULL);
+	mpa_copy((mpanum)b, (const mpanum)a);
+	return CRYPT_OK;
+}
+
+static int init_copy(void **a, void *b)
+{
+	if (init(a) != CRYPT_OK) {
+		return CRYPT_MEM;
+	}
+	return copy(b, *a);
+}
+
+/* ---- trivial ---- */
+static int set_int(void *a, unsigned long b)
+{
+	LTC_ARGCHK(a != NULL);
+	if (b > (unsigned long) UINT32_MAX) {
+		return CRYPT_INVALID_ARG;
+	}
+	mpa_set_word((mpanum) a, (mpa_word_t)b);
+	return CRYPT_OK;
+}
+
+static unsigned long get_int(void *a)
+{
+	LTC_ARGCHK(a != NULL);
+	return mpa_get_word((mpanum)a);
+}
+
+static ltc_mp_digit get_digit(void *a, int n)
+{
+	LTC_ARGCHK(a != NULL);
+	return __mpanum_get_word(n, (mpanum) a);
+}
+
+static int get_digit_count(void *a)
+{
+	LTC_ARGCHK(a != NULL);
+	return __mpanum_size((mpanum) a);
+}
+
+static int compare(void *a, void *b)
+{
+	int ret;
+	LTC_ARGCHK(a != NULL);
+	LTC_ARGCHK(b != NULL);
+	ret = mpa_cmp((const mpanum)a, (const mpanum)b);
+	if (ret < 0) {
+		return LTC_MP_LT;
+	} else if (ret > 0) {
+		return LTC_MP_GT;
+	} else {
+		return LTC_MP_EQ;
+	}
+}
+
+static int compare_d(void *a, unsigned long b)
+{
+	int ret;
+	LTC_ARGCHK(a != NULL);
+	// this particular case must be handled separately...
+	if (b > (unsigned long) MPA_INT_MAX) {
+		mpanum tmp = (mpanum) a;
+		ret = (tmp->size <= 0 ? LTC_MP_LT :
+				tmp->size > 1  ? LTC_MP_GT :
+						tmp->d[0] < b  ? LTC_MP_LT :
+								tmp->d[0] == b ? LTC_MP_EQ : LTC_MP_GT);
+	} else {
+		ret = mpa_cmp_short(((const mpanum)a), b);
+	}
+	if (ret < 0) {
+		return LTC_MP_LT;
+	} else if (ret > 0) {
+		return LTC_MP_GT;
+	} else {
+		return LTC_MP_EQ;
+	}
+}
+
+static int count_bits(void *a)
+{
+	LTC_ARGCHK(a != NULL);
+	// mpa_highest_bit_index returns the index of the highest '1' bit starting at 0
+	// so adding 1 to the result gives the wanted result
+	return mpa_highest_bit_index((const mpanum)a) + 1;
+}
+
+static int count_lsb_bits(void *a)
+{
+	LTC_ARGCHK(a != NULL);
+	if (((mpanum)a)->size == 0) {
+		return 0;
+	}
+	int zero_limb_nbr = 0;
+	int zero_bit_nbr = 0;
+	const mpanum aa = (mpanum) a;
+	while (aa->d[zero_limb_nbr] == 0) {
+		++zero_limb_nbr;
+	}
+	zero_bit_nbr = zero_limb_nbr * MPA_WORD_SIZE;
+	while (!mpa_get_bit(aa, zero_bit_nbr)) {
+		++zero_bit_nbr;
+	}
+
+	return zero_bit_nbr;
+}
+
+
+static int twoexpt(void *a, int n)
+{
+	LTC_ARGCHK(a != NULL);
+	LTC_ARGCHK(n >= 0);
+	int i;
+	mpa_asize_t q; /* quotient of n div WORD_SIZE */
+	mpa_asize_t r; /* remainder of n div WORD_SIZE */
+
+	r = n & (MPA_WORD_SIZE - 1);    /* 0 <= r < WORD_SIZE */
+	q = n >> MPA_LOG_OF_WORD_SIZE;  /* 0 <= q */
+
+	if (((mpanum)a)->alloc < (q + 1)) {
+		return CRYPT_MEM;
+	}
+	((mpanum)a)->size = q + 1;
+	for (i = 0; i < ((mpanum)a)->size; ++i) {
+		((mpanum)a)->d[i] = 0;
+	}
+	((mpanum)a)->d[q] = 1UL << r;
+	return CRYPT_OK;
+}
+
+/* ---- conversions ---- */
+
+/* read ascii string */
+static int read_radix(void *a, const char *b, int radix)
+{
+	LTC_ARGCHK(a != NULL);
+	LTC_ARGCHK(b != NULL);
+	if (radix != 16) {
+		return CRYPT_ERROR;
+	}
+	mpa_set_str((mpanum)a, b);
+	return CRYPT_OK;
+}
+
+/* write one */
+static int write_radix(void *a, char *b, int radix)
+{
+	LTC_ARGCHK(a != NULL);
+	LTC_ARGCHK(b != NULL);
+	if (mpa_get_str(b, MPA_STRING_MODE_HEX_UC, (const mpanum)a) == 0) {
+		return CRYPT_MEM;
+	}
+	return CRYPT_OK;
+}
+
+/* get size as unsigned char string */
+static unsigned long unsigned_size(void *a)
+{
+	unsigned long t;
+	LTC_ARGCHK(a != NULL);
+	t = count_bits(a);
+	if (mpa_cmp_short((const mpanum)a, 0) == 0) return 0;
+	return (t>>3) + ((t&7)?1:0);
+}
+
+/* store */
+static int unsigned_write(void *a, unsigned char *b)
+{
+	LTC_ARGCHK(a != NULL);
+	LTC_ARGCHK(b != NULL);
+	size_t len = unsigned_size(a);
+	mpa_get_oct_str(b, &len, (const mpanum) a);
+	return CRYPT_OK;
+}
+
+/* read */
+static int unsigned_read(void *a, unsigned char *b, unsigned long len)
+{
+	LTC_ARGCHK(a != NULL);
+	LTC_ARGCHK(b != NULL);
+	mpa_set_oct_str((mpanum) a, b, len, 0);
+	return CRYPT_OK;
+}
+
+/* add */
+static int add(void *a, void *b, void *c)
+{
+	LTC_ARGCHK(a != NULL);
+	LTC_ARGCHK(b != NULL);
+	LTC_ARGCHK(c != NULL);
+	mpa_add((mpanum) c, (const mpanum) a, (const mpanum) b, external_mem_pool);
+	return CRYPT_OK;
+}
+
+static int addi(void *a, unsigned long b, void *c)
+{
+	LTC_ARGCHK(a != NULL);
+	LTC_ARGCHK(c != NULL);
+	if (b > (unsigned long) UINT32_MAX) {
+		return CRYPT_INVALID_ARG;
+	}
+	mpa_add_word((mpanum) c, (const mpanum) a, b, external_mem_pool);
+	return CRYPT_OK;
+}
+
+/* sub */
+static int sub(void *a, void *b, void *c)
+{
+	LTC_ARGCHK(a != NULL);
+	LTC_ARGCHK(b != NULL);
+	LTC_ARGCHK(c != NULL);
+	mpa_sub((mpanum) c, (const mpanum) a, (const mpanum) b, external_mem_pool);
+	return CRYPT_OK;
+}
+
+static int subi(void *a, unsigned long b, void *c)
+{
+	LTC_ARGCHK(a != NULL);
+	LTC_ARGCHK(c != NULL);
+	if (b > (unsigned long) UINT32_MAX) {
+		return CRYPT_INVALID_ARG;
+	}
+	mpa_sub_word((mpanum) c, (const mpanum) a, b, external_mem_pool);
+	return CRYPT_OK;
+}
+
+/* mul */
+static int mul(void *a, void *b, void *c)
+{
+	LTC_ARGCHK(a != NULL);
+	LTC_ARGCHK(b != NULL);
+	LTC_ARGCHK(c != NULL);
+	mpa_mul((mpanum) c, (const mpanum) a, (const mpanum) b, external_mem_pool);
+	return CRYPT_OK;
+}
+
+static int muli(void *a, unsigned long b, void *c)
+{
+	LTC_ARGCHK(a != NULL);
+	LTC_ARGCHK(c != NULL);
+	if (b > (unsigned long) UINT32_MAX) {
+		return CRYPT_INVALID_ARG;
+	}
+	mpa_mul_word((mpanum) c, (const mpanum) a, b, external_mem_pool);
+	return CRYPT_OK;
+}
+
+/* sqr */
+static int sqr(void *a, void *b)
+{
+	LTC_ARGCHK(a != NULL);
+	LTC_ARGCHK(b != NULL);
+	mpa_mul((mpanum) b, (const mpanum) a, (const mpanum) a, external_mem_pool);
+	return CRYPT_OK;
+}
+
+/* div */
+static int divide(void *a, void *b, void *c, void *d)
+{
+	LTC_ARGCHK(a != NULL);
+	LTC_ARGCHK(b != NULL);
+	mpa_div(c, d, (const mpanum) a, (const mpanum) b, external_mem_pool);
+	return CRYPT_OK;
+}
+
+static int div_2(void *a, void *b)
+{
+	LTC_ARGCHK(a != NULL);
+	LTC_ARGCHK(b != NULL);
+	mpa_shift_right(b, a, 1);
+	return CRYPT_OK;
+}
+
+/* modi */
+static int modi(void *a, unsigned long b, unsigned long *c)
+{
+	LTC_ARGCHK(a != NULL);
+	LTC_ARGCHK(c != NULL);
+	if (b > (unsigned long) UINT32_MAX) {
+		return CRYPT_INVALID_ARG;
+	}
+	int err;
+	void *tmp;
+	if ((err = init(&tmp)) != CRYPT_OK) {
+		return CRYPT_MEM;
+	}
+
+	if (set_int(tmp, b) != CRYPT_OK) {goto err;}
+	if (divide(a, tmp, NULL, tmp) != CRYPT_OK) {goto err;}
+
+	*c = get_int(tmp);
+
+	err:
+	deinit(tmp);
+	return CRYPT_OK;
+}
+
+/* gcd */
+static int gcd(void *a, void *b, void *c)
+{
+	LTC_ARGCHK(a != NULL);
+	LTC_ARGCHK(b != NULL);
+	LTC_ARGCHK(c != NULL);
+	mpa_gcd((mpanum) c, (const mpanum) a, (const mpanum) b, external_mem_pool);
+	return CRYPT_OK;
+}
+
+/* lcm */
+static int lcm(void *a, void *b, void *c)
+{
+	LTC_ARGCHK(a != NULL);
+	LTC_ARGCHK(b != NULL);
+	LTC_ARGCHK(c != NULL);
+	void *tmp;
+	if (init(&tmp) != CRYPT_OK) {
+		return CRYPT_MEM;
+	}
+
+	if (mul(a, b, tmp) != CRYPT_OK) {goto err;}
+	if (gcd(a, b, c) != CRYPT_OK) {goto err;}
+
+	/* We use the following equality: gcd(a, b) * lcm(a, b) = a * b */
+	if (divide(tmp, c, c, NULL) != CRYPT_OK) {goto err;}
+	err:
+	deinit(tmp);
+	return CRYPT_OK;
+}
+
+static int mod(void *a, void *b, void *c)
+{
+	LTC_ARGCHK(a != NULL);
+	LTC_ARGCHK(b != NULL);
+	LTC_ARGCHK(c != NULL);
+	mpa_mod((mpanum) c, (const mpanum) a, (const mpanum) b, external_mem_pool);
+	if (mpa_cmp_short(c, 0) < 0) {
+		mpa_add(c, c, b, external_mem_pool);
+	}
+	return CRYPT_OK;
+}
+
+static int mulmod(void *a, void *b, void *c, void *d)
+{
+	LTC_ARGCHK(a != NULL);
+	LTC_ARGCHK(b != NULL);
+	LTC_ARGCHK(c != NULL);
+	LTC_ARGCHK(d != NULL);
+	void *tmpa, *tmpb;
+
+	mp_init_multi(&tmpa, &tmpb, NULL);
+
+	mod(a, c, tmpa);
+	mod(b, c, tmpb);
+	mpa_mul_mod((mpanum) d, (const mpanum) tmpa, (const mpanum) tmpb, (const mpanum) c, external_mem_pool);
+	mp_clear_multi(tmpa, tmpb, NULL);
+	return CRYPT_OK;
+}
+
+static int sqrmod(void *a, void *b, void *c)
+{
+	LTC_ARGCHK(a != NULL);
+	LTC_ARGCHK(b != NULL);
+	LTC_ARGCHK(c != NULL);
+	return mulmod(a, a, b, c);
+}
+
+/* invmod */
+static int invmod(void *a, void *b, void *c)
+{
+	LTC_ARGCHK(a != NULL);
+	LTC_ARGCHK(b != NULL);
+	LTC_ARGCHK(c != NULL);
+	LTC_ARGCHK(b != c);
+	mod(a, b, c);
+	if (mpa_inv_mod((mpanum) c, (const mpanum) c, (const mpanum) b, external_mem_pool) != 0) {
+		return CRYPT_ERROR;
+	}
+
+	return CRYPT_OK;
+}
+
+/* setup */
+static int montgomery_setup(void *a, void **b)
+{
+	LTC_ARGCHK(a != NULL);
+	LTC_ARGCHK(b != NULL);
+	mpa_word_t len = mpa_fmm_context_size_in_U32(count_bits(a));
+	*b = malloc(len * sizeof(mpa_word_t));
+	if (*b == NULL) {
+		return CRYPT_MEM;
+	}
+	mpa_fmm_context_base * b_tmp = (mpa_fmm_context_base *) *b;
+	mpa_init_static_fmm_context(b_tmp, len);
+	mpa_compute_fmm_context((const mpanum) a, b_tmp->r_ptr, b_tmp->r2_ptr, &(b_tmp->n_inv), external_mem_pool);
+	return CRYPT_OK;
+}
+
+/* get normalization value */
+static int montgomery_normalization(void *a, void *b)
+{
+	LTC_ARGCHK(a != NULL);
+	LTC_ARGCHK(b != NULL);
+	mpa_asize_t s;
+	s = __mpanum_size((mpanum) b);
+	twoexpt(a, s * MPA_WORD_SIZE);
+	mpa_mod((mpanum) a, (const mpanum) a, (const mpanum) b, external_mem_pool);
+	return CRYPT_OK;
+}
+
+/* reduce */
+static int montgomery_reduce(void *a, void *b, void *c)
+{
+	LTC_ARGCHK(a != NULL);
+	LTC_ARGCHK(b != NULL);
+	LTC_ARGCHK(c != NULL);
+	mpanum tmp;
+
+	if (init_mpanum(&tmp) != CRYPT_OK) {
+		return CRYPT_MEM;
+	}
+	// WARNING
+	//  Workaround for a bug when a > b (a greater than the modulus)
+	if (compare(a, b) == LTC_MP_GT) {
+		mpa_mod((mpanum) a, (const mpanum) a, (const mpanum) b, external_mem_pool);
+	}
+	mpa_montgomery_mul(tmp,
+			(mpanum) a,
+			mpa_constant_one(),
+			(mpanum) b,
+			((mpa_fmm_context) c)->n_inv,
+			external_mem_pool);
+	mpa_copy(a, tmp);
+	deinit(tmp);
+	return CRYPT_OK;
+}
+
+/* clean up */
+static void montgomery_deinit(void *a)
+{
+	free(a);
+}
+
+static int exptmod(void *a, void *b, void *c, void *d)
+{
+	LTC_ARGCHK(a != NULL);
+	LTC_ARGCHK(b != NULL);
+	LTC_ARGCHK(c != NULL);
+	LTC_ARGCHK(d != NULL);
+	void *c_mont;
+	if (montgomery_setup(c, &c_mont) != CRYPT_OK) {
+		return CRYPT_MEM;
+	}
+
+	void *d_tmp;
+	int memguard;
+
+	memguard = (a == d || b == d);
+
+	if (memguard) {
+		if (init(&d_tmp) != CRYPT_OK) {
+			return CRYPT_MEM;
+		}
+	} else {
+		d_tmp = d;
+	}
+
+	// WARNING !!
+	// Temporary fix, since ExpMod behaves badly when a > c
+	//           (ie "a" is greater than the modulus)
+	mod(a, c, d_tmp);
+
+
+	mpa_exp_mod((mpanum) d,
+			(const mpanum) d_tmp,
+			(const mpanum) b,
+			(const mpanum) c,
+			((mpa_fmm_context)c_mont)->r_ptr,
+			((mpa_fmm_context)c_mont)->r2_ptr,
+			((mpa_fmm_context)c_mont)->n_inv,
+			external_mem_pool);
+	montgomery_deinit(c_mont);
+	if (memguard) {
+		deinit(d_tmp);
+	}
+	return CRYPT_OK;
+}
+
+static int isprime(void *a, int b, int *c)
+{
+	LTC_ARGCHK(a != NULL);
+	LTC_ARGCHK(c != NULL);
+	LTC_UNUSED_PARAM(b);
+	*c = mpa_is_prob_prime((mpanum) a, 100, external_mem_pool) != 0 ? LTC_MP_YES : LTC_MP_NO;
+	return CRYPT_OK;
+}
+
+ltc_math_descriptor ltc_mp = {
+	.name = "MPA",
+	.bits_per_digit = MPA_WORD_SIZE,
+
+	.init = &init,
+	.init_size = &init_size,
+	.init_copy = &init_copy,
+	.deinit = &deinit,
+
+	.neg = &neg,
+	.copy = &copy,
+
+	.set_int = &set_int,
+	.get_int = &get_int,
+	.get_digit = &get_digit,
+	.get_digit_count = &get_digit_count,
+	.compare = &compare,
+	.compare_d = &compare_d,
+	.count_bits = &count_bits,
+	.count_lsb_bits = &count_lsb_bits,
+	.twoexpt = &twoexpt,
+
+	.read_radix = &read_radix,
+	.write_radix = &write_radix,
+	.unsigned_size = &unsigned_size,
+	.unsigned_write = &unsigned_write,
+	.unsigned_read = &unsigned_read,
+
+	.add = &add,
+	.addi = &addi,
+	.sub = &sub,
+	.subi = &subi,
+	.mul = &mul,
+	.muli = &muli,
+	.sqr = &sqr,
+	.mpdiv = &divide,
+	.div_2 = &div_2,
+	.modi = &modi,
+	.gcd = &gcd,
+	.lcm = &lcm,
+
+	.mod = &mod,
+	.mulmod = &mulmod,
+	.sqrmod = &sqrmod,
+	.invmod = &invmod,
+
+	.montgomery_setup = &montgomery_setup,
+	.montgomery_normalization = &montgomery_normalization,
+	.montgomery_reduce = &montgomery_reduce,
+	.montgomery_deinit = &montgomery_deinit,
+
+	.exptmod = &exptmod,
+	.isprime = &isprime,
+
+#ifdef LTC_MECC
+#ifdef LTC_MECC_FP
+	.ecc_ptmul = &ltc_ecc_fp_mulmod,
+#else
+	.ecc_ptmul = &ltc_ecc_mulmod,
+#endif /* LTC_MECC_FP */
+	.ecc_ptadd = &ltc_ecc_projective_add_point,
+	.ecc_ptdbl = &ltc_ecc_projective_dbl_point,
+	.ecc_map = &ltc_ecc_map,
+#ifdef LTC_ECC_SHAMIR
+#ifdef LTC_MECC_FP
+	.ecc_mul2add = &ltc_ecc_fp_mul2add,
+#else
+	.ecc_mul2add = &ltc_ecc_mul2add,
+#endif /* LTC_MECC_FP */
+#endif /* LTC_ECC_SHAMIR */
+#endif /* LTC_MECC */
+
+#ifdef LTC_MRSA
+	.rsa_keygen = &rsa_make_key,
+	.rsa_me = &rsa_exptmod,
+#endif
+
+};
diff --git a/core/lib/libtomcrypt/src/pk/asn1/der/bit/der_decode_bit_string.c b/core/lib/libtomcrypt/src/pk/asn1/der/bit/der_decode_bit_string.c
new file mode 100644
index 0000000..9d16d90
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/asn1/der/bit/der_decode_bit_string.c
@@ -0,0 +1,129 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file der_decode_bit_string.c
+  ASN.1 DER, encode a BIT STRING, Tom St Denis
+*/
+
+
+#ifdef LTC_DER
+
+/**
+  Store a BIT STRING
+  @param in      The DER encoded BIT STRING
+  @param inlen   The size of the DER BIT STRING
+  @param out     [out] The array of bits stored (one per char)
+  @param outlen  [in/out] The number of bits stored
+  @return CRYPT_OK if successful
+*/
+int der_decode_bit_string(const unsigned char *in,  unsigned long inlen,
+                                unsigned char *out, unsigned long *outlen)
+{
+   unsigned long dlen, blen, x, y;
+
+   LTC_ARGCHK(in     != NULL);
+   LTC_ARGCHK(out    != NULL);
+   LTC_ARGCHK(outlen != NULL);
+
+   /* packet must be at least 4 bytes */
+   if (inlen < 4) {
+       return CRYPT_INVALID_ARG;
+   }
+
+   /* check for 0x03 */
+   if ((in[0]&0x1F) != 0x03) {
+      return CRYPT_INVALID_PACKET;
+   }
+
+    /* offset in the data */
+    x = 1;
+
+   /* get the length of the data */
+   if (in[x] & 0x80) {
+      /* long format get number of length bytes */
+      y = in[x++] & 0x7F;
+
+      /* invalid if 0 or > 2 */
+      if (y == 0 || y > 2) {
+         return CRYPT_INVALID_PACKET;
+      }
+
+      /* read the data len */
+      dlen = 0;
+      while (y--) {
+         dlen = (dlen << 8) | (unsigned long)in[x++];
+      }
+   } else {
+      /* short format */
+      dlen = in[x++] & 0x7F;
+   }
+  
+   /* is the data len too long or too short? */
+   if ((dlen == 0) || (dlen + x > inlen)) {
+       return CRYPT_INVALID_PACKET;
+   }
+
+   /* get padding count */
+   blen = ((dlen - 1) << 3) - (in[x++] & 7);
+
+   /* too many bits? */
+   if (blen > *outlen) {
+      *outlen = blen;
+      return CRYPT_BUFFER_OVERFLOW;
+   }
+
+   /* decode/store the bits */
+   for (y = 0; y < blen; y++) {
+       out[y] = (in[x] & (1 << (7 - (y & 7)))) ? 1 : 0;
+       if ((y & 7) == 7) {
+          ++x;
+       }
+   }
+
+   /* we done */
+   *outlen = blen;
+   return CRYPT_OK;
+}
+
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/pk/asn1/der/bit/der_decode_bit_string.c,v $ */
+/* $Revision: 1.5 $ */
+/* $Date: 2006/12/28 01:27:24 $ */
diff --git a/core/lib/libtomcrypt/src/pk/asn1/der/bit/der_decode_raw_bit_string.c b/core/lib/libtomcrypt/src/pk/asn1/der/bit/der_decode_raw_bit_string.c
new file mode 100644
index 0000000..a857933
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/asn1/der/bit/der_decode_raw_bit_string.c
@@ -0,0 +1,133 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file der_decode_bit_string.c
+  ASN.1 DER, encode a BIT STRING, Tom St Denis
+*/
+
+
+#ifdef LTC_DER
+
+#define SETBIT(v, n)    (v=((unsigned char)(v) | (1U << (unsigned char)(n))))
+
+/**
+  Store a BIT STRING
+  @param in      The DER encoded BIT STRING
+  @param inlen   The size of the DER BIT STRING
+  @param out     [out] The array of bits stored (8 per char)
+  @param outlen  [in/out] The number of bits stored
+  @return CRYPT_OK if successful
+*/
+int der_decode_raw_bit_string(const unsigned char *in,  unsigned long inlen,
+                                unsigned char *out, unsigned long *outlen)
+{
+   unsigned long dlen, blen, x, y;
+
+   LTC_ARGCHK(in     != NULL);
+   LTC_ARGCHK(out    != NULL);
+   LTC_ARGCHK(outlen != NULL);
+
+   /* packet must be at least 4 bytes */
+   if (inlen < 4) {
+       return CRYPT_INVALID_ARG;
+   }
+
+   /* check for 0x03 */
+   if ((in[0]&0x1F) != 0x03) {
+      return CRYPT_INVALID_PACKET;
+   }
+
+    /* offset in the data */
+    x = 1;
+
+   /* get the length of the data */
+   if (in[x] & 0x80) {
+      /* long format get number of length bytes */
+      y = in[x++] & 0x7F;
+
+      /* invalid if 0 or > 2 */
+      if (y == 0 || y > 2) {
+         return CRYPT_INVALID_PACKET;
+      }
+
+      /* read the data len */
+      dlen = 0;
+      while (y--) {
+         dlen = (dlen << 8) | (unsigned long)in[x++];
+      }
+   } else {
+      /* short format */
+      dlen = in[x++] & 0x7F;
+   }
+
+   /* is the data len too long or too short? */
+   if ((dlen == 0) || (dlen + x > inlen)) {
+       return CRYPT_INVALID_PACKET;
+   }
+
+   /* get padding count */
+   blen = ((dlen - 1) << 3) - (in[x++] & 7);
+
+   /* too many bits? */
+   if (blen > *outlen) {
+      *outlen = blen;
+      return CRYPT_BUFFER_OVERFLOW;
+   }
+
+   /* decode/store the bits */
+   for (y = 0; y < blen; y++) {
+       if (in[x] & (1 << (7 - (y & 7)))) {
+          SETBIT(out[y/8], 7-(y%8));
+       }
+       if ((y & 7) == 7) {
+          ++x;
+       }
+   }
+
+   /* we done */
+   *outlen = blen;
+   return CRYPT_OK;
+}
+
+#endif
+
+/* $Source$ */
+/* $Revision$ */
+/* $Date$ */
diff --git a/core/lib/libtomcrypt/src/pk/asn1/der/bit/der_encode_bit_string.c b/core/lib/libtomcrypt/src/pk/asn1/der/bit/der_encode_bit_string.c
new file mode 100644
index 0000000..1776fc0
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/asn1/der/bit/der_encode_bit_string.c
@@ -0,0 +1,116 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file der_encode_bit_string.c
+  ASN.1 DER, encode a BIT STRING, Tom St Denis
+*/
+
+
+#ifdef LTC_DER
+
+/**
+  Store a BIT STRING
+  @param in       The array of bits to store (one per char)
+  @param inlen    The number of bits tostore
+  @param out      [out] The destination for the DER encoded BIT STRING
+  @param outlen   [in/out] The max size and resulting size of the DER BIT STRING
+  @return CRYPT_OK if successful
+*/
+int der_encode_bit_string(const unsigned char *in, unsigned long inlen,
+                                unsigned char *out, unsigned long *outlen)
+{
+   unsigned long len, x, y;
+   unsigned char buf;
+   int           err;
+
+   LTC_ARGCHK(in     != NULL);
+   LTC_ARGCHK(out    != NULL);
+   LTC_ARGCHK(outlen != NULL);
+
+   /* avoid overflows */
+   if ((err = der_length_bit_string(inlen, &len)) != CRYPT_OK) {
+      return err;
+   }
+
+   if (len > *outlen) {
+      *outlen = len;
+      return CRYPT_BUFFER_OVERFLOW;
+   }
+
+   /* store header (include bit padding count in length) */
+   x = 0;
+   y = (inlen >> 3) + ((inlen&7) ? 1 : 0) + 1;
+
+   out[x++] = 0x03;
+   if (y < 128) {
+      out[x++] = (unsigned char)y;
+   } else if (y < 256) {
+      out[x++] = 0x81;
+      out[x++] = (unsigned char)y;
+   } else if (y < 65536) {
+      out[x++] = 0x82;
+      out[x++] = (unsigned char)((y>>8)&255);
+      out[x++] = (unsigned char)(y&255);
+   }
+
+   /* store number of zero padding bits */
+   out[x++] = (unsigned char)((8 - inlen) & 7);
+
+   /* store the bits in big endian format */
+   for (y = buf = 0; y < inlen; y++) {
+       buf |= (in[y] ? 1 : 0) << (7 - (y & 7));
+       if ((y & 7) == 7) {
+          out[x++] = buf;
+          buf      = 0;
+       }
+   }
+   /* store last byte */
+   if (inlen & 7) {
+      out[x++] = buf;
+   }
+   *outlen = x;
+   return CRYPT_OK;
+}
+
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/pk/asn1/der/bit/der_encode_bit_string.c,v $ */
+/* $Revision: 1.5 $ */
+/* $Date: 2006/12/28 01:27:24 $ */
diff --git a/core/lib/libtomcrypt/src/pk/asn1/der/bit/der_encode_raw_bit_string.c b/core/lib/libtomcrypt/src/pk/asn1/der/bit/der_encode_raw_bit_string.c
new file mode 100644
index 0000000..8035e44
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/asn1/der/bit/der_encode_raw_bit_string.c
@@ -0,0 +1,119 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file der_encode_bit_string.c
+  ASN.1 DER, encode a BIT STRING, Tom St Denis
+*/
+
+
+#ifdef LTC_DER
+
+#define getbit(n, k) (((n) & ( 1 << (k) )) >> (k))
+
+/**
+  Store a BIT STRING
+  @param in       The array of bits to store (8 per char)
+  @param inlen    The number of bits tostore
+  @param out      [out] The destination for the DER encoded BIT STRING
+  @param outlen   [in/out] The max size and resulting size of the DER BIT STRING
+  @return CRYPT_OK if successful
+*/
+int der_encode_raw_bit_string(const unsigned char *in, unsigned long inlen,
+                                unsigned char *out, unsigned long *outlen)
+{
+   unsigned long len, x, y;
+   unsigned char buf;
+   int           err;
+
+   LTC_ARGCHK(in     != NULL);
+   LTC_ARGCHK(out    != NULL);
+   LTC_ARGCHK(outlen != NULL);
+
+   /* avoid overflows */
+   if ((err = der_length_bit_string(inlen, &len)) != CRYPT_OK) {
+      return err;
+   }
+
+   if (len > *outlen) {
+      *outlen = len;
+      return CRYPT_BUFFER_OVERFLOW;
+   }
+
+   /* store header (include bit padding count in length) */
+   x = 0;
+   y = (inlen >> 3) + ((inlen&7) ? 1 : 0) + 1;
+
+   out[x++] = 0x03;
+   if (y < 128) {
+      out[x++] = (unsigned char)y;
+   } else if (y < 256) {
+      out[x++] = 0x81;
+      out[x++] = (unsigned char)y;
+   } else if (y < 65536) {
+      out[x++] = 0x82;
+      out[x++] = (unsigned char)((y>>8)&255);
+      out[x++] = (unsigned char)(y&255);
+   }
+
+   /* store number of zero padding bits */
+   out[x++] = (unsigned char)((8 - inlen) & 7);
+
+   /* store the bits in big endian format */
+   for (y = buf = 0; y < inlen; y++) {
+        buf |= (getbit(in[y/8],7-y%8)?1:0) << (7 - (y & 7));
+       if ((y & 7) == 7) {
+          out[x++] = buf;
+          buf      = 0;
+       }
+   }
+   /* store last byte */
+   if (inlen & 7) {
+      out[x++] = buf;
+   }
+
+   *outlen = x;
+   return CRYPT_OK;
+}
+
+#endif
+
+/* $Source$ */
+/* $Revision$ */
+/* $Date$ */
diff --git a/core/lib/libtomcrypt/src/pk/asn1/der/bit/der_length_bit_string.c b/core/lib/libtomcrypt/src/pk/asn1/der/bit/der_length_bit_string.c
new file mode 100644
index 0000000..3966716
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/asn1/der/bit/der_length_bit_string.c
@@ -0,0 +1,81 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file der_length_bit_string.c
+  ASN.1 DER, get length of BIT STRING, Tom St Denis
+*/
+
+#ifdef LTC_DER
+/**
+  Gets length of DER encoding of BIT STRING 
+  @param nbits  The number of bits in the string to encode
+  @param outlen [out] The length of the DER encoding for the given string
+  @return CRYPT_OK if successful
+*/
+int der_length_bit_string(unsigned long nbits, unsigned long *outlen)
+{
+   unsigned long nbytes;
+   LTC_ARGCHK(outlen != NULL);
+
+   /* get the number of the bytes */
+   nbytes = (nbits >> 3) + ((nbits & 7) ? 1 : 0) + 1;
+ 
+   if (nbytes < 128) {
+      /* 03 LL PP DD DD DD ... */
+      *outlen = 2 + nbytes;
+   } else if (nbytes < 256) {
+      /* 03 81 LL PP DD DD DD ... */
+      *outlen = 3 + nbytes;
+   } else if (nbytes < 65536) {
+      /* 03 82 LL LL PP DD DD DD ... */
+      *outlen = 4 + nbytes;
+   } else {
+      return CRYPT_INVALID_ARG;
+   }
+
+   return CRYPT_OK;
+}
+
+#endif
+
+
+/* $Source: /cvs/libtom/libtomcrypt/src/pk/asn1/der/bit/der_length_bit_string.c,v $ */
+/* $Revision: 1.3 $ */
+/* $Date: 2006/12/28 01:27:24 $ */
diff --git a/core/lib/libtomcrypt/src/pk/asn1/der/bit/sub.mk b/core/lib/libtomcrypt/src/pk/asn1/der/bit/sub.mk
new file mode 100644
index 0000000..4cf829e
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/asn1/der/bit/sub.mk
@@ -0,0 +1,5 @@
+srcs-y += der_decode_bit_string.c
+srcs-y += der_encode_bit_string.c
+srcs-y += der_length_bit_string.c
+srcs-y += der_decode_raw_bit_string.c
+srcs-y += der_encode_raw_bit_string.c
diff --git a/core/lib/libtomcrypt/src/pk/asn1/der/boolean/der_decode_boolean.c b/core/lib/libtomcrypt/src/pk/asn1/der/boolean/der_decode_boolean.c
new file mode 100644
index 0000000..96c31fe
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/asn1/der/boolean/der_decode_boolean.c
@@ -0,0 +1,74 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file der_decode_boolean.c
+  ASN.1 DER, decode a BOOLEAN, Tom St Denis
+*/
+
+
+#ifdef LTC_DER
+
+/**
+  Read a BOOLEAN
+  @param in      The destination for the DER encoded BOOLEAN
+  @param inlen   The size of the DER BOOLEAN
+  @param out     [out]  The boolean to decode
+  @return CRYPT_OK if successful
+*/
+int der_decode_boolean(const unsigned char *in, unsigned long inlen,
+                                       int *out)
+{
+   LTC_ARGCHK(in  != NULL);
+   LTC_ARGCHK(out != NULL);
+   
+   if (inlen < 3 || in[0] != 0x01 || in[1] != 0x01 || (in[2] != 0x00 && in[2] != 0xFF)) {
+      return CRYPT_INVALID_ARG;
+   }
+   
+   *out = (in[2]==0xFF) ? 1 : 0;
+   
+   return CRYPT_OK;
+}
+
+#endif
+
+/* $Source$ */
+/* $Revision$ */
+/* $Date$ */
diff --git a/core/lib/libtomcrypt/src/pk/asn1/der/boolean/der_encode_boolean.c b/core/lib/libtomcrypt/src/pk/asn1/der/boolean/der_encode_boolean.c
new file mode 100644
index 0000000..90bf24b
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/asn1/der/boolean/der_encode_boolean.c
@@ -0,0 +1,78 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file der_encode_boolean.c
+  ASN.1 DER, encode a BOOLEAN, Tom St Denis
+*/
+
+
+#ifdef LTC_DER
+
+/**
+  Store a BOOLEAN
+  @param in       The boolean to encode
+  @param out      [out] The destination for the DER encoded BOOLEAN
+  @param outlen   [in/out] The max size and resulting size of the DER BOOLEAN
+  @return CRYPT_OK if successful
+*/
+int der_encode_boolean(int in, 
+                       unsigned char *out, unsigned long *outlen)
+{
+   LTC_ARGCHK(outlen != NULL);
+   LTC_ARGCHK(out    != NULL);
+   
+   if (*outlen < 3) {
+       *outlen = 3;
+       return CRYPT_BUFFER_OVERFLOW;
+   }
+   
+   *outlen = 3;
+   out[0] = 0x01;
+   out[1] = 0x01;
+   out[2] = in ? 0xFF : 0x00;
+   
+   return CRYPT_OK;
+}
+
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/pk/asn1/der/boolean/der_encode_boolean.c,v $ */
+/* $Revision: 1.4 $ */
+/* $Date: 2006/12/28 01:27:24 $ */
diff --git a/core/lib/libtomcrypt/src/pk/asn1/der/boolean/der_length_boolean.c b/core/lib/libtomcrypt/src/pk/asn1/der/boolean/der_length_boolean.c
new file mode 100644
index 0000000..3f677c5
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/asn1/der/boolean/der_length_boolean.c
@@ -0,0 +1,62 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file der_length_boolean.c
+  ASN.1 DER, get length of a BOOLEAN, Tom St Denis
+*/
+
+#ifdef LTC_DER
+/**
+  Gets length of DER encoding of a BOOLEAN 
+  @param outlen [out] The length of the DER encoding
+  @return CRYPT_OK if successful
+*/
+int der_length_boolean(unsigned long *outlen)
+{
+   LTC_ARGCHK(outlen != NULL);
+   *outlen = 3;
+   return CRYPT_OK;
+}
+
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/pk/asn1/der/boolean/der_length_boolean.c,v $ */
+/* $Revision: 1.3 $ */
+/* $Date: 2006/12/28 01:27:24 $ */
diff --git a/core/lib/libtomcrypt/src/pk/asn1/der/boolean/sub.mk b/core/lib/libtomcrypt/src/pk/asn1/der/boolean/sub.mk
new file mode 100644
index 0000000..3761249
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/asn1/der/boolean/sub.mk
@@ -0,0 +1,3 @@
+srcs-y += der_decode_boolean.c
+srcs-y += der_encode_boolean.c
+srcs-y += der_length_boolean.c
diff --git a/core/lib/libtomcrypt/src/pk/asn1/der/choice/der_decode_choice.c b/core/lib/libtomcrypt/src/pk/asn1/der/choice/der_decode_choice.c
new file mode 100644
index 0000000..b2b8bc0
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/asn1/der/choice/der_decode_choice.c
@@ -0,0 +1,245 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file der_decode_choice.c
+  ASN.1 DER, decode a CHOICE, Tom St Denis
+*/
+
+#ifdef LTC_DER
+
+/**
+   Decode a CHOICE
+   @param in       The DER encoded input
+   @param inlen    [in/out] The size of the input and resulting size of read type
+   @param list     The list of items to decode
+   @param outlen   The number of items in the list
+   @return CRYPT_OK on success
+*/
+int der_decode_choice(const unsigned char *in,   unsigned long *inlen,
+                            ltc_asn1_list *list, unsigned long  outlen)
+{
+   unsigned long size, x, z;
+   void          *data;
+
+   LTC_ARGCHK(in    != NULL);
+   LTC_ARGCHK(inlen != NULL);
+   LTC_ARGCHK(list  != NULL);
+
+   /* get blk size */
+   if (*inlen < 2) {
+      return CRYPT_INVALID_PACKET;
+   }
+
+   /* set all of the "used" flags to zero */
+   for (x = 0; x < outlen; x++) {
+       list[x].used = 0;
+   }
+
+   /* now scan until we have a winner */
+   for (x = 0; x < outlen; x++) {
+       size = list[x].size;
+       data = list[x].data;
+
+       switch (list[x].type) {
+           case LTC_ASN1_BOOLEAN:
+               if (der_decode_boolean(in, *inlen, data) == CRYPT_OK) {
+                  if (der_length_boolean(&z) == CRYPT_OK) {
+                      list[x].used = 1;
+                      *inlen       = z;
+                      return CRYPT_OK;
+                  }
+               }
+               break;
+
+           case LTC_ASN1_INTEGER:
+               if (der_decode_integer(in, *inlen, data) == CRYPT_OK) {
+                  if (der_length_integer(data, &z) == CRYPT_OK) {
+                      list[x].used = 1;
+                      *inlen       = z;
+                      return CRYPT_OK;
+                  }
+               }
+               break;
+
+           case LTC_ASN1_SHORT_INTEGER:
+               if (der_decode_short_integer(in, *inlen, data) == CRYPT_OK) {
+                  if (der_length_short_integer(size, &z) == CRYPT_OK) {
+                      list[x].used = 1;
+                      *inlen       = z;
+                      return CRYPT_OK;
+                  }
+               }
+               break;
+
+           case LTC_ASN1_BIT_STRING:
+               if (der_decode_bit_string(in, *inlen, data, &size) == CRYPT_OK) {
+                  if (der_length_bit_string(size, &z) == CRYPT_OK) {
+                     list[x].used = 1;
+                     list[x].size = size;
+                     *inlen       = z;
+                     return CRYPT_OK;
+                  }
+               }
+               break;
+
+           case LTC_ASN1_RAW_BIT_STRING:
+               if (der_decode_raw_bit_string(in, *inlen, data, &size) == CRYPT_OK) {
+                  if (der_length_bit_string(size, &z) == CRYPT_OK) {
+                     list[x].used = 1;
+                     list[x].size = size;
+                     *inlen       = z;
+                     return CRYPT_OK;
+                  }
+               }
+               break;
+
+           case LTC_ASN1_OCTET_STRING:
+               if (der_decode_octet_string(in, *inlen, data, &size) == CRYPT_OK) {
+                  if (der_length_octet_string(size, &z) == CRYPT_OK) {
+                     list[x].used = 1;
+                     list[x].size = size;
+                     *inlen       = z;
+                     return CRYPT_OK;
+                  }
+               }
+               break;
+
+           case LTC_ASN1_NULL:
+               if (*inlen == 2 && in[x] == 0x05 && in[x+1] == 0x00) {
+                  *inlen = 2;
+                  list[x].used   = 1;
+                  return CRYPT_OK;
+               }
+               break;
+
+           case LTC_ASN1_OBJECT_IDENTIFIER:
+               if (der_decode_object_identifier(in, *inlen, data, &size) == CRYPT_OK) {
+                  if (der_length_object_identifier(data, size, &z) == CRYPT_OK) {
+                     list[x].used = 1;
+                     list[x].size = size;
+                     *inlen       = z;
+                     return CRYPT_OK;
+                  }
+               }
+               break;
+
+           case LTC_ASN1_TELETEX_STRING:
+               if (der_decode_teletex_string(in, *inlen, data, &size) == CRYPT_OK) {
+                  if (der_length_teletex_string(data, size, &z) == CRYPT_OK) {
+                     list[x].used = 1;
+                     list[x].size = size;
+                     *inlen       = z;
+                     return CRYPT_OK;
+                  }
+               }
+               break;
+
+           case LTC_ASN1_IA5_STRING:
+               if (der_decode_ia5_string(in, *inlen, data, &size) == CRYPT_OK) {
+                  if (der_length_ia5_string(data, size, &z) == CRYPT_OK) {
+                     list[x].used = 1;
+                     list[x].size = size;
+                     *inlen       = z;
+                     return CRYPT_OK;
+                  }
+               }
+               break;
+
+           case LTC_ASN1_PRINTABLE_STRING:
+               if (der_decode_printable_string(in, *inlen, data, &size) == CRYPT_OK) {
+                  if (der_length_printable_string(data, size, &z) == CRYPT_OK) {
+                     list[x].used = 1;
+                     list[x].size = size;
+                     *inlen       = z;
+                     return CRYPT_OK;
+                  }
+               }
+               break;
+
+           case LTC_ASN1_UTF8_STRING:
+               if (der_decode_utf8_string(in, *inlen, data, &size) == CRYPT_OK) {
+                  if (der_length_utf8_string(data, size, &z) == CRYPT_OK) {
+                     list[x].used = 1;
+                     list[x].size = size;
+                     *inlen       = z;
+                     return CRYPT_OK;
+                  }
+               }
+               break;
+
+           case LTC_ASN1_UTCTIME:
+               z = *inlen;
+               if (der_decode_utctime(in, &z, data) == CRYPT_OK) {
+                  list[x].used = 1;
+                  *inlen       = z;
+                  return CRYPT_OK;
+               }
+               break;
+
+           case LTC_ASN1_SET:
+           case LTC_ASN1_SETOF:
+           case LTC_ASN1_SEQUENCE:
+               if (der_decode_sequence(in, *inlen, data, size) == CRYPT_OK) {
+                  if (der_length_sequence(data, size, &z) == CRYPT_OK) {
+                     list[x].used = 1;
+                     *inlen       = z;
+                     return CRYPT_OK;
+                  }
+               }
+               break;
+
+           case LTC_ASN1_CHOICE:
+           case LTC_ASN1_CONSTRUCTED:
+           case LTC_ASN1_CONTEXT_SPECIFIC:
+           case LTC_ASN1_EOL:
+               return CRYPT_INVALID_ARG;
+           default:
+               return CRYPT_INVALID_ARG;
+       }
+   }
+
+   return CRYPT_INVALID_PACKET;
+}
+
+#endif
+
+/* $Source$ */
+/* $Revision$ */
+/* $Date$ */
diff --git a/core/lib/libtomcrypt/src/pk/asn1/der/choice/sub.mk b/core/lib/libtomcrypt/src/pk/asn1/der/choice/sub.mk
new file mode 100644
index 0000000..d3b95ec
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/asn1/der/choice/sub.mk
@@ -0,0 +1 @@
+srcs-y += der_decode_choice.c
diff --git a/core/lib/libtomcrypt/src/pk/asn1/der/ia5/der_decode_ia5_string.c b/core/lib/libtomcrypt/src/pk/asn1/der/ia5/der_decode_ia5_string.c
new file mode 100644
index 0000000..d1dc468
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/asn1/der/ia5/der_decode_ia5_string.c
@@ -0,0 +1,123 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file der_decode_ia5_string.c
+  ASN.1 DER, encode a IA5 STRING, Tom St Denis
+*/
+
+
+#ifdef LTC_DER
+
+/**
+  Store a IA5 STRING
+  @param in      The DER encoded IA5 STRING
+  @param inlen   The size of the DER IA5 STRING
+  @param out     [out] The array of octets stored (one per char)
+  @param outlen  [in/out] The number of octets stored
+  @return CRYPT_OK if successful
+*/
+int der_decode_ia5_string(const unsigned char *in, unsigned long inlen,
+                                unsigned char *out, unsigned long *outlen)
+{
+   unsigned long x, y, len;
+   int           t;
+
+   LTC_ARGCHK(in     != NULL);
+   LTC_ARGCHK(out    != NULL);
+   LTC_ARGCHK(outlen != NULL);
+
+   /* must have header at least */
+   if (inlen < 2) {
+      return CRYPT_INVALID_PACKET;
+   }
+
+   /* check for 0x16 */
+   if ((in[0] & 0x1F) != 0x16) {
+      return CRYPT_INVALID_PACKET;
+   }
+   x = 1;
+
+   /* decode the length */
+   if (in[x] & 0x80) {
+      /* valid # of bytes in length are 1,2,3 */
+      y = in[x] & 0x7F;
+      if ((y == 0) || (y > 3) || ((x + y) > inlen)) {
+         return CRYPT_INVALID_PACKET;
+      }
+
+      /* read the length in */
+      len = 0;
+      ++x;
+      while (y--) {
+         len = (len << 8) | in[x++];
+      }
+   } else {
+      len = in[x++] & 0x7F;
+   }
+
+   /* is it too long? */
+   if (len > *outlen) {
+      *outlen = len;
+      return CRYPT_BUFFER_OVERFLOW;
+   }
+
+   if (len + x > inlen) {
+      return CRYPT_INVALID_PACKET;
+   }
+
+   /* read the data */
+   for (y = 0; y < len; y++) {
+       t = der_ia5_value_decode(in[x++]);
+       if (t == -1) {
+           return CRYPT_INVALID_ARG;
+       }
+       out[y] = t;
+   }
+
+   *outlen = y;
+
+   return CRYPT_OK;
+}
+ 
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/pk/asn1/der/ia5/der_decode_ia5_string.c,v $ */
+/* $Revision: 1.4 $ */
+/* $Date: 2006/12/28 01:27:24 $ */
diff --git a/core/lib/libtomcrypt/src/pk/asn1/der/ia5/der_encode_ia5_string.c b/core/lib/libtomcrypt/src/pk/asn1/der/ia5/der_encode_ia5_string.c
new file mode 100644
index 0000000..17a6354
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/asn1/der/ia5/der_encode_ia5_string.c
@@ -0,0 +1,112 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file der_encode_ia5_string.c
+  ASN.1 DER, encode a IA5 STRING, Tom St Denis
+*/
+
+#ifdef LTC_DER
+
+/**
+  Store an IA5 STRING
+  @param in       The array of IA5 to store (one per char)
+  @param inlen    The number of IA5 to store
+  @param out      [out] The destination for the DER encoded IA5 STRING
+  @param outlen   [in/out] The max size and resulting size of the DER IA5 STRING
+  @return CRYPT_OK if successful
+*/
+int der_encode_ia5_string(const unsigned char *in, unsigned long inlen,
+                                unsigned char *out, unsigned long *outlen)
+{
+   unsigned long x, y, len;
+   int           err;
+
+   LTC_ARGCHK(in     != NULL);
+   LTC_ARGCHK(out    != NULL);
+   LTC_ARGCHK(outlen != NULL);
+
+   /* get the size */
+   if ((err = der_length_ia5_string(in, inlen, &len)) != CRYPT_OK) {
+      return err; 
+   }
+
+   /* too big? */
+   if (len > *outlen) {
+      *outlen = len;
+      return CRYPT_BUFFER_OVERFLOW;
+   }
+
+   /* encode the header+len */
+   x = 0;
+   out[x++] = 0x16;
+   if (inlen < 128) {
+      out[x++] = (unsigned char)inlen;
+   } else if (inlen < 256) {
+      out[x++] = 0x81;
+      out[x++] = (unsigned char)inlen;
+   } else if (inlen < 65536UL) {
+      out[x++] = 0x82;
+      out[x++] = (unsigned char)((inlen>>8)&255);
+      out[x++] = (unsigned char)(inlen&255);
+   } else if (inlen < 16777216UL) {
+      out[x++] = 0x83;
+      out[x++] = (unsigned char)((inlen>>16)&255);
+      out[x++] = (unsigned char)((inlen>>8)&255);
+      out[x++] = (unsigned char)(inlen&255);
+   } else {
+      return CRYPT_INVALID_ARG;
+   }
+
+   /* store octets */
+   for (y = 0; y < inlen; y++) {
+       out[x++] = der_ia5_char_encode(in[y]);
+   }
+
+   /* retun length */
+   *outlen = x;
+
+   return CRYPT_OK;
+}
+
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/pk/asn1/der/ia5/der_encode_ia5_string.c,v $ */
+/* $Revision: 1.5 $ */
+/* $Date: 2006/12/28 01:27:24 $ */
diff --git a/core/lib/libtomcrypt/src/pk/asn1/der/ia5/der_length_ia5_string.c b/core/lib/libtomcrypt/src/pk/asn1/der/ia5/der_length_ia5_string.c
new file mode 100644
index 0000000..7acea72
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/asn1/der/ia5/der_length_ia5_string.c
@@ -0,0 +1,221 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file der_length_ia5_string.c
+  ASN.1 DER, get length of IA5 STRING, Tom St Denis
+*/
+
+#ifdef LTC_DER
+
+static const struct {
+   int code, value;
+} ia5_table[] = {
+{ '\0', 0 },
+{ '\a', 7 }, 
+{ '\b', 8 }, 
+{ '\t', 9 }, 
+{ '\n', 10 }, 
+{ '\f', 12 }, 
+{ '\r', 13 }, 
+{ ' ', 32 }, 
+{ '!', 33 }, 
+{ '"', 34 }, 
+{ '#', 35 }, 
+{ '$', 36 }, 
+{ '%', 37 }, 
+{ '&', 38 }, 
+{ '\'', 39 }, 
+{ '(', 40 }, 
+{ ')', 41 }, 
+{ '*', 42 }, 
+{ '+', 43 }, 
+{ ',', 44 }, 
+{ '-', 45 }, 
+{ '.', 46 }, 
+{ '/', 47 }, 
+{ '0', 48 }, 
+{ '1', 49 }, 
+{ '2', 50 }, 
+{ '3', 51 }, 
+{ '4', 52 }, 
+{ '5', 53 }, 
+{ '6', 54 }, 
+{ '7', 55 }, 
+{ '8', 56 }, 
+{ '9', 57 }, 
+{ ':', 58 }, 
+{ ';', 59 }, 
+{ '<', 60 }, 
+{ '=', 61 }, 
+{ '>', 62 }, 
+{ '?', 63 }, 
+{ '@', 64 }, 
+{ 'A', 65 }, 
+{ 'B', 66 }, 
+{ 'C', 67 }, 
+{ 'D', 68 }, 
+{ 'E', 69 }, 
+{ 'F', 70 }, 
+{ 'G', 71 }, 
+{ 'H', 72 }, 
+{ 'I', 73 }, 
+{ 'J', 74 }, 
+{ 'K', 75 }, 
+{ 'L', 76 }, 
+{ 'M', 77 }, 
+{ 'N', 78 }, 
+{ 'O', 79 }, 
+{ 'P', 80 }, 
+{ 'Q', 81 }, 
+{ 'R', 82 }, 
+{ 'S', 83 }, 
+{ 'T', 84 }, 
+{ 'U', 85 }, 
+{ 'V', 86 }, 
+{ 'W', 87 }, 
+{ 'X', 88 }, 
+{ 'Y', 89 }, 
+{ 'Z', 90 }, 
+{ '[', 91 }, 
+{ '\\', 92 }, 
+{ ']', 93 }, 
+{ '^', 94 }, 
+{ '_', 95 }, 
+{ '`', 96 }, 
+{ 'a', 97 }, 
+{ 'b', 98 }, 
+{ 'c', 99 }, 
+{ 'd', 100 }, 
+{ 'e', 101 }, 
+{ 'f', 102 }, 
+{ 'g', 103 }, 
+{ 'h', 104 }, 
+{ 'i', 105 }, 
+{ 'j', 106 }, 
+{ 'k', 107 }, 
+{ 'l', 108 }, 
+{ 'm', 109 }, 
+{ 'n', 110 }, 
+{ 'o', 111 }, 
+{ 'p', 112 }, 
+{ 'q', 113 }, 
+{ 'r', 114 }, 
+{ 's', 115 }, 
+{ 't', 116 }, 
+{ 'u', 117 }, 
+{ 'v', 118 }, 
+{ 'w', 119 }, 
+{ 'x', 120 }, 
+{ 'y', 121 }, 
+{ 'z', 122 }, 
+{ '{', 123 }, 
+{ '|', 124 }, 
+{ '}', 125 }, 
+{ '~', 126 }
+};
+
+int der_ia5_char_encode(int c)
+{
+   int x;
+   for (x = 0; x < (int)(sizeof(ia5_table)/sizeof(ia5_table[0])); x++) {
+       if (ia5_table[x].code == c) {
+          return ia5_table[x].value;
+       }
+   }
+   return -1;
+}
+
+int der_ia5_value_decode(int v)
+{
+   int x;
+   for (x = 0; x < (int)(sizeof(ia5_table)/sizeof(ia5_table[0])); x++) {
+       if (ia5_table[x].value == v) {
+          return ia5_table[x].code;
+       }
+   }
+   return -1;
+}
+   
+/**
+  Gets length of DER encoding of IA5 STRING 
+  @param octets   The values you want to encode 
+  @param noctets  The number of octets in the string to encode
+  @param outlen   [out] The length of the DER encoding for the given string
+  @return CRYPT_OK if successful
+*/
+int der_length_ia5_string(const unsigned char *octets, unsigned long noctets, unsigned long *outlen)
+{
+   unsigned long x;
+
+   LTC_ARGCHK(outlen != NULL);
+   LTC_ARGCHK(octets != NULL);
+
+   /* scan string for validity */
+   for (x = 0; x < noctets; x++) {
+       if (der_ia5_char_encode(octets[x]) == -1) {
+          return CRYPT_INVALID_ARG;
+       }
+   }
+
+   if (noctets < 128) {
+      /* 16 LL DD DD DD ... */
+      *outlen = 2 + noctets;
+   } else if (noctets < 256) {
+      /* 16 81 LL DD DD DD ... */
+      *outlen = 3 + noctets;
+   } else if (noctets < 65536UL) {
+      /* 16 82 LL LL DD DD DD ... */
+      *outlen = 4 + noctets;
+   } else if (noctets < 16777216UL) {
+      /* 16 83 LL LL LL DD DD DD ... */
+      *outlen = 5 + noctets;
+   } else {
+      return CRYPT_INVALID_ARG;
+   }
+
+   return CRYPT_OK;
+}
+
+#endif
+
+
+/* $Source: /cvs/libtom/libtomcrypt/src/pk/asn1/der/ia5/der_length_ia5_string.c,v $ */
+/* $Revision: 1.3 $ */
+/* $Date: 2006/12/28 01:27:24 $ */
diff --git a/core/lib/libtomcrypt/src/pk/asn1/der/ia5/sub.mk b/core/lib/libtomcrypt/src/pk/asn1/der/ia5/sub.mk
new file mode 100644
index 0000000..dc32a5d
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/asn1/der/ia5/sub.mk
@@ -0,0 +1,3 @@
+srcs-y += der_decode_ia5_string.c
+srcs-y += der_encode_ia5_string.c
+srcs-y += der_length_ia5_string.c
diff --git a/core/lib/libtomcrypt/src/pk/asn1/der/integer/der_decode_integer.c b/core/lib/libtomcrypt/src/pk/asn1/der/integer/der_decode_integer.c
new file mode 100644
index 0000000..195ba0f
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/asn1/der/integer/der_decode_integer.c
@@ -0,0 +1,137 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file der_decode_integer.c
+  ASN.1 DER, decode an integer, Tom St Denis
+*/
+
+
+#ifdef LTC_DER
+
+/**
+  Read a mp_int integer
+  @param in       The DER encoded data
+  @param inlen    Size of DER encoded data
+  @param num      The first mp_int to decode
+  @return CRYPT_OK if successful
+*/
+int der_decode_integer(const unsigned char *in, unsigned long inlen, void *num)
+{
+   unsigned long x, y, z;
+   int           err;
+
+   LTC_ARGCHK(num    != NULL);
+   LTC_ARGCHK(in     != NULL);
+
+   /* min DER INTEGER is 0x02 01 00 == 0 */
+   if (inlen < (1 + 1 + 1)) {
+      return CRYPT_INVALID_PACKET;
+   }
+
+   /* ok expect 0x02 when we AND with 0001 1111 [1F] */
+   x = 0;
+   if ((in[x++] & 0x1F) != 0x02) {
+      return CRYPT_INVALID_PACKET;
+   }
+
+   /* now decode the len stuff */
+   z = in[x++];
+
+   if ((z & 0x80) == 0x00) {
+      /* short form */
+
+      /* will it overflow? */
+      if (x + z > inlen) {
+         return CRYPT_INVALID_PACKET;
+      }
+     
+      /* no so read it */
+      if ((err = mp_read_unsigned_bin(num, (unsigned char *)in + x, z)) != CRYPT_OK) {
+         return err;
+      }
+   } else {
+      /* long form */
+      z &= 0x7F;
+      
+      /* will number of length bytes overflow? (or > 4) */
+      if (((x + z) > inlen) || (z > 4) || (z == 0)) {
+         return CRYPT_INVALID_PACKET;
+      }
+
+      /* now read it in */
+      y = 0;
+      while (z--) {
+         y = ((unsigned long)(in[x++])) | (y << 8);
+      }
+
+      /* now will reading y bytes overrun? */
+      if ((x + y) > inlen) {
+         return CRYPT_INVALID_PACKET;
+      }
+
+      /* no so read it */
+      if ((err = mp_read_unsigned_bin(num, (unsigned char *)in + x, y)) != CRYPT_OK) {
+         return err;
+      }
+   }
+
+   /* see if it's negative */
+   if (in[x] & 0x80) {
+      void *tmp;
+      if (mp_init(&tmp) != CRYPT_OK) {
+         return CRYPT_MEM;
+      }
+
+      if (mp_2expt(tmp, mp_count_bits(num)) != CRYPT_OK || mp_sub(num, tmp, num) != CRYPT_OK) {
+         mp_clear(tmp);
+         return CRYPT_MEM;
+      }
+      mp_clear(tmp);
+   } 
+
+   return CRYPT_OK;
+
+}
+
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/pk/asn1/der/integer/der_decode_integer.c,v $ */
+/* $Revision: 1.5 $ */
+/* $Date: 2006/12/28 01:27:24 $ */
diff --git a/core/lib/libtomcrypt/src/pk/asn1/der/integer/der_encode_integer.c b/core/lib/libtomcrypt/src/pk/asn1/der/integer/der_encode_integer.c
new file mode 100644
index 0000000..466522b
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/asn1/der/integer/der_encode_integer.c
@@ -0,0 +1,157 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file der_encode_integer.c
+  ASN.1 DER, encode an integer, Tom St Denis
+*/
+
+
+#ifdef LTC_DER
+
+/* Exports a positive bignum as DER format (upto 2^32 bytes in size) */
+/**
+  Store a mp_int integer
+  @param num      The first mp_int to encode
+  @param out      [out] The destination for the DER encoded integers
+  @param outlen   [in/out] The max size and resulting size of the DER encoded integers
+  @return CRYPT_OK if successful
+*/
+int der_encode_integer(void *num, unsigned char *out, unsigned long *outlen)
+{  
+   unsigned long tmplen, y;
+   int           err, leading_zero;
+
+   LTC_ARGCHK(num    != NULL);
+   LTC_ARGCHK(out    != NULL);
+   LTC_ARGCHK(outlen != NULL);
+
+   /* find out how big this will be */
+   if ((err = der_length_integer(num, &tmplen)) != CRYPT_OK) {
+      return err;
+   }
+
+   if (*outlen < tmplen) {
+      *outlen = tmplen;
+      return CRYPT_BUFFER_OVERFLOW;
+   }
+
+   if (mp_cmp_d(num, 0) != LTC_MP_LT) {
+      /* we only need a leading zero if the msb of the first byte is one */
+      if ((mp_count_bits(num) & 7) == 0 || mp_iszero(num) == LTC_MP_YES) {
+         leading_zero = 1;
+      } else {
+         leading_zero = 0;
+      }
+
+      /* get length of num in bytes (plus 1 since we force the msbyte to zero) */
+      y = mp_unsigned_bin_size(num) + leading_zero;
+   } else {
+      leading_zero = 0;
+      y            = mp_count_bits(num);
+      y            = y + (8 - (y & 7));
+      y            = y >> 3;
+      if (((mp_cnt_lsb(num)+1)==mp_count_bits(num)) && ((mp_count_bits(num)&7)==0)) --y;
+   }
+
+   /* now store initial data */
+   *out++ = 0x02;
+   if (y < 128) {
+      /* short form */
+      *out++ = (unsigned char)y;
+   } else if (y < 256) {
+      *out++ = 0x81;
+      *out++ = (unsigned char)y;
+   } else if (y < 65536UL) {
+      *out++ = 0x82;
+      *out++ = (unsigned char)((y>>8)&255);
+      *out++ = (unsigned char)y;
+   } else if (y < 16777216UL) {
+      *out++ = 0x83;
+      *out++ = (unsigned char)((y>>16)&255);
+      *out++ = (unsigned char)((y>>8)&255);
+      *out++ = (unsigned char)y;
+   } else {
+      return CRYPT_INVALID_ARG;
+   }
+
+   /* now store msbyte of zero if num is non-zero */
+   if (leading_zero) {
+      *out++ = 0x00;
+   }
+
+   /* if it's not zero store it as big endian */
+   if (mp_cmp_d(num, 0) == LTC_MP_GT) {
+      /* now store the mpint */
+      if ((err = mp_to_unsigned_bin(num, out)) != CRYPT_OK) {
+          return err;
+      }
+   } else if (mp_iszero(num) != LTC_MP_YES) {
+      void *tmp;
+         
+      /* negative */
+      if (mp_init(&tmp) != CRYPT_OK) {
+         return CRYPT_MEM;
+      }
+
+      /* 2^roundup and subtract */
+      y = mp_count_bits(num);
+      y = y + (8 - (y & 7));
+      if (((mp_cnt_lsb(num)+1)==mp_count_bits(num)) && ((mp_count_bits(num)&7)==0)) y -= 8;
+      if (mp_2expt(tmp, y) != CRYPT_OK || mp_add(tmp, num, tmp) != CRYPT_OK) {
+         mp_clear(tmp);
+         return CRYPT_MEM;
+      }
+      if ((err = mp_to_unsigned_bin(tmp, out)) != CRYPT_OK) {
+         mp_clear(tmp);
+         return err;
+      }
+      mp_clear(tmp);
+   }
+
+   /* we good */
+   *outlen = tmplen; 
+   return CRYPT_OK;
+}
+
+#endif
+
+/* $Source$ */
+/* $Revision$ */
+/* $Date$ */
diff --git a/core/lib/libtomcrypt/src/pk/asn1/der/integer/der_length_integer.c b/core/lib/libtomcrypt/src/pk/asn1/der/integer/der_length_integer.c
new file mode 100644
index 0000000..7205a9d
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/asn1/der/integer/der_length_integer.c
@@ -0,0 +1,108 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file der_length_integer.c
+  ASN.1 DER, get length of encoding, Tom St Denis
+*/
+
+
+#ifdef LTC_DER
+/**
+  Gets length of DER encoding of num
+  @param num    The int to get the size of
+  @param outlen [out] The length of the DER encoding for the given integer
+  @return CRYPT_OK if successful
+*/
+int der_length_integer(void *num, unsigned long *outlen)
+{
+   unsigned long z, len;
+   int           leading_zero;
+
+   LTC_ARGCHK(num     != NULL);
+   LTC_ARGCHK(outlen  != NULL);
+
+   if (mp_cmp_d(num, 0) != LTC_MP_LT) {
+      /* positive */
+
+      /* we only need a leading zero if the msb of the first byte is one */
+      if ((mp_count_bits(num) & 7) == 0 || mp_iszero(num) == LTC_MP_YES) {
+         leading_zero = 1;
+      } else {
+         leading_zero = 0;
+      }
+
+      /* size for bignum */
+      z = len = leading_zero + mp_unsigned_bin_size(num);
+   } else {
+      /* it's negative */
+      /* find power of 2 that is a multiple of eight and greater than count bits */
+      z = mp_count_bits(num);
+      z = z + (8 - (z & 7));
+      if (((mp_cnt_lsb(num)+1)==mp_count_bits(num)) && ((mp_count_bits(num)&7)==0)) --z;
+      len = z = z >> 3;
+   }
+
+   /* now we need a length */
+   if (z < 128) {
+      /* short form */
+      ++len;
+   } else {
+      /* long form (relies on z != 0), assumes length bytes < 128 */
+      ++len;
+
+      while (z) {
+         ++len;
+         z >>= 8;
+      }
+   }
+
+   /* we need a 0x02 to indicate it's INTEGER */
+   ++len;
+
+   /* return length */
+   *outlen = len;
+   return CRYPT_OK;
+}
+
+#endif
+
+/* $Source$ */
+/* $Revision$ */
+/* $Date$ */
diff --git a/core/lib/libtomcrypt/src/pk/asn1/der/integer/sub.mk b/core/lib/libtomcrypt/src/pk/asn1/der/integer/sub.mk
new file mode 100644
index 0000000..d319f91
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/asn1/der/integer/sub.mk
@@ -0,0 +1,3 @@
+srcs-y += der_decode_integer.c
+srcs-y += der_encode_integer.c
+srcs-y += der_length_integer.c
diff --git a/core/lib/libtomcrypt/src/pk/asn1/der/object_identifier/der_decode_object_identifier.c b/core/lib/libtomcrypt/src/pk/asn1/der/object_identifier/der_decode_object_identifier.c
new file mode 100644
index 0000000..13d5f1c
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/asn1/der/object_identifier/der_decode_object_identifier.c
@@ -0,0 +1,126 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file der_decode_object_identifier.c
+  ASN.1 DER, Decode Object Identifier, Tom St Denis
+*/
+
+#ifdef LTC_DER
+/**
+  Decode OID data and store the array of integers in words
+  @param in      The OID DER encoded data
+  @param inlen   The length of the OID data
+  @param words   [out] The destination of the OID words
+  @param outlen  [in/out] The number of OID words
+  @return CRYPT_OK if successful
+*/
+int der_decode_object_identifier(const unsigned char *in,    unsigned long  inlen,
+                                       unsigned long *words, unsigned long *outlen)
+{
+   unsigned long x, y, t, len;
+
+   LTC_ARGCHK(in     != NULL);
+   LTC_ARGCHK(words  != NULL);
+   LTC_ARGCHK(outlen != NULL);
+
+   /* header is at least 3 bytes */
+   if (inlen < 3) {
+      return CRYPT_INVALID_PACKET;
+   }
+
+   /* must be room for at least two words */
+   if (*outlen < 2) {
+      return CRYPT_BUFFER_OVERFLOW;
+   }
+
+   /* decode the packet header */
+   x = 0;
+   if ((in[x++] & 0x1F) != 0x06) {
+      return CRYPT_INVALID_PACKET;
+   }
+   
+   /* get the length */
+   if (in[x] < 128) {
+      len = in[x++]; 
+   } else {
+       if (in[x] < 0x81 || in[x] > 0x82) {
+          return CRYPT_INVALID_PACKET;
+       }
+       y   = in[x++] & 0x7F;
+       len = 0;
+       while (y--) {
+          len = (len << 8) | (unsigned long)in[x++];
+       }
+   }
+
+   if (len < 1 || (len + x) > inlen) {
+      return CRYPT_INVALID_PACKET;
+   }
+
+   /* decode words */
+   y = 0;
+   t = 0;
+   while (len--) {
+       t = (t << 7) | (in[x] & 0x7F);
+       if (!(in[x++] & 0x80)) {
+           /* store t */
+           if (y >= *outlen) {
+              return CRYPT_BUFFER_OVERFLOW;
+           }
+      if (y == 0) {
+         words[0] = t / 40;
+         words[1] = t % 40;
+         y = 2;
+      } else {
+              words[y++] = t;
+      }
+           t          = 0;
+       }
+   }
+       
+   *outlen = y;
+   return CRYPT_OK;
+}
+
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/pk/asn1/der/object_identifier/der_decode_object_identifier.c,v $ */
+/* $Revision: 1.6 $ */
+/* $Date: 2006/12/28 01:27:24 $ */
diff --git a/core/lib/libtomcrypt/src/pk/asn1/der/object_identifier/der_encode_object_identifier.c b/core/lib/libtomcrypt/src/pk/asn1/der/object_identifier/der_encode_object_identifier.c
new file mode 100644
index 0000000..890114d
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/asn1/der/object_identifier/der_encode_object_identifier.c
@@ -0,0 +1,138 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file der_encode_object_identifier.c
+  ASN.1 DER, Encode Object Identifier, Tom St Denis
+*/
+
+#ifdef LTC_DER
+/**
+  Encode an OID
+  @param words   The words to encode  (upto 32-bits each)
+  @param nwords  The number of words in the OID
+  @param out     [out] Destination of OID data
+  @param outlen  [in/out] The max and resulting size of the OID
+  @return CRYPT_OK if successful
+*/
+int der_encode_object_identifier(unsigned long *words, unsigned long  nwords,
+                                 unsigned char *out,   unsigned long *outlen)
+{
+   unsigned long i, x, y, z, t, mask, wordbuf;
+   int           err;
+
+   LTC_ARGCHK(words  != NULL);
+   LTC_ARGCHK(out    != NULL);
+   LTC_ARGCHK(outlen != NULL);
+
+   /* check length */
+   if ((err = der_length_object_identifier(words, nwords, &x)) != CRYPT_OK) {
+      return err;
+   }
+   if (x > *outlen) {
+      *outlen = x;
+      return CRYPT_BUFFER_OVERFLOW;
+   }
+
+   /* compute length to store OID data */
+   z = 0;
+   wordbuf = words[0] * 40 + words[1];
+   for (y = 1; y < nwords; y++) {
+       t = der_object_identifier_bits(wordbuf);
+       z += t/7 + ((t%7) ? 1 : 0) + (wordbuf == 0 ? 1 : 0);
+       if (y < nwords - 1) {
+          wordbuf = words[y + 1];
+       }
+   }
+
+   /* store header + length */
+   x = 0; 
+   out[x++] = 0x06;
+   if (z < 128) {
+      out[x++] = (unsigned char)z;
+   } else if (z < 256) {
+      out[x++] = 0x81;
+      out[x++] = (unsigned char)z;
+   } else if (z < 65536UL) {
+      out[x++] = 0x82;
+      out[x++] = (unsigned char)((z>>8)&255);
+      out[x++] = (unsigned char)(z&255);
+   } else {
+      return CRYPT_INVALID_ARG;
+   }
+
+   /* store first byte */
+    wordbuf = words[0] * 40 + words[1];   
+    for (i = 1; i < nwords; i++) {
+        /* store 7 bit words in little endian */
+        t    = wordbuf & 0xFFFFFFFF;
+        if (t) {
+           y    = x;
+           mask = 0;
+           while (t) {
+               out[x++] = (unsigned char)((t & 0x7F) | mask);
+               t    >>= 7;
+               mask  |= 0x80;  /* upper bit is set on all but the last byte */
+           }
+           /* now swap bytes y...x-1 */
+           z = x - 1;
+           while (y < z) {
+               t = out[y]; out[y] = out[z]; out[z] = (unsigned char)t;
+               ++y; 
+               --z;
+           }
+       } else {
+          /* zero word */
+          out[x++] = 0x00;
+       }
+       
+       if (i < nwords - 1) {
+          wordbuf = words[i + 1];
+       }
+   }
+
+   *outlen = x;
+   return CRYPT_OK;
+}
+
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/pk/asn1/der/object_identifier/der_encode_object_identifier.c,v $ */
+/* $Revision: 1.7 $ */
+/* $Date: 2006/12/28 01:27:24 $ */
diff --git a/core/lib/libtomcrypt/src/pk/asn1/der/object_identifier/der_length_object_identifier.c b/core/lib/libtomcrypt/src/pk/asn1/der/object_identifier/der_length_object_identifier.c
new file mode 100644
index 0000000..d2fd0e0
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/asn1/der/object_identifier/der_length_object_identifier.c
@@ -0,0 +1,116 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file der_length_object_identifier.c
+  ASN.1 DER, get length of Object Identifier, Tom St Denis
+*/
+
+#ifdef LTC_DER
+
+unsigned long der_object_identifier_bits(unsigned long x)
+{
+   unsigned long c;
+   x &= 0xFFFFFFFF;
+   c  = 0;
+   while (x) {
+     ++c;
+     x >>= 1;
+   }
+   return c;
+}
+
+
+/**
+  Gets length of DER encoding of Object Identifier
+  @param nwords   The number of OID words 
+  @param words    The actual OID words to get the size of
+  @param outlen   [out] The length of the DER encoding for the given string
+  @return CRYPT_OK if successful
+*/
+int der_length_object_identifier(unsigned long *words, unsigned long nwords, unsigned long *outlen)
+{
+   unsigned long y, z, t, wordbuf;   
+
+   LTC_ARGCHK(words  != NULL);
+   LTC_ARGCHK(outlen != NULL);
+
+
+   /* must be >= 2 words */
+   if (nwords < 2) {
+      return CRYPT_INVALID_ARG;
+   }
+
+   /* word1 = 0,1,2,3 and word2 0..39 */
+   if (words[0] > 3 || (words[0] < 2 && words[1] > 39)) {
+      return CRYPT_INVALID_ARG;
+   }
+
+   /* leading word is the first two */
+   z = 0;
+   wordbuf = words[0] * 40 + words[1];
+   for (y = 1; y < nwords; y++) {
+       t = der_object_identifier_bits(wordbuf);
+       z += t/7 + ((t%7) ? 1 : 0) + (wordbuf == 0 ? 1 : 0);
+       if (y < nwords - 1) {
+          /* grab next word */
+          wordbuf = words[y+1];
+       }
+   }
+
+   /* now depending on the length our length encoding changes */
+   if (z < 128) {
+      z += 2;
+   } else if (z < 256) {
+      z += 3;
+   } else if (z < 65536UL) {
+      z += 4;
+   } else {
+      return CRYPT_INVALID_ARG;
+   }
+
+   *outlen = z;
+   return CRYPT_OK;
+}
+
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/pk/asn1/der/object_identifier/der_length_object_identifier.c,v $ */
+/* $Revision: 1.5 $ */
+/* $Date: 2006/12/28 01:27:24 $ */
diff --git a/core/lib/libtomcrypt/src/pk/asn1/der/object_identifier/sub.mk b/core/lib/libtomcrypt/src/pk/asn1/der/object_identifier/sub.mk
new file mode 100644
index 0000000..b56efb0
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/asn1/der/object_identifier/sub.mk
@@ -0,0 +1,3 @@
+srcs-y += der_decode_object_identifier.c
+srcs-y += der_encode_object_identifier.c
+srcs-y += der_length_object_identifier.c
diff --git a/core/lib/libtomcrypt/src/pk/asn1/der/octet/der_decode_octet_string.c b/core/lib/libtomcrypt/src/pk/asn1/der/octet/der_decode_octet_string.c
new file mode 100644
index 0000000..bae4760
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/asn1/der/octet/der_decode_octet_string.c
@@ -0,0 +1,118 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file der_decode_octet_string.c
+  ASN.1 DER, encode a OCTET STRING, Tom St Denis
+*/
+
+
+#ifdef LTC_DER
+
+/**
+  Store a OCTET STRING
+  @param in      The DER encoded OCTET STRING
+  @param inlen   The size of the DER OCTET STRING
+  @param out     [out] The array of octets stored (one per char)
+  @param outlen  [in/out] The number of octets stored
+  @return CRYPT_OK if successful
+*/
+int der_decode_octet_string(const unsigned char *in, unsigned long inlen,
+                                  unsigned char *out, unsigned long *outlen)
+{
+   unsigned long x, y, len;
+
+   LTC_ARGCHK(in     != NULL);
+   LTC_ARGCHK(out    != NULL);
+   LTC_ARGCHK(outlen != NULL);
+
+   /* must have header at least */
+   if (inlen < 2) {
+      return CRYPT_INVALID_PACKET;
+   }
+
+   /* check for 0x04 */
+   if ((in[0] & 0x1F) != 0x04) {
+      return CRYPT_INVALID_PACKET;
+   }
+   x = 1;
+
+   /* decode the length */
+   if (in[x] & 0x80) {
+      /* valid # of bytes in length are 1,2,3 */
+      y = in[x] & 0x7F;
+      if ((y == 0) || (y > 3) || ((x + y) > inlen)) {
+         return CRYPT_INVALID_PACKET;
+      }
+
+      /* read the length in */
+      len = 0;
+      ++x;
+      while (y--) {
+         len = (len << 8) | in[x++];
+      }
+   } else {
+      len = in[x++] & 0x7F;
+   }
+
+   /* is it too long? */
+   if (len > *outlen) {
+      *outlen = len;
+      return CRYPT_BUFFER_OVERFLOW;
+   }
+
+   if (len + x > inlen) {
+      return CRYPT_INVALID_PACKET;
+   }
+
+   /* read the data */
+   for (y = 0; y < len; y++) {
+       out[y] = in[x++];
+   }
+
+   *outlen = y;
+
+   return CRYPT_OK;
+}
+ 
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/pk/asn1/der/octet/der_decode_octet_string.c,v $ */
+/* $Revision: 1.4 $ */
+/* $Date: 2006/12/28 01:27:24 $ */
diff --git a/core/lib/libtomcrypt/src/pk/asn1/der/octet/der_encode_octet_string.c b/core/lib/libtomcrypt/src/pk/asn1/der/octet/der_encode_octet_string.c
new file mode 100644
index 0000000..395774c
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/asn1/der/octet/der_encode_octet_string.c
@@ -0,0 +1,113 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file der_encode_octet_string.c
+  ASN.1 DER, encode a OCTET STRING, Tom St Denis
+*/
+
+
+#ifdef LTC_DER
+
+/**
+  Store an OCTET STRING
+  @param in       The array of OCTETS to store (one per char)
+  @param inlen    The number of OCTETS to store
+  @param out      [out] The destination for the DER encoded OCTET STRING
+  @param outlen   [in/out] The max size and resulting size of the DER OCTET STRING
+  @return CRYPT_OK if successful
+*/
+int der_encode_octet_string(const unsigned char *in, unsigned long inlen,
+                                  unsigned char *out, unsigned long *outlen)
+{
+   unsigned long x, y, len;
+   int           err;
+
+   LTC_ARGCHK(in     != NULL);
+   LTC_ARGCHK(out    != NULL);
+   LTC_ARGCHK(outlen != NULL);
+
+   /* get the size */
+   if ((err = der_length_octet_string(inlen, &len)) != CRYPT_OK) {
+      return err; 
+   }
+
+   /* too big? */
+   if (len > *outlen) {
+      *outlen = len;
+      return CRYPT_BUFFER_OVERFLOW;
+   }
+
+   /* encode the header+len */
+   x = 0;
+   out[x++] = 0x04;
+   if (inlen < 128) {
+      out[x++] = (unsigned char)inlen;
+   } else if (inlen < 256) {
+      out[x++] = 0x81;
+      out[x++] = (unsigned char)inlen;
+   } else if (inlen < 65536UL) {
+      out[x++] = 0x82;
+      out[x++] = (unsigned char)((inlen>>8)&255);
+      out[x++] = (unsigned char)(inlen&255);
+   } else if (inlen < 16777216UL) {
+      out[x++] = 0x83;
+      out[x++] = (unsigned char)((inlen>>16)&255);
+      out[x++] = (unsigned char)((inlen>>8)&255);
+      out[x++] = (unsigned char)(inlen&255);
+   } else {
+      return CRYPT_INVALID_ARG;
+   }
+
+   /* store octets */
+   for (y = 0; y < inlen; y++) {
+       out[x++] = in[y];
+   }
+
+   /* retun length */
+   *outlen = x;
+
+   return CRYPT_OK;
+}
+
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/pk/asn1/der/octet/der_encode_octet_string.c,v $ */
+/* $Revision: 1.5 $ */
+/* $Date: 2006/12/28 01:27:24 $ */
diff --git a/core/lib/libtomcrypt/src/pk/asn1/der/octet/der_length_octet_string.c b/core/lib/libtomcrypt/src/pk/asn1/der/octet/der_length_octet_string.c
new file mode 100644
index 0000000..25ff5f8
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/asn1/der/octet/der_length_octet_string.c
@@ -0,0 +1,80 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file der_length_octet_string.c
+  ASN.1 DER, get length of OCTET STRING, Tom St Denis
+*/
+
+#ifdef LTC_DER
+/**
+  Gets length of DER encoding of OCTET STRING 
+  @param noctets  The number of octets in the string to encode
+  @param outlen   [out] The length of the DER encoding for the given string
+  @return CRYPT_OK if successful
+*/
+int der_length_octet_string(unsigned long noctets, unsigned long *outlen)
+{
+   LTC_ARGCHK(outlen != NULL);
+
+   if (noctets < 128) {
+      /* 04 LL DD DD DD ... */
+      *outlen = 2 + noctets;
+   } else if (noctets < 256) {
+      /* 04 81 LL DD DD DD ... */
+      *outlen = 3 + noctets;
+   } else if (noctets < 65536UL) {
+      /* 04 82 LL LL DD DD DD ... */
+      *outlen = 4 + noctets;
+   } else if (noctets < 16777216UL) {
+      /* 04 83 LL LL LL DD DD DD ... */
+      *outlen = 5 + noctets;
+   } else {
+      return CRYPT_INVALID_ARG;
+   }
+
+   return CRYPT_OK;
+}
+
+#endif
+
+
+/* $Source: /cvs/libtom/libtomcrypt/src/pk/asn1/der/octet/der_length_octet_string.c,v $ */
+/* $Revision: 1.3 $ */
+/* $Date: 2006/12/28 01:27:24 $ */
diff --git a/core/lib/libtomcrypt/src/pk/asn1/der/octet/sub.mk b/core/lib/libtomcrypt/src/pk/asn1/der/octet/sub.mk
new file mode 100644
index 0000000..04607ee
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/asn1/der/octet/sub.mk
@@ -0,0 +1,3 @@
+srcs-y += der_decode_octet_string.c
+srcs-y += der_encode_octet_string.c
+srcs-y += der_length_octet_string.c
diff --git a/core/lib/libtomcrypt/src/pk/asn1/der/printable_string/der_decode_printable_string.c b/core/lib/libtomcrypt/src/pk/asn1/der/printable_string/der_decode_printable_string.c
new file mode 100644
index 0000000..4688c38
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/asn1/der/printable_string/der_decode_printable_string.c
@@ -0,0 +1,123 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file der_decode_printable_string.c
+  ASN.1 DER, encode a printable STRING, Tom St Denis
+*/
+
+
+#ifdef LTC_DER
+
+/**
+  Store a printable STRING
+  @param in      The DER encoded printable STRING
+  @param inlen   The size of the DER printable STRING
+  @param out     [out] The array of octets stored (one per char)
+  @param outlen  [in/out] The number of octets stored
+  @return CRYPT_OK if successful
+*/
+int der_decode_printable_string(const unsigned char *in, unsigned long inlen,
+                                unsigned char *out, unsigned long *outlen)
+{
+   unsigned long x, y, len;
+   int           t;
+
+   LTC_ARGCHK(in     != NULL);
+   LTC_ARGCHK(out    != NULL);
+   LTC_ARGCHK(outlen != NULL);
+
+   /* must have header at least */
+   if (inlen < 2) {
+      return CRYPT_INVALID_PACKET;
+   }
+
+   /* check for 0x13 */
+   if ((in[0] & 0x1F) != 0x13) {
+      return CRYPT_INVALID_PACKET;
+   }
+   x = 1;
+
+   /* decode the length */
+   if (in[x] & 0x80) {
+      /* valid # of bytes in length are 1,2,3 */
+      y = in[x] & 0x7F;
+      if ((y == 0) || (y > 3) || ((x + y) > inlen)) {
+         return CRYPT_INVALID_PACKET;
+      }
+
+      /* read the length in */
+      len = 0;
+      ++x;
+      while (y--) {
+         len = (len << 8) | in[x++];
+      }
+   } else {
+      len = in[x++] & 0x7F;
+   }
+
+   /* is it too long? */
+   if (len > *outlen) {
+      *outlen = len;
+      return CRYPT_BUFFER_OVERFLOW;
+   }
+
+   if (len + x > inlen) {
+      return CRYPT_INVALID_PACKET;
+   }
+
+   /* read the data */
+   for (y = 0; y < len; y++) {
+       t = der_printable_value_decode(in[x++]);
+       if (t == -1) {
+           return CRYPT_INVALID_ARG;
+       }
+       out[y] = t;
+   }
+
+   *outlen = y;
+
+   return CRYPT_OK;
+}
+ 
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/pk/asn1/der/printable_string/der_decode_printable_string.c,v $ */
+/* $Revision: 1.4 $ */
+/* $Date: 2006/12/28 01:27:24 $ */
diff --git a/core/lib/libtomcrypt/src/pk/asn1/der/printable_string/der_encode_printable_string.c b/core/lib/libtomcrypt/src/pk/asn1/der/printable_string/der_encode_printable_string.c
new file mode 100644
index 0000000..f94bd5e
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/asn1/der/printable_string/der_encode_printable_string.c
@@ -0,0 +1,112 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file der_encode_printable_string.c
+  ASN.1 DER, encode a printable STRING, Tom St Denis
+*/
+
+#ifdef LTC_DER
+
+/**
+  Store an printable STRING
+  @param in       The array of printable to store (one per char)
+  @param inlen    The number of printable to store
+  @param out      [out] The destination for the DER encoded printable STRING
+  @param outlen   [in/out] The max size and resulting size of the DER printable STRING
+  @return CRYPT_OK if successful
+*/
+int der_encode_printable_string(const unsigned char *in, unsigned long inlen,
+                                unsigned char *out, unsigned long *outlen)
+{
+   unsigned long x, y, len;
+   int           err;
+
+   LTC_ARGCHK(in     != NULL);
+   LTC_ARGCHK(out    != NULL);
+   LTC_ARGCHK(outlen != NULL);
+
+   /* get the size */
+   if ((err = der_length_printable_string(in, inlen, &len)) != CRYPT_OK) {
+      return err; 
+   }
+
+   /* too big? */
+   if (len > *outlen) {
+      *outlen = len;
+      return CRYPT_BUFFER_OVERFLOW;
+   }
+
+   /* encode the header+len */
+   x = 0;
+   out[x++] = 0x13;
+   if (inlen < 128) {
+      out[x++] = (unsigned char)inlen;
+   } else if (inlen < 256) {
+      out[x++] = 0x81;
+      out[x++] = (unsigned char)inlen;
+   } else if (inlen < 65536UL) {
+      out[x++] = 0x82;
+      out[x++] = (unsigned char)((inlen>>8)&255);
+      out[x++] = (unsigned char)(inlen&255);
+   } else if (inlen < 16777216UL) {
+      out[x++] = 0x83;
+      out[x++] = (unsigned char)((inlen>>16)&255);
+      out[x++] = (unsigned char)((inlen>>8)&255);
+      out[x++] = (unsigned char)(inlen&255);
+   } else {
+      return CRYPT_INVALID_ARG;
+   }
+
+   /* store octets */
+   for (y = 0; y < inlen; y++) {
+       out[x++] = der_printable_char_encode(in[y]);
+   }
+
+   /* retun length */
+   *outlen = x;
+
+   return CRYPT_OK;
+}
+
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/pk/asn1/der/printable_string/der_encode_printable_string.c,v $ */
+/* $Revision: 1.5 $ */
+/* $Date: 2006/12/28 01:27:24 $ */
diff --git a/core/lib/libtomcrypt/src/pk/asn1/der/printable_string/der_length_printable_string.c b/core/lib/libtomcrypt/src/pk/asn1/der/printable_string/der_length_printable_string.c
new file mode 100644
index 0000000..73cd8ee
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/asn1/der/printable_string/der_length_printable_string.c
@@ -0,0 +1,193 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file der_length_printable_string.c
+  ASN.1 DER, get length of Printable STRING, Tom St Denis
+*/
+
+#ifdef LTC_DER
+
+static const struct {
+   int code, value;
+} printable_table[] = {
+{ ' ', 32 }, 
+{ '\'', 39 }, 
+{ '(', 40 }, 
+{ ')', 41 }, 
+{ '+', 43 }, 
+{ ',', 44 }, 
+{ '-', 45 }, 
+{ '.', 46 }, 
+{ '/', 47 }, 
+{ '0', 48 }, 
+{ '1', 49 }, 
+{ '2', 50 }, 
+{ '3', 51 }, 
+{ '4', 52 }, 
+{ '5', 53 }, 
+{ '6', 54 }, 
+{ '7', 55 }, 
+{ '8', 56 }, 
+{ '9', 57 }, 
+{ ':', 58 }, 
+{ '=', 61 }, 
+{ '?', 63 }, 
+{ 'A', 65 }, 
+{ 'B', 66 }, 
+{ 'C', 67 }, 
+{ 'D', 68 }, 
+{ 'E', 69 }, 
+{ 'F', 70 }, 
+{ 'G', 71 }, 
+{ 'H', 72 }, 
+{ 'I', 73 }, 
+{ 'J', 74 }, 
+{ 'K', 75 }, 
+{ 'L', 76 }, 
+{ 'M', 77 }, 
+{ 'N', 78 }, 
+{ 'O', 79 }, 
+{ 'P', 80 }, 
+{ 'Q', 81 }, 
+{ 'R', 82 }, 
+{ 'S', 83 }, 
+{ 'T', 84 }, 
+{ 'U', 85 }, 
+{ 'V', 86 }, 
+{ 'W', 87 }, 
+{ 'X', 88 }, 
+{ 'Y', 89 }, 
+{ 'Z', 90 }, 
+{ 'a', 97 }, 
+{ 'b', 98 }, 
+{ 'c', 99 }, 
+{ 'd', 100 }, 
+{ 'e', 101 }, 
+{ 'f', 102 }, 
+{ 'g', 103 }, 
+{ 'h', 104 }, 
+{ 'i', 105 }, 
+{ 'j', 106 }, 
+{ 'k', 107 }, 
+{ 'l', 108 }, 
+{ 'm', 109 }, 
+{ 'n', 110 }, 
+{ 'o', 111 }, 
+{ 'p', 112 }, 
+{ 'q', 113 }, 
+{ 'r', 114 }, 
+{ 's', 115 }, 
+{ 't', 116 }, 
+{ 'u', 117 }, 
+{ 'v', 118 }, 
+{ 'w', 119 }, 
+{ 'x', 120 }, 
+{ 'y', 121 }, 
+{ 'z', 122 }, 
+};
+
+int der_printable_char_encode(int c)
+{
+   int x;
+   for (x = 0; x < (int)(sizeof(printable_table)/sizeof(printable_table[0])); x++) {
+       if (printable_table[x].code == c) {
+          return printable_table[x].value;
+       }
+   }
+   return -1;
+}
+
+int der_printable_value_decode(int v)
+{
+   int x;
+   for (x = 0; x < (int)(sizeof(printable_table)/sizeof(printable_table[0])); x++) {
+       if (printable_table[x].value == v) {
+          return printable_table[x].code;
+       }
+   }
+   return -1;
+}
+   
+/**
+  Gets length of DER encoding of Printable STRING 
+  @param octets   The values you want to encode 
+  @param noctets  The number of octets in the string to encode
+  @param outlen   [out] The length of the DER encoding for the given string
+  @return CRYPT_OK if successful
+*/
+int der_length_printable_string(const unsigned char *octets, unsigned long noctets, unsigned long *outlen)
+{
+   unsigned long x;
+
+   LTC_ARGCHK(outlen != NULL);
+   LTC_ARGCHK(octets != NULL);
+
+   /* scan string for validity */
+   for (x = 0; x < noctets; x++) {
+       if (der_printable_char_encode(octets[x]) == -1) {
+          return CRYPT_INVALID_ARG;
+       }
+   }
+
+   if (noctets < 128) {
+      /* 16 LL DD DD DD ... */
+      *outlen = 2 + noctets;
+   } else if (noctets < 256) {
+      /* 16 81 LL DD DD DD ... */
+      *outlen = 3 + noctets;
+   } else if (noctets < 65536UL) {
+      /* 16 82 LL LL DD DD DD ... */
+      *outlen = 4 + noctets;
+   } else if (noctets < 16777216UL) {
+      /* 16 83 LL LL LL DD DD DD ... */
+      *outlen = 5 + noctets;
+   } else {
+      return CRYPT_INVALID_ARG;
+   }
+
+   return CRYPT_OK;
+}
+
+#endif
+
+
+/* $Source: /cvs/libtom/libtomcrypt/src/pk/asn1/der/printable_string/der_length_printable_string.c,v $ */
+/* $Revision: 1.3 $ */
+/* $Date: 2006/12/28 01:27:24 $ */
diff --git a/core/lib/libtomcrypt/src/pk/asn1/der/printable_string/sub.mk b/core/lib/libtomcrypt/src/pk/asn1/der/printable_string/sub.mk
new file mode 100644
index 0000000..b509209
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/asn1/der/printable_string/sub.mk
@@ -0,0 +1,3 @@
+srcs-y += der_decode_printable_string.c
+srcs-y += der_encode_printable_string.c
+srcs-y += der_length_printable_string.c
diff --git a/core/lib/libtomcrypt/src/pk/asn1/der/sequence/der_decode_sequence_ex.c b/core/lib/libtomcrypt/src/pk/asn1/der/sequence/der_decode_sequence_ex.c
new file mode 100644
index 0000000..2973361
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/asn1/der/sequence/der_decode_sequence_ex.c
@@ -0,0 +1,347 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+#include <stdarg.h>
+
+
+/**
+  @file der_decode_sequence_ex.c
+  ASN.1 DER, decode a SEQUENCE, Tom St Denis
+*/
+
+#ifdef LTC_DER
+
+/**
+   Decode a SEQUENCE
+   @param in       The DER encoded input
+   @param inlen    The size of the input
+   @param list     The list of items to decode
+   @param outlen   The number of items in the list
+   @param ordered  Search an unordeded or ordered list
+   @return CRYPT_OK on success
+*/
+int der_decode_sequence_ex(const unsigned char *in, unsigned long  inlen,
+                           ltc_asn1_list *list,     unsigned long  outlen, int ordered)
+{
+   int           err, i;
+   ltc_asn1_type type;
+   unsigned long size, x, y, z, blksize;
+   void          *data;
+
+   LTC_ARGCHK(in   != NULL);
+   LTC_ARGCHK(list != NULL);
+
+   /* get blk size */
+   if (inlen < 2) {
+      return CRYPT_INVALID_PACKET;
+   }
+
+   /* sequence type? We allow 0x30 SEQUENCE and 0x31 SET since fundamentally they're the same structure */
+   x = 0;
+   if (in[x] != 0x30 && in[x] != 0x31) {
+      return CRYPT_INVALID_PACKET;
+   }
+   ++x;
+
+   /* check if the msb is set, which signals that the
+    * 7 lsb bits represent the number of bytes of the length
+    */
+   if (in[x] < 128) {
+      blksize = in[x++];
+   } else {
+      if (in[x] < 0x81 || in[x] > 0x83) {
+         return CRYPT_INVALID_PACKET;
+      }
+      y = in[x++] & 0x7F;
+
+      /* would reading the len bytes overrun? */
+      if (x + y > inlen) {
+         return CRYPT_INVALID_PACKET;
+      }
+
+      /* read len */
+      blksize = 0;
+      while (y--) {
+          blksize = (blksize << 8) | (unsigned long)in[x++];
+      }
+  }
+
+  /* would this blksize overflow? */
+  if (x + blksize > inlen) {
+     return CRYPT_INVALID_PACKET;
+  }
+
+   /* mark all as unused */
+   for (i = 0; i < (int)outlen; i++) {
+       list[i].used = 0;
+   }
+
+  /* ok read data */
+   inlen = blksize;
+   for (i = 0; i < (int)outlen; i++) {
+       z    = 0;
+       type = list[i].type;
+       size = list[i].size;
+       data = list[i].data;
+       if (!ordered && list[i].used == 1) { continue; }
+
+       if (type == LTC_ASN1_EOL) {
+          break;
+       }
+
+       switch (type) {
+           case LTC_ASN1_BOOLEAN:
+               z = inlen;
+               if ((err = der_decode_boolean(in + x, z, ((int *)data))) != CRYPT_OK) {
+                   goto LBL_ERR;
+               }
+               if ((err = der_length_boolean(&z)) != CRYPT_OK) {
+                   goto LBL_ERR;
+                }
+                break;
+
+           case LTC_ASN1_INTEGER:
+               z = inlen;
+               if ((err = der_decode_integer(in + x, z, data)) != CRYPT_OK) {
+                  if (!ordered) {  continue; }
+                  goto LBL_ERR;
+               }
+               if ((err = der_length_integer(data, &z)) != CRYPT_OK) {
+                  goto LBL_ERR;
+               }
+               break;
+
+           case LTC_ASN1_SHORT_INTEGER:
+               z = inlen;
+               if ((err = der_decode_short_integer(in + x, z, data)) != CRYPT_OK) {
+                  if (!ordered) { continue; }
+                  goto LBL_ERR;
+               }
+               if ((err = der_length_short_integer(((unsigned long*)data)[0], &z)) != CRYPT_OK) {
+                  goto LBL_ERR;
+               }
+
+               break;
+
+           case LTC_ASN1_BIT_STRING:
+               z = inlen;
+               if ((err = der_decode_bit_string(in + x, z, data, &size)) != CRYPT_OK) {
+                  if (!ordered) { continue; }
+                  goto LBL_ERR;
+               }
+               list[i].size = size;
+               if ((err = der_length_bit_string(size, &z)) != CRYPT_OK) {
+                  goto LBL_ERR;
+               }
+               break;
+
+           case LTC_ASN1_RAW_BIT_STRING:
+               z = inlen;
+               if ((err = der_decode_raw_bit_string(in + x, z, data, &size)) != CRYPT_OK) {
+                  if (!ordered) { continue; }
+                  goto LBL_ERR;
+               }
+               list[i].size = size;
+               if ((err = der_length_bit_string(size, &z)) != CRYPT_OK) {
+                  goto LBL_ERR;
+               }
+               break;
+
+           case LTC_ASN1_OCTET_STRING:
+               z = inlen;
+               if ((err = der_decode_octet_string(in + x, z, data, &size)) != CRYPT_OK) {
+                  if (!ordered) { continue; }
+                  goto LBL_ERR;
+               }
+               list[i].size = size;
+               if ((err = der_length_octet_string(size, &z)) != CRYPT_OK) {
+                  goto LBL_ERR;
+               }
+               break;
+
+           case LTC_ASN1_NULL:
+               if (inlen < 2 || in[x] != 0x05 || in[x+1] != 0x00) {
+                  if (!ordered) { continue; }
+                  err = CRYPT_INVALID_PACKET;
+                  goto LBL_ERR;
+               }
+               z = 2;
+               break;
+
+           case LTC_ASN1_OBJECT_IDENTIFIER:
+               z = inlen;
+               if ((err = der_decode_object_identifier(in + x, z, data, &size)) != CRYPT_OK) {
+                  if (!ordered) { continue; }
+                  goto LBL_ERR;
+               }
+               list[i].size = size;
+               if ((err = der_length_object_identifier(data, size, &z)) != CRYPT_OK) {
+                  goto LBL_ERR;
+               }
+               break;
+
+           case LTC_ASN1_TELETEX_STRING:
+               z = inlen;
+               if ((err = der_decode_teletex_string(in + x, z, data, &size)) != CRYPT_OK) {
+                  if (!ordered) { continue; }
+                  goto LBL_ERR;
+               }
+               list[i].size = size;
+               if ((err = der_length_teletex_string(data, size, &z)) != CRYPT_OK) {
+                  goto LBL_ERR;
+               }
+               break;
+
+           case LTC_ASN1_IA5_STRING:
+               z = inlen;
+               if ((err = der_decode_ia5_string(in + x, z, data, &size)) != CRYPT_OK) {
+                  if (!ordered) { continue; }
+                  goto LBL_ERR;
+               }
+               list[i].size = size;
+               if ((err = der_length_ia5_string(data, size, &z)) != CRYPT_OK) {
+                  goto LBL_ERR;
+               }
+               break;
+
+
+           case LTC_ASN1_PRINTABLE_STRING:
+               z = inlen;
+               if ((err = der_decode_printable_string(in + x, z, data, &size)) != CRYPT_OK) {
+                  if (!ordered) { continue; }
+                  goto LBL_ERR;
+               }
+               list[i].size = size;
+               if ((err = der_length_printable_string(data, size, &z)) != CRYPT_OK) {
+                  goto LBL_ERR;
+               }
+               break;
+
+           case LTC_ASN1_UTF8_STRING:
+               z = inlen;
+               if ((err = der_decode_utf8_string(in + x, z, data, &size)) != CRYPT_OK) {
+                  if (!ordered) { continue; }
+                  goto LBL_ERR;
+               }
+               list[i].size = size;
+               if ((err = der_length_utf8_string(data, size, &z)) != CRYPT_OK) {
+                  goto LBL_ERR;
+               }
+               break;
+
+           case LTC_ASN1_UTCTIME:
+               z = inlen;
+               if ((err = der_decode_utctime(in + x, &z, data)) != CRYPT_OK) {
+                  if (!ordered) { continue; }
+                  goto LBL_ERR;
+               }
+               break;
+
+           case LTC_ASN1_SET:
+               z = inlen;
+               if ((err = der_decode_set(in + x, z, data, size)) != CRYPT_OK) {
+                  if (!ordered) { continue; }
+                  goto LBL_ERR;
+               }
+               if ((err = der_length_sequence(data, size, &z)) != CRYPT_OK) {
+                  goto LBL_ERR;
+               }
+               break;
+
+           case LTC_ASN1_SETOF:
+           case LTC_ASN1_SEQUENCE:
+               /* detect if we have the right type */
+               if ((type == LTC_ASN1_SETOF && (in[x] & 0x3F) != 0x31) || (type == LTC_ASN1_SEQUENCE && (in[x] & 0x3F) != 0x30)) {
+                  err = CRYPT_INVALID_PACKET;
+                  goto LBL_ERR;
+               }
+
+               z = inlen;
+               if ((err = der_decode_sequence(in + x, z, data, size)) != CRYPT_OK) {
+                  if (!ordered) { continue; }
+                  goto LBL_ERR;
+               }
+               if ((err = der_length_sequence(data, size, &z)) != CRYPT_OK) {
+                  goto LBL_ERR;
+               }
+               break;
+
+
+           case LTC_ASN1_CHOICE:
+               z = inlen;
+               if ((err = der_decode_choice(in + x, &z, data, size)) != CRYPT_OK) {
+                  if (!ordered) { continue; }
+                  goto LBL_ERR;
+               }
+               break;
+
+           case LTC_ASN1_CONSTRUCTED:
+           case LTC_ASN1_CONTEXT_SPECIFIC:
+           case LTC_ASN1_EOL:
+               err = CRYPT_INVALID_ARG;
+               goto LBL_ERR;
+           default:
+               err = CRYPT_INVALID_ARG;
+               goto LBL_ERR;
+       }
+       x           += z;
+       inlen       -= z;
+       list[i].used = 1;
+       if (!ordered) {
+          /* restart the decoder */
+          i = -1;
+       }
+   }
+
+   for (i = 0; i < (int)outlen; i++) {
+      if (list[i].used == 0) {
+          err = CRYPT_INVALID_PACKET;
+          goto LBL_ERR;
+      }
+   }
+   err = CRYPT_OK;
+
+LBL_ERR:
+   return err;
+}
+
+#endif
+
+/* $Source$ */
+/* $Revision$ */
+/* $Date$ */
diff --git a/core/lib/libtomcrypt/src/pk/asn1/der/sequence/der_decode_sequence_flexi.c b/core/lib/libtomcrypt/src/pk/asn1/der/sequence/der_decode_sequence_flexi.c
new file mode 100644
index 0000000..e8969ed
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/asn1/der/sequence/der_decode_sequence_flexi.c
@@ -0,0 +1,460 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file der_decode_sequence_flexi.c
+  ASN.1 DER, decode an array of ASN.1 types with a flexi parser, Tom St Denis
+*/
+
+#ifdef LTC_DER
+
+static unsigned long fetch_length(const unsigned char *in, unsigned long inlen, unsigned long *data_offset)
+{
+   unsigned long x, z;
+
+   *data_offset = 0;
+
+   /* skip type and read len */
+   if (inlen < 2) {
+      return 0xFFFFFFFF;
+   }
+   ++in; ++(*data_offset);
+
+   /* read len */
+   x = *in++; ++(*data_offset);
+
+   /* <128 means literal */
+   if (x < 128) {
+      return x+*data_offset;
+   }
+   x     &= 0x7F; /* the lower 7 bits are the length of the length */
+   inlen -= 2;
+
+   /* len means len of len! */
+   if (x == 0 || x > 4 || x > inlen) {
+      return 0xFFFFFFFF;
+   }
+
+   *data_offset += x;
+   z = 0;
+   while (x--) {
+      z = (z<<8) | ((unsigned long)*in);
+      ++in;
+   }
+   return z+*data_offset;
+}
+
+/**
+   ASN.1 DER Flexi(ble) decoder will decode arbitrary DER packets and create a linked list of the decoded elements.
+   @param in      The input buffer
+   @param inlen   [in/out] The length of the input buffer and on output the amount of decoded data
+   @param out     [out] A pointer to the linked list
+   @return CRYPT_OK on success.
+*/
+int der_decode_sequence_flexi(const unsigned char *in, unsigned long *inlen, ltc_asn1_list **out)
+{
+   ltc_asn1_list *l;
+   unsigned long err, type, len, totlen, data_offset;
+   void          *realloc_tmp;
+
+   LTC_ARGCHK(in    != NULL);
+   LTC_ARGCHK(inlen != NULL);
+   LTC_ARGCHK(out   != NULL);
+
+   l = NULL;
+   totlen = 0;
+
+   /* scan the input and and get lengths and what not */
+   while (*inlen) {
+      /* read the type byte */
+      type = *in;
+
+      /* fetch length */
+      len = fetch_length(in, *inlen, &data_offset);
+      if (len > *inlen) {
+         err = CRYPT_INVALID_PACKET;
+         goto error;
+      }
+
+      /* alloc new link */
+      if (l == NULL) {
+         l = XCALLOC(1, sizeof(*l));
+         if (l == NULL) {
+            err = CRYPT_MEM;
+            goto error;
+         }
+      } else {
+         l->next = XCALLOC(1, sizeof(*l));
+         if (l->next == NULL) {
+            err = CRYPT_MEM;
+            goto error;
+         }
+         l->next->prev = l;
+         l = l->next;
+      }
+
+      if ((type & 0x20) && (type != 0x30) && (type != 0x31)) {
+         /* constructed, use the 'used' field to store the original identifier */
+         l->used = type;
+         /* treat constructed elements like SETs */
+         type = 0x20;
+      }
+      else if ((type & 0xC0) == 0x80) {
+         /* context-specific, use the 'used' field to store the original identifier */
+         l->used = type;
+         /* context-specific elements are treated as opaque data */
+         type = 0x80;
+      }
+
+     /* now switch on type */
+      switch (type) {
+         case 0x01: /* BOOLEAN */
+            l->type = LTC_ASN1_BOOLEAN;
+            l->size = 1;
+            l->data = XCALLOC(1, sizeof(int));
+
+            if ((err = der_decode_boolean(in, *inlen, l->data)) != CRYPT_OK) {
+               goto error;
+            }
+
+            if ((err = der_length_boolean(&len)) != CRYPT_OK) {
+               goto error;
+            }
+            break;
+
+         case 0x02: /* INTEGER */
+             /* init field */
+             l->type = LTC_ASN1_INTEGER;
+             l->size = 1;
+             if ((err = mp_init(&l->data)) != CRYPT_OK) {
+                 goto error;
+             }
+
+             /* decode field */
+             if ((err = der_decode_integer(in, *inlen, l->data)) != CRYPT_OK) {
+                 goto error;
+             }
+
+             /* calc length of object */
+             if ((err = der_length_integer(l->data, &len)) != CRYPT_OK) {
+                 goto error;
+             }
+             break;
+
+         case 0x03: /* BIT */
+            /* init field */
+            l->type = LTC_ASN1_BIT_STRING;
+            l->size = len * 8; /* *8 because we store decoded bits one per char and they are encoded 8 per char.  */
+
+            if ((l->data = XCALLOC(1, l->size)) == NULL) {
+               err = CRYPT_MEM;
+               goto error;
+            }
+
+            if ((err = der_decode_bit_string(in, *inlen, l->data, &l->size)) != CRYPT_OK) {
+               goto error;
+            }
+
+            if ((err = der_length_bit_string(l->size, &len)) != CRYPT_OK) {
+               goto error;
+            }
+            break;
+
+         case 0x04: /* OCTET */
+
+            /* init field */
+            l->type = LTC_ASN1_OCTET_STRING;
+            l->size = len;
+
+            if ((l->data = XCALLOC(1, l->size)) == NULL) {
+               err = CRYPT_MEM;
+               goto error;
+            }
+
+            if ((err = der_decode_octet_string(in, *inlen, l->data, &l->size)) != CRYPT_OK) {
+               goto error;
+            }
+
+            if ((err = der_length_octet_string(l->size, &len)) != CRYPT_OK) {
+               goto error;
+            }
+            break;
+
+         case 0x05: /* NULL */
+
+            /* valid NULL is 0x05 0x00 */
+            if (in[0] != 0x05 || in[1] != 0x00) {
+               err = CRYPT_INVALID_PACKET;
+               goto error;
+            }
+
+            /* simple to store ;-) */
+            l->type = LTC_ASN1_NULL;
+            l->data = NULL;
+            l->size = 0;
+            len     = 2;
+
+            break;
+
+         case 0x06: /* OID */
+
+            /* init field */
+            l->type = LTC_ASN1_OBJECT_IDENTIFIER;
+            l->size = len;
+
+            if ((l->data = XCALLOC(len, sizeof(unsigned long))) == NULL) {
+               err = CRYPT_MEM;
+               goto error;
+            }
+
+            if ((err = der_decode_object_identifier(in, *inlen, l->data, &l->size)) != CRYPT_OK) {
+               goto error;
+            }
+
+            if ((err = der_length_object_identifier(l->data, l->size, &len)) != CRYPT_OK) {
+               goto error;
+            }
+
+            /* resize it to save a bunch of mem */
+            if ((realloc_tmp = XREALLOC(l->data, l->size * sizeof(unsigned long))) == NULL) {
+               /* out of heap but this is not an error */
+               break;
+            }
+            l->data = realloc_tmp;
+            break;
+
+         case 0x0C: /* UTF8 */
+
+            /* init field */
+            l->type = LTC_ASN1_UTF8_STRING;
+            l->size = len;
+
+            if ((l->data = XCALLOC(sizeof(wchar_t), l->size)) == NULL) {
+               err = CRYPT_MEM;
+               goto error;
+            }
+
+            if ((err = der_decode_utf8_string(in, *inlen, l->data, &l->size)) != CRYPT_OK) {
+               goto error;
+            }
+
+            if ((err = der_length_utf8_string(l->data, l->size, &len)) != CRYPT_OK) {
+               goto error;
+            }
+            break;
+
+         case 0x13: /* PRINTABLE */
+
+            /* init field */
+            l->type = LTC_ASN1_PRINTABLE_STRING;
+            l->size = len;
+
+            if ((l->data = XCALLOC(1, l->size)) == NULL) {
+               err = CRYPT_MEM;
+               goto error;
+            }
+
+            if ((err = der_decode_printable_string(in, *inlen, l->data, &l->size)) != CRYPT_OK) {
+               goto error;
+            }
+
+            if ((err = der_length_printable_string(l->data, l->size, &len)) != CRYPT_OK) {
+               goto error;
+            }
+            break;
+
+         case 0x14: /* TELETEXT */
+
+            /* init field */
+            l->type = LTC_ASN1_TELETEX_STRING;
+            l->size = len;
+
+            if ((l->data = XCALLOC(1, l->size)) == NULL) {
+               err = CRYPT_MEM;
+               goto error;
+            }
+
+            if ((err = der_decode_teletex_string(in, *inlen, l->data, &l->size)) != CRYPT_OK) {
+               goto error;
+            }
+
+            if ((err = der_length_teletex_string(l->data, l->size, &len)) != CRYPT_OK) {
+               goto error;
+            }
+            break;
+
+         case 0x16: /* IA5 */
+
+            /* init field */
+            l->type = LTC_ASN1_IA5_STRING;
+            l->size = len;
+
+            if ((l->data = XCALLOC(1, l->size)) == NULL) {
+               err = CRYPT_MEM;
+               goto error;
+            }
+
+            if ((err = der_decode_ia5_string(in, *inlen, l->data, &l->size)) != CRYPT_OK) {
+               goto error;
+            }
+
+            if ((err = der_length_ia5_string(l->data, l->size, &len)) != CRYPT_OK) {
+               goto error;
+            }
+            break;
+
+         case 0x17: /* UTC TIME */
+
+            /* init field */
+            l->type = LTC_ASN1_UTCTIME;
+            l->size = 1;
+
+            if ((l->data = XCALLOC(1, sizeof(ltc_utctime))) == NULL) {
+               err = CRYPT_MEM;
+               goto error;
+            }
+
+            len = *inlen;
+            if ((err = der_decode_utctime(in, &len, l->data)) != CRYPT_OK) {
+               goto error;
+            }
+
+            if ((err = der_length_utctime(l->data, &len)) != CRYPT_OK) {
+               goto error;
+            }
+            break;
+
+         case 0x20: /* Any CONSTRUCTED element that is neither SEQUENCE nor SET */
+         case 0x30: /* SEQUENCE */
+         case 0x31: /* SET */
+
+             /* init field */
+             if (type == 0x20) {
+                l->type = LTC_ASN1_CONSTRUCTED;
+             }
+             else if (type == 0x30) {
+                l->type = LTC_ASN1_SEQUENCE;
+             }
+             else {
+                l->type = LTC_ASN1_SET;
+             }
+
+             /* jump to the start of the data */
+             in     += data_offset;
+             *inlen -= data_offset;
+             len = len - data_offset;
+
+             /* Sequence elements go as child */
+             if ((err = der_decode_sequence_flexi(in, &len, &(l->child))) != CRYPT_OK) {
+                goto error;
+             }
+
+             /* len update */
+             totlen += data_offset;
+
+             /* the flexi decoder can also do nothing, so make sure a child has been allocated */
+             if (l->child) {
+                /* link them up y0 */
+                l->child->parent = l;
+             }
+
+             break;
+
+         case 0x80: /* Context-specific */
+             l->type = LTC_ASN1_CONTEXT_SPECIFIC;
+
+             if ((l->data = XCALLOC(1, len - data_offset)) == NULL) {
+                err = CRYPT_MEM;
+                goto error;
+             }
+
+             XMEMCPY(l->data, in + data_offset, len - data_offset);
+             l->size = len - data_offset;
+
+             break;
+
+         default:
+           /* invalid byte ... this is a soft error */
+           /* remove link */
+           if (l->prev) {
+              l       = l->prev;
+              XFREE(l->next);
+              l->next = NULL;
+           }
+           goto outside;
+      }
+
+      /* advance pointers */
+      totlen  += len;
+      in      += len;
+      *inlen  -= len;
+   }
+
+outside:
+
+   /* in case we processed anything */
+   if (totlen) {
+      /* rewind l please */
+      while (l->prev != NULL || l->parent != NULL) {
+         if (l->parent != NULL) {
+            l = l->parent;
+         } else {
+            l = l->prev;
+         }
+      }
+   }
+
+   /* return */
+   *out   = l;
+   *inlen = totlen;
+   return CRYPT_OK;
+
+error:
+   /* free list */
+   der_sequence_free(l);
+
+   return err;
+}
+
+#endif
+
+
+/* $Source$ */
+/* $Revision$ */
+/* $Date$ */
diff --git a/core/lib/libtomcrypt/src/pk/asn1/der/sequence/der_decode_sequence_multi.c b/core/lib/libtomcrypt/src/pk/asn1/der/sequence/der_decode_sequence_multi.c
new file mode 100644
index 0000000..ea23558
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/asn1/der/sequence/der_decode_sequence_multi.c
@@ -0,0 +1,180 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+#include <stdarg.h>
+
+
+/**
+  @file der_decode_sequence_multi.c
+  ASN.1 DER, decode a SEQUENCE, Tom St Denis
+*/
+
+#ifdef LTC_DER
+
+/**
+  Decode a SEQUENCE type using a VA list
+  @param in    Input buffer
+  @param inlen Length of input in octets
+  @remark <...> is of the form <type, size, data> (int, unsigned long, void*)
+  @return CRYPT_OK on success
+*/
+int der_decode_sequence_multi(const unsigned char *in, unsigned long inlen, ...)
+{
+   int           err;
+   ltc_asn1_type type;
+   unsigned long size, x;
+   void          *data;
+   va_list       args;
+   ltc_asn1_list *list;
+
+   LTC_ARGCHK(in    != NULL);
+
+   /* get size of output that will be required */
+   va_start(args, inlen);
+   x = 0;
+   for (;;) {
+       type = va_arg(args, ltc_asn1_type);
+       size = va_arg(args, unsigned long);
+       data = va_arg(args, void*);
+       LTC_UNUSED_PARAM(size);
+       LTC_UNUSED_PARAM(data);
+
+       if (type == LTC_ASN1_EOL) {
+          break;
+       }
+
+       switch (type) {
+           case LTC_ASN1_BOOLEAN:
+           case LTC_ASN1_INTEGER:
+           case LTC_ASN1_SHORT_INTEGER:
+           case LTC_ASN1_BIT_STRING:
+           case LTC_ASN1_OCTET_STRING:
+           case LTC_ASN1_NULL:
+           case LTC_ASN1_OBJECT_IDENTIFIER:
+           case LTC_ASN1_IA5_STRING:
+           case LTC_ASN1_PRINTABLE_STRING:
+           case LTC_ASN1_UTF8_STRING:
+           case LTC_ASN1_UTCTIME:
+           case LTC_ASN1_SET:
+           case LTC_ASN1_SETOF:
+           case LTC_ASN1_SEQUENCE:
+           case LTC_ASN1_CHOICE:
+           case LTC_ASN1_RAW_BIT_STRING:
+           case LTC_ASN1_TELETEX_STRING:
+                ++x;
+                break;
+
+           case LTC_ASN1_EOL:
+           case LTC_ASN1_CONSTRUCTED:
+           case LTC_ASN1_CONTEXT_SPECIFIC:
+               va_end(args);
+               return CRYPT_INVALID_ARG;
+           default:
+               va_end(args);
+               return CRYPT_INVALID_ARG;
+       }
+   }
+   va_end(args);
+
+   /* allocate structure for x elements */
+   if (x == 0) {
+      return CRYPT_NOP;
+   }
+
+   list = XCALLOC(sizeof(*list), x);
+   if (list == NULL) {
+      return CRYPT_MEM;
+   }
+
+   /* fill in the structure */
+   va_start(args, inlen);
+   x = 0;
+   for (;;) {
+       type = va_arg(args, ltc_asn1_type);
+       size = va_arg(args, unsigned long);
+       data = va_arg(args, void*);
+
+       if (type == LTC_ASN1_EOL) {
+          break;
+       }
+
+       switch (type) {
+           case LTC_ASN1_BOOLEAN:
+           case LTC_ASN1_INTEGER:
+           case LTC_ASN1_SHORT_INTEGER:
+           case LTC_ASN1_BIT_STRING:
+           case LTC_ASN1_OCTET_STRING:
+           case LTC_ASN1_NULL:
+           case LTC_ASN1_OBJECT_IDENTIFIER:
+           case LTC_ASN1_IA5_STRING:
+           case LTC_ASN1_PRINTABLE_STRING:
+           case LTC_ASN1_UTF8_STRING:
+           case LTC_ASN1_UTCTIME:
+           case LTC_ASN1_SEQUENCE:
+           case LTC_ASN1_SET:
+           case LTC_ASN1_SETOF:
+           case LTC_ASN1_CHOICE:
+           case LTC_ASN1_RAW_BIT_STRING:
+           case LTC_ASN1_TELETEX_STRING:
+                LTC_SET_ASN1(list, x++, type, data, size);
+                break;
+           /* coverity[dead_error_line] */
+           case LTC_ASN1_EOL:
+           case LTC_ASN1_CONSTRUCTED:
+           case LTC_ASN1_CONTEXT_SPECIFIC:
+                break;
+           default:
+               va_end(args);
+               err = CRYPT_INVALID_ARG;
+               goto LBL_ERR;
+       }
+   }
+   va_end(args);
+
+   err = der_decode_sequence(in, inlen, list, x);
+LBL_ERR:
+   XFREE(list);
+   return err;
+}
+
+#endif
+
+
+/* $Source$ */
+/* $Revision$ */
+/* $Date$ */
diff --git a/core/lib/libtomcrypt/src/pk/asn1/der/sequence/der_decode_subject_public_key_info.c b/core/lib/libtomcrypt/src/pk/asn1/der/sequence/der_decode_subject_public_key_info.c
new file mode 100644
index 0000000..22569bb
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/asn1/der/sequence/der_decode_subject_public_key_info.c
@@ -0,0 +1,131 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ */
+#include "tomcrypt.h"
+/**
+  @file der_encode_sequence_multi.c
+  ASN.1 DER, encode a Subject Public Key structure --nmav
+*/
+
+#ifdef LTC_DER
+
+/* AlgorithmIdentifier := SEQUENCE {
+ *    algorithm OBJECT IDENTIFIER,
+ *    parameters ANY DEFINED BY algorithm
+ * }
+ *
+ * SubjectPublicKeyInfo := SEQUENCE {
+ *    algorithm AlgorithmIdentifier,
+ *    subjectPublicKey BIT STRING
+ * }
+ */
+/**
+  Encode a SEQUENCE type using a VA list
+  @param out    [out] Destination for data
+  @param outlen [in/out] Length of buffer and resulting length of output
+  @remark <...> is of the form <type, size, data> (int, unsigned long, void*)
+  @return CRYPT_OK on success
+*/
+int der_decode_subject_public_key_info(const unsigned char *in, unsigned long inlen,
+        unsigned int algorithm, void* public_key, unsigned long* public_key_len,
+        unsigned long parameters_type, ltc_asn1_list* parameters, unsigned long parameters_len)
+{
+   int err;
+   unsigned long len;
+   oid_st oid;
+   unsigned char *tmpbuf;
+   unsigned long  tmpoid[16];
+   ltc_asn1_list alg_id[2];
+   ltc_asn1_list subject_pubkey[2];
+
+   LTC_ARGCHK(in    != NULL);
+   LTC_ARGCHK(inlen != 0);
+   LTC_ARGCHK(public_key_len != NULL);
+
+   err = pk_get_oid(algorithm, &oid);
+   if (err != CRYPT_OK) {
+        return err;
+   }
+
+   /* see if the OpenSSL DER format RSA public key will work */
+   tmpbuf = XCALLOC(1, LTC_DER_MAX_PUBKEY_SIZE*8);
+   if (tmpbuf == NULL) {
+       err = CRYPT_MEM;
+       goto LBL_ERR;
+   }
+
+   /* this includes the internal hash ID and optional params (NULL in this case) */
+   LTC_SET_ASN1(alg_id, 0, LTC_ASN1_OBJECT_IDENTIFIER, tmpoid, sizeof(tmpoid)/sizeof(tmpoid[0]));
+   LTC_SET_ASN1(alg_id, 1, parameters_type, parameters, parameters_len);
+
+   /* the actual format of the SSL DER key is odd, it stores a RSAPublicKey
+    * in a **BIT** string ... so we have to extract it then proceed to convert bit to octet
+    */
+   LTC_SET_ASN1(subject_pubkey, 0, LTC_ASN1_SEQUENCE, alg_id, 2);
+   LTC_SET_ASN1(subject_pubkey, 1, LTC_ASN1_RAW_BIT_STRING, tmpbuf, LTC_DER_MAX_PUBKEY_SIZE*8);
+
+   err=der_decode_sequence(in, inlen, subject_pubkey, 2UL);
+   if (err != CRYPT_OK) {
+           goto LBL_ERR;
+   }
+
+   if ((alg_id[0].size != oid.OIDlen) ||
+        XMEMCMP(oid.OID, alg_id[0].data, oid.OIDlen * sizeof(oid.OID[0]))) {
+        /* OID mismatch */
+        err = CRYPT_PK_INVALID_TYPE;
+        goto LBL_ERR;
+   }
+
+   len = subject_pubkey[1].size/8;
+   if (*public_key_len > len) {
+       XMEMCPY(public_key, subject_pubkey[1].data, len);
+       *public_key_len = len;
+    } else {
+        *public_key_len = len;
+        err = CRYPT_BUFFER_OVERFLOW;
+        goto LBL_ERR;
+    }
+
+    err = CRYPT_OK;
+
+LBL_ERR:
+
+    XFREE(tmpbuf);
+
+    return err;
+}
+
+#endif
diff --git a/core/lib/libtomcrypt/src/pk/asn1/der/sequence/der_encode_sequence_ex.c b/core/lib/libtomcrypt/src/pk/asn1/der/sequence/der_encode_sequence_ex.c
new file mode 100644
index 0000000..69ae0f1
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/asn1/der/sequence/der_encode_sequence_ex.c
@@ -0,0 +1,387 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+#include <stdarg.h>
+
+
+/**
+  @file der_encode_sequence_ex.c
+  ASN.1 DER, encode a SEQUENCE, Tom St Denis
+*/
+
+#ifdef LTC_DER
+
+/**
+   Encode a SEQUENCE
+   @param list      The list of items to encode
+   @param inlen     The number of items in the list
+   @param out       [out] The destination
+   @param outlen    [in/out] The size of the output
+   @param type_of   LTC_ASN1_SEQUENCE or LTC_ASN1_SET/LTC_ASN1_SETOF
+   @return CRYPT_OK on success
+*/
+int der_encode_sequence_ex(ltc_asn1_list *list, unsigned long inlen,
+                           unsigned char *out,  unsigned long *outlen, int type_of)
+{
+   int           err;
+   ltc_asn1_type type;
+   unsigned long size, x, y, z, i;
+   void          *data;
+
+   LTC_ARGCHK(list    != NULL);
+   LTC_ARGCHK(out     != NULL);
+   LTC_ARGCHK(outlen  != NULL);
+
+   /* get size of output that will be required */
+   y = 0;
+   for (i = 0; i < inlen; i++) {
+       type = list[i].type;
+       size = list[i].size;
+       data = list[i].data;
+
+       if (type == LTC_ASN1_EOL) {
+          break;
+       }
+
+       switch (type) {
+            case LTC_ASN1_BOOLEAN:
+               if ((err = der_length_boolean(&x)) != CRYPT_OK) {
+                  goto LBL_ERR;
+               }
+               y += x;
+               break;
+
+           case LTC_ASN1_INTEGER:
+               if ((err = der_length_integer(data, &x)) != CRYPT_OK) {
+                  goto LBL_ERR;
+               }
+               y += x;
+               break;
+
+           case LTC_ASN1_SHORT_INTEGER:
+               if ((err = der_length_short_integer(*((unsigned long*)data), &x)) != CRYPT_OK) {
+                  goto LBL_ERR;
+               }
+               y += x;
+               break;
+
+           case LTC_ASN1_BIT_STRING:
+           case LTC_ASN1_RAW_BIT_STRING:
+               if ((err = der_length_bit_string(size, &x)) != CRYPT_OK) {
+                  goto LBL_ERR;
+               }
+               y += x;
+               break;
+
+           case LTC_ASN1_OCTET_STRING:
+               if ((err = der_length_octet_string(size, &x)) != CRYPT_OK) {
+                  goto LBL_ERR;
+               }
+               y += x;
+               break;
+
+           case LTC_ASN1_NULL:
+               y += 2;
+               break;
+
+           case LTC_ASN1_OBJECT_IDENTIFIER:
+               if ((err = der_length_object_identifier(data, size, &x)) != CRYPT_OK) {
+                  goto LBL_ERR;
+               }
+               y += x;
+               break;
+
+           case LTC_ASN1_IA5_STRING:
+               if ((err = der_length_ia5_string(data, size, &x)) != CRYPT_OK) {
+                  goto LBL_ERR;
+               }
+               y += x;
+               break;
+
+           case LTC_ASN1_PRINTABLE_STRING:
+               if ((err = der_length_printable_string(data, size, &x)) != CRYPT_OK) {
+                  goto LBL_ERR;
+               }
+               y += x;
+               break;
+
+           case LTC_ASN1_UTF8_STRING:
+               if ((err = der_length_utf8_string(data, size, &x)) != CRYPT_OK) {
+                  goto LBL_ERR;
+               }
+               y += x;
+               break;
+
+           case LTC_ASN1_UTCTIME:
+               if ((err = der_length_utctime(data, &x)) != CRYPT_OK) {
+                  goto LBL_ERR;
+               }
+               y += x;
+               break;
+
+           case LTC_ASN1_SET:
+           case LTC_ASN1_SETOF:
+           case LTC_ASN1_SEQUENCE:
+               if ((err = der_length_sequence(data, size, &x)) != CRYPT_OK) {
+                  goto LBL_ERR;
+               }
+               y += x;
+               break;
+
+           case LTC_ASN1_CHOICE:
+           case LTC_ASN1_CONSTRUCTED:
+           case LTC_ASN1_CONTEXT_SPECIFIC:
+           case LTC_ASN1_EOL:
+           case LTC_ASN1_TELETEX_STRING:
+               err = CRYPT_INVALID_ARG;
+               goto LBL_ERR;
+           default:
+               err = CRYPT_INVALID_ARG;
+               goto LBL_ERR;
+       }
+   }
+
+   /* calc header size */
+   z = y;
+   if (y < 128) {
+      y += 2;
+   } else if (y < 256) {
+      /* 0x30 0x81 LL */
+      y += 3;
+   } else if (y < 65536UL) {
+      /* 0x30 0x82 LL LL */
+      y += 4;
+   } else if (y < 16777216UL) {
+      /* 0x30 0x83 LL LL LL */
+      y += 5;
+   } else {
+      err = CRYPT_INVALID_ARG;
+      goto LBL_ERR;
+   }
+
+   /* too big ? */
+   if (*outlen < y) {
+      *outlen = y;
+      err = CRYPT_BUFFER_OVERFLOW;
+      goto LBL_ERR;
+   }
+
+   /* store header */
+   x = 0;
+   out[x++] = (type_of == LTC_ASN1_SEQUENCE) ? 0x30 : 0x31;
+
+   if (z < 128) {
+      out[x++] = (unsigned char)z;
+   } else if (z < 256) {
+      out[x++] = 0x81;
+      out[x++] = (unsigned char)z;
+   } else if (z < 65536UL) {
+      out[x++] = 0x82;
+      out[x++] = (unsigned char)((z>>8UL)&255);
+      out[x++] = (unsigned char)(z&255);
+   } else if (z < 16777216UL) {
+      out[x++] = 0x83;
+      out[x++] = (unsigned char)((z>>16UL)&255);
+      out[x++] = (unsigned char)((z>>8UL)&255);
+      out[x++] = (unsigned char)(z&255);
+   }
+
+   /* store data */
+   *outlen -= x;
+   for (i = 0; i < inlen; i++) {
+       type = list[i].type;
+       size = list[i].size;
+       data = list[i].data;
+
+       if (type == LTC_ASN1_EOL) {
+          break;
+       }
+
+       switch (type) {
+            case LTC_ASN1_BOOLEAN:
+               z = *outlen;
+               if ((err = der_encode_boolean(*((int *)data), out + x, &z)) != CRYPT_OK) {
+                  goto LBL_ERR;
+               }
+               x       += z;
+               *outlen -= z;
+               break;
+
+           case LTC_ASN1_INTEGER:
+               z = *outlen;
+               if ((err = der_encode_integer(data, out + x, &z)) != CRYPT_OK) {
+                  goto LBL_ERR;
+               }
+               x       += z;
+               *outlen -= z;
+               break;
+
+           case LTC_ASN1_SHORT_INTEGER:
+               z = *outlen;
+               if ((err = der_encode_short_integer(*((unsigned long*)data), out + x, &z)) != CRYPT_OK) {
+                  goto LBL_ERR;
+               }
+               x       += z;
+               *outlen -= z;
+               break;
+
+           case LTC_ASN1_BIT_STRING:
+               z = *outlen;
+               if ((err = der_encode_bit_string(data, size, out + x, &z)) != CRYPT_OK) {
+                  goto LBL_ERR;
+               }
+               x       += z;
+               *outlen -= z;
+               break;
+
+           case LTC_ASN1_RAW_BIT_STRING:
+               z = *outlen;
+               if ((err = der_encode_raw_bit_string(data, size, out + x, &z)) != CRYPT_OK) {
+                  goto LBL_ERR;
+               }
+               x       += z;
+               *outlen -= z;
+               break;
+
+           case LTC_ASN1_OCTET_STRING:
+               z = *outlen;
+               if ((err = der_encode_octet_string(data, size, out + x, &z)) != CRYPT_OK) {
+                  goto LBL_ERR;
+               }
+               x       += z;
+               *outlen -= z;
+               break;
+
+           case LTC_ASN1_NULL:
+               out[x++] = 0x05;
+               out[x++] = 0x00;
+               *outlen -= 2;
+               break;
+
+           case LTC_ASN1_OBJECT_IDENTIFIER:
+               z = *outlen;
+               if ((err = der_encode_object_identifier(data, size, out + x, &z)) != CRYPT_OK) {
+                  goto LBL_ERR;
+               }
+               x       += z;
+               *outlen -= z;
+               break;
+
+           case LTC_ASN1_IA5_STRING:
+               z = *outlen;
+               if ((err = der_encode_ia5_string(data, size, out + x, &z)) != CRYPT_OK) {
+                  goto LBL_ERR;
+               }
+               x       += z;
+               *outlen -= z;
+               break;
+
+           case LTC_ASN1_PRINTABLE_STRING:
+               z = *outlen;
+               if ((err = der_encode_printable_string(data, size, out + x, &z)) != CRYPT_OK) {
+                  goto LBL_ERR;
+               }
+               x       += z;
+               *outlen -= z;
+               break;
+
+           case LTC_ASN1_UTF8_STRING:
+               z = *outlen;
+               if ((err = der_encode_utf8_string(data, size, out + x, &z)) != CRYPT_OK) {
+                  goto LBL_ERR;
+               }
+               x       += z;
+               *outlen -= z;
+               break;
+
+           case LTC_ASN1_UTCTIME:
+               z = *outlen;
+               if ((err = der_encode_utctime(data, out + x, &z)) != CRYPT_OK) {
+                  goto LBL_ERR;
+               }
+               x       += z;
+               *outlen -= z;
+               break;
+
+           case LTC_ASN1_SET:
+               z = *outlen;
+               if ((err = der_encode_set(data, size, out + x, &z)) != CRYPT_OK) {
+                  goto LBL_ERR;
+               }
+               x       += z;
+               *outlen -= z;
+               break;
+
+           case LTC_ASN1_SETOF:
+               z = *outlen;
+               if ((err = der_encode_setof(data, size, out + x, &z)) != CRYPT_OK) {
+                  goto LBL_ERR;
+               }
+               x       += z;
+               *outlen -= z;
+               break;
+
+           case LTC_ASN1_SEQUENCE:
+               z = *outlen;
+               if ((err = der_encode_sequence_ex(data, size, out + x, &z, type)) != CRYPT_OK) {
+                  goto LBL_ERR;
+               }
+               x       += z;
+               *outlen -= z;
+               break;
+
+           case LTC_ASN1_CHOICE:
+           case LTC_ASN1_CONSTRUCTED:
+           case LTC_ASN1_CONTEXT_SPECIFIC:
+           case LTC_ASN1_EOL:
+           case LTC_ASN1_TELETEX_STRING:
+               err = CRYPT_INVALID_ARG;
+               goto LBL_ERR;
+           default:
+               err = CRYPT_INVALID_ARG;
+               goto LBL_ERR;
+       }
+   }
+   *outlen = x;
+   err = CRYPT_OK;
+
+LBL_ERR:
+   return err;
+}
+
+#endif
diff --git a/core/lib/libtomcrypt/src/pk/asn1/der/sequence/der_encode_sequence_multi.c b/core/lib/libtomcrypt/src/pk/asn1/der/sequence/der_encode_sequence_multi.c
new file mode 100644
index 0000000..8bfc9ae
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/asn1/der/sequence/der_encode_sequence_multi.c
@@ -0,0 +1,183 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+#include <stdarg.h>
+
+
+/**
+  @file der_encode_sequence_multi.c
+  ASN.1 DER, encode a SEQUENCE, Tom St Denis
+*/
+
+#ifdef LTC_DER
+
+/**
+  Encode a SEQUENCE type using a VA list
+  @param out    [out] Destination for data
+  @param outlen [in/out] Length of buffer and resulting length of output
+  @remark <...> is of the form <type, size, data> (int, unsigned long, void*)
+  @return CRYPT_OK on success
+*/
+int der_encode_sequence_multi(unsigned char *out, unsigned long *outlen, ...)
+{
+   int           err;
+   ltc_asn1_type type;
+   unsigned long size, x;
+   void          *data;
+   va_list       args;
+   ltc_asn1_list *list;
+
+   LTC_ARGCHK(out    != NULL);
+   LTC_ARGCHK(outlen != NULL);
+
+   /* get size of output that will be required */
+   va_start(args, outlen);
+   x = 0;
+   for (;;) {
+       type = va_arg(args, ltc_asn1_type);
+       size = va_arg(args, unsigned long);
+       data = va_arg(args, void*);
+       LTC_UNUSED_PARAM(size);
+       LTC_UNUSED_PARAM(data);
+
+       if (type == LTC_ASN1_EOL) {
+          break;
+       }
+
+       switch (type) {
+           case LTC_ASN1_BOOLEAN:
+           case LTC_ASN1_INTEGER:
+           case LTC_ASN1_SHORT_INTEGER:
+           case LTC_ASN1_BIT_STRING:
+           case LTC_ASN1_OCTET_STRING:
+           case LTC_ASN1_NULL:
+           case LTC_ASN1_OBJECT_IDENTIFIER:
+           case LTC_ASN1_IA5_STRING:
+           case LTC_ASN1_PRINTABLE_STRING:
+           case LTC_ASN1_UTF8_STRING:
+           case LTC_ASN1_UTCTIME:
+           case LTC_ASN1_SEQUENCE:
+           case LTC_ASN1_SET:
+           case LTC_ASN1_SETOF:
+           case LTC_ASN1_RAW_BIT_STRING:
+                ++x;
+                break;
+
+           case LTC_ASN1_CHOICE:
+           case LTC_ASN1_CONSTRUCTED:
+           case LTC_ASN1_CONTEXT_SPECIFIC:
+           case LTC_ASN1_EOL:
+           case LTC_ASN1_TELETEX_STRING:
+               va_end(args);
+               return CRYPT_INVALID_ARG;
+           default:
+               va_end(args);
+               return CRYPT_INVALID_ARG;
+       }
+   }
+   va_end(args);
+
+   /* allocate structure for x elements */
+   if (x == 0) {
+      return CRYPT_NOP;
+   }
+
+   list = XCALLOC(sizeof(*list), x);
+   if (list == NULL) {
+      return CRYPT_MEM;
+   }
+
+   /* fill in the structure */
+   va_start(args, outlen);
+   x = 0;
+   for (;;) {
+       type = va_arg(args, ltc_asn1_type);
+       size = va_arg(args, unsigned long);
+       data = va_arg(args, void*);
+
+       if (type == LTC_ASN1_EOL) {
+          break;
+       }
+
+       switch (type) {
+           case LTC_ASN1_BOOLEAN:
+           case LTC_ASN1_INTEGER:
+           case LTC_ASN1_SHORT_INTEGER:
+           case LTC_ASN1_BIT_STRING:
+           case LTC_ASN1_OCTET_STRING:
+           case LTC_ASN1_NULL:
+           case LTC_ASN1_OBJECT_IDENTIFIER:
+           case LTC_ASN1_IA5_STRING:
+           case LTC_ASN1_PRINTABLE_STRING:
+           case LTC_ASN1_UTF8_STRING:
+           case LTC_ASN1_UTCTIME:
+           case LTC_ASN1_SEQUENCE:
+           case LTC_ASN1_SET:
+           case LTC_ASN1_SETOF:
+           case LTC_ASN1_RAW_BIT_STRING:
+                LTC_SET_ASN1(list, x++, type, data, size);
+                break;
+
+           case LTC_ASN1_CHOICE:
+           case LTC_ASN1_CONSTRUCTED:
+           case LTC_ASN1_CONTEXT_SPECIFIC:
+           case LTC_ASN1_EOL:
+           case LTC_ASN1_TELETEX_STRING:
+               va_end(args);
+               err = CRYPT_INVALID_ARG;
+               goto LBL_ERR;
+           default:
+               va_end(args);
+               err = CRYPT_INVALID_ARG;
+               goto LBL_ERR;
+       }
+   }
+   va_end(args);
+
+   err = der_encode_sequence(list, x, out, outlen);
+LBL_ERR:
+   XFREE(list);
+   return err;
+}
+
+#endif
+
+
+/* $Source$ */
+/* $Revision$ */
+/* $Date$ */
diff --git a/core/lib/libtomcrypt/src/pk/asn1/der/sequence/der_encode_subject_public_key_info.c b/core/lib/libtomcrypt/src/pk/asn1/der/sequence/der_encode_subject_public_key_info.c
new file mode 100644
index 0000000..a075e7c
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/asn1/der/sequence/der_encode_subject_public_key_info.c
@@ -0,0 +1,91 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ */
+#include "tomcrypt.h"
+
+/**
+  @file der_encode_sequence_multi.c
+  ASN.1 DER, encode a Subject Public Key structure --nmav
+*/
+
+#ifdef LTC_DER
+
+/* AlgorithmIdentifier := SEQUENCE {
+ *    algorithm OBJECT IDENTIFIER,
+ *    parameters ANY DEFINED BY algorithm
+ * }
+ *
+ * SubjectPublicKeyInfo := SEQUENCE {
+ *    algorithm AlgorithmIdentifier,
+ *    subjectPublicKey BIT STRING
+ * }
+ */
+/**
+  Encode a SEQUENCE type using a VA list
+  @param out    [out] Destination for data
+  @param outlen [in/out] Length of buffer and resulting length of output
+  @remark <...> is of the form <type, size, data> (int, unsigned long, void*)
+  @return CRYPT_OK on success
+*/
+int der_encode_subject_public_key_info(unsigned char *out, unsigned long *outlen,
+        unsigned int algorithm, void* public_key, unsigned long public_key_len,
+        unsigned long parameters_type, void* parameters, unsigned long parameters_len)
+{
+   int           err;
+   ltc_asn1_list alg_id[2];
+   oid_st oid;
+
+   LTC_ARGCHK(out    != NULL);
+   LTC_ARGCHK(outlen != NULL);
+
+   err = pk_get_oid(algorithm, &oid);
+   if (err != CRYPT_OK) {
+        return err;
+   }
+
+   LTC_SET_ASN1(alg_id, 0, LTC_ASN1_OBJECT_IDENTIFIER, oid.OID,    oid.OIDlen);
+   LTC_SET_ASN1(alg_id, 1, parameters_type,            parameters, parameters_len);
+
+   return der_encode_sequence_multi(out, outlen,
+        LTC_ASN1_SEQUENCE, (unsigned long)sizeof(alg_id)/sizeof(alg_id[0]), alg_id,
+        LTC_ASN1_RAW_BIT_STRING, (unsigned long)(public_key_len*8), public_key,
+        LTC_ASN1_EOL,     0UL, NULL);
+
+}
+
+#endif
+
+
diff --git a/core/lib/libtomcrypt/src/pk/asn1/der/sequence/der_length_sequence.c b/core/lib/libtomcrypt/src/pk/asn1/der/sequence/der_length_sequence.c
new file mode 100644
index 0000000..6f9d688
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/asn1/der/sequence/der_length_sequence.c
@@ -0,0 +1,210 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file der_length_sequence.c
+  ASN.1 DER, length a SEQUENCE, Tom St Denis
+*/
+
+#ifdef LTC_DER
+
+/**
+   Get the length of a DER sequence
+   @param list   The sequences of items in the SEQUENCE
+   @param inlen  The number of items
+   @param outlen [out] The length required in octets to store it
+   @return CRYPT_OK on success
+*/
+int der_length_sequence(ltc_asn1_list *list, unsigned long inlen,
+                        unsigned long *outlen)
+{
+   int           err;
+   ltc_asn1_type type;
+   unsigned long size, x, y, i;
+   void          *data;
+
+   LTC_ARGCHK(list    != NULL);
+   LTC_ARGCHK(outlen  != NULL);
+
+   /* get size of output that will be required */
+   y = 0;
+   for (i = 0; i < inlen; i++) {
+       type = list[i].type;
+       size = list[i].size;
+       data = list[i].data;
+
+       if (type == LTC_ASN1_EOL) {
+          break;
+       }
+
+       switch (type) {
+           case LTC_ASN1_BOOLEAN:
+              if ((err = der_length_boolean(&x)) != CRYPT_OK) {
+                 goto LBL_ERR;
+              }
+              y += x;
+              break;
+
+           case LTC_ASN1_INTEGER:
+               if ((err = der_length_integer(data, &x)) != CRYPT_OK) {
+                  goto LBL_ERR;
+               }
+               y += x;
+               break;
+
+           case LTC_ASN1_SHORT_INTEGER:
+               if ((err = der_length_short_integer(*((unsigned long *)data), &x)) != CRYPT_OK) {
+                  goto LBL_ERR;
+               }
+               y += x;
+               break;
+
+           case LTC_ASN1_BIT_STRING:
+           case LTC_ASN1_RAW_BIT_STRING:
+               if ((err = der_length_bit_string(size, &x)) != CRYPT_OK) {
+                  goto LBL_ERR;
+               }
+               y += x;
+               break;
+
+           case LTC_ASN1_OCTET_STRING:
+               if ((err = der_length_octet_string(size, &x)) != CRYPT_OK) {
+                  goto LBL_ERR;
+               }
+               y += x;
+               break;
+
+           case LTC_ASN1_NULL:
+               y += 2;
+               break;
+
+           case LTC_ASN1_OBJECT_IDENTIFIER:
+               if ((err = der_length_object_identifier(data, size, &x)) != CRYPT_OK) {
+                  goto LBL_ERR;
+               }
+               y += x;
+               break;
+
+           case LTC_ASN1_IA5_STRING:
+               if ((err = der_length_ia5_string(data, size, &x)) != CRYPT_OK) {
+                  goto LBL_ERR;
+               }
+               y += x;
+               break;
+
+           case LTC_ASN1_TELETEX_STRING:
+               if ((err = der_length_teletex_string(data, size, &x)) != CRYPT_OK) {
+                  goto LBL_ERR;
+               }
+               y += x;
+               break;
+
+           case LTC_ASN1_PRINTABLE_STRING:
+               if ((err = der_length_printable_string(data, size, &x)) != CRYPT_OK) {
+                  goto LBL_ERR;
+               }
+               y += x;
+               break;
+
+           case LTC_ASN1_UTCTIME:
+               if ((err = der_length_utctime(data, &x)) != CRYPT_OK) {
+                  goto LBL_ERR;
+               }
+               y += x;
+               break;
+
+           case LTC_ASN1_UTF8_STRING:
+               if ((err = der_length_utf8_string(data, size, &x)) != CRYPT_OK) {
+                  goto LBL_ERR;
+               }
+               y += x;
+               break;
+
+           case LTC_ASN1_SET:
+           case LTC_ASN1_SETOF:
+           case LTC_ASN1_SEQUENCE:
+               if ((err = der_length_sequence(data, size, &x)) != CRYPT_OK) {
+                  goto LBL_ERR;
+               }
+               y += x;
+               break;
+
+
+           case LTC_ASN1_CHOICE:
+           case LTC_ASN1_CONSTRUCTED:
+           case LTC_ASN1_CONTEXT_SPECIFIC:
+           case LTC_ASN1_EOL:
+               err = CRYPT_INVALID_ARG;
+               goto LBL_ERR;
+           default:
+               err = CRYPT_INVALID_ARG;
+               goto LBL_ERR;
+       }
+   }
+
+   /* calc header size */
+   if (y < 128) {
+      y += 2;
+   } else if (y < 256) {
+      /* 0x30 0x81 LL */
+      y += 3;
+   } else if (y < 65536UL) {
+      /* 0x30 0x82 LL LL */
+      y += 4;
+   } else if (y < 16777216UL) {
+      /* 0x30 0x83 LL LL LL */
+      y += 5;
+   } else {
+      err = CRYPT_INVALID_ARG;
+      goto LBL_ERR;
+   }
+
+   /* store size */
+   *outlen = y;
+   err     = CRYPT_OK;
+
+LBL_ERR:
+   return err;
+}
+
+#endif
+
+/* $Source$ */
+/* $Revision$ */
+/* $Date$ */
diff --git a/core/lib/libtomcrypt/src/pk/asn1/der/sequence/der_sequence_free.c b/core/lib/libtomcrypt/src/pk/asn1/der/sequence/der_sequence_free.c
new file mode 100644
index 0000000..d2ded80
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/asn1/der/sequence/der_sequence_free.c
@@ -0,0 +1,94 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file der_sequence_free.c
+  ASN.1 DER, free's a structure allocated by der_decode_sequence_flexi(), Tom St Denis
+*/
+
+#ifdef LTC_DER
+
+/**
+  Free memory allocated by der_decode_sequence_flexi()
+  @param in     The list to free
+*/  
+void der_sequence_free(ltc_asn1_list *in)
+{
+   ltc_asn1_list *l;
+
+   if (!in) return;
+   
+   /* walk to the start of the chain */
+   while (in->prev != NULL || in->parent != NULL) {
+      if (in->parent != NULL) {
+          in = in->parent;
+      } else {
+          in = in->prev;
+      }
+   }
+   
+   /* now walk the list and free stuff */
+   while (in != NULL) {
+      /* is there a child? */
+      if (in->child) {
+         /* disconnect */
+         in->child->parent = NULL;
+         der_sequence_free(in->child);
+      }
+      
+      switch (in->type) { 
+         case LTC_ASN1_SET:
+         case LTC_ASN1_SETOF:
+         case LTC_ASN1_SEQUENCE: break;
+         case LTC_ASN1_INTEGER : if (in->data != NULL) { mp_clear(in->data); } break;
+         default               : if (in->data != NULL) { XFREE(in->data);    }
+      }
+      
+      /* move to next and free current */
+      l = in->next;
+      XFREE(in);
+      in = l;
+   }     
+}
+
+#endif
+
+/* $Source$ */
+/* $Revision$ */
+/* $Date$ */
diff --git a/core/lib/libtomcrypt/src/pk/asn1/der/sequence/sub.mk b/core/lib/libtomcrypt/src/pk/asn1/der/sequence/sub.mk
new file mode 100644
index 0000000..a50b4ba
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/asn1/der/sequence/sub.mk
@@ -0,0 +1,9 @@
+srcs-y += der_decode_sequence_ex.c
+srcs-y += der_decode_sequence_flexi.c
+srcs-y += der_decode_sequence_multi.c
+srcs-y += der_encode_sequence_ex.c
+srcs-y += der_encode_sequence_multi.c
+srcs-y += der_length_sequence.c
+srcs-y += der_sequence_free.c
+srcs-y += der_decode_subject_public_key_info.c
+srcs-y += der_encode_subject_public_key_info.c
diff --git a/core/lib/libtomcrypt/src/pk/asn1/der/set/der_encode_set.c b/core/lib/libtomcrypt/src/pk/asn1/der/set/der_encode_set.c
new file mode 100644
index 0000000..881a226
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/asn1/der/set/der_encode_set.c
@@ -0,0 +1,137 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+  * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file der_encode_set.c
+  ASN.1 DER, Encode a SET, Tom St Denis
+*/
+
+#ifdef LTC_DER
+
+/* LTC define to ASN.1 TAG */
+static int ltc_to_asn1(ltc_asn1_type v)
+{
+   switch (v) {
+      case LTC_ASN1_BOOLEAN:                 return 0x01;
+      case LTC_ASN1_INTEGER:
+      case LTC_ASN1_SHORT_INTEGER:           return 0x02;
+      case LTC_ASN1_RAW_BIT_STRING:
+      case LTC_ASN1_BIT_STRING:              return 0x03;
+      case LTC_ASN1_OCTET_STRING:            return 0x04;
+      case LTC_ASN1_NULL:                    return 0x05;
+      case LTC_ASN1_OBJECT_IDENTIFIER:       return 0x06;
+      case LTC_ASN1_UTF8_STRING:             return 0x0C;
+      case LTC_ASN1_PRINTABLE_STRING:        return 0x13;
+      case LTC_ASN1_TELETEX_STRING:          return 0x14;
+      case LTC_ASN1_IA5_STRING:              return 0x16;
+      case LTC_ASN1_UTCTIME:                 return 0x17;
+      case LTC_ASN1_SEQUENCE:                return 0x30;
+      case LTC_ASN1_SET:
+      case LTC_ASN1_SETOF:                   return 0x31;
+      case LTC_ASN1_CHOICE:
+      case LTC_ASN1_CONSTRUCTED:
+      case LTC_ASN1_CONTEXT_SPECIFIC:
+      case LTC_ASN1_EOL:                     return -1;
+      default:                               return -1;
+   }
+   return -1;
+}
+
+
+static int qsort_helper(const void *a, const void *b)
+{
+   ltc_asn1_list *A = (ltc_asn1_list *)a, *B = (ltc_asn1_list *)b;
+   int            r;
+
+   r = ltc_to_asn1(A->type) - ltc_to_asn1(B->type);
+
+   /* for QSORT the order is UNDEFINED if they are "equal" which means it is NOT DETERMINISTIC.  So we force it to be :-) */
+   if (r == 0) {
+      /* their order in the original list now determines the position */
+      return A->used - B->used;
+   } else {
+      return r;
+   }
+}
+
+/*
+   Encode a SET type
+   @param list      The list of items to encode
+   @param inlen     The number of items in the list
+   @param out       [out] The destination
+   @param outlen    [in/out] The size of the output
+   @return CRYPT_OK on success
+*/
+int der_encode_set(ltc_asn1_list *list, unsigned long inlen,
+                   unsigned char *out,  unsigned long *outlen)
+{
+   ltc_asn1_list  *copy;
+   unsigned long   x;
+   int             err;
+
+   /* make copy of list */
+   copy = XCALLOC(inlen, sizeof(*copy));
+   if (copy == NULL) {
+      return CRYPT_MEM;
+   }
+
+   /* fill in used member with index so we can fully sort it */
+   for (x = 0; x < inlen; x++) {
+       copy[x]      = list[x];
+       copy[x].used = x;
+   }
+
+   /* sort it by the "type" field */
+   XQSORT(copy, inlen, sizeof(*copy), &qsort_helper);
+
+   /* call der_encode_sequence_ex() */
+   err = der_encode_sequence_ex(copy, inlen, out, outlen, LTC_ASN1_SET);
+
+   /* free list */
+   XFREE(copy);
+
+   return err;
+}
+
+
+#endif
+
+/* $Source$ */
+/* $Revision$ */
+/* $Date$ */
diff --git a/core/lib/libtomcrypt/src/pk/asn1/der/set/der_encode_setof.c b/core/lib/libtomcrypt/src/pk/asn1/der/set/der_encode_setof.c
new file mode 100644
index 0000000..66fe72f
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/asn1/der/set/der_encode_setof.c
@@ -0,0 +1,190 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file der_encode_setof.c
+  ASN.1 DER, Encode SET OF, Tom St Denis
+*/
+
+#ifdef LTC_DER
+
+struct edge {
+   unsigned char *start;
+   unsigned long  size;
+};
+
+static int qsort_helper(const void *a, const void *b)
+{
+   struct edge   *A = (struct edge *)a, *B = (struct edge *)b;
+   int            r;
+   unsigned long  x;
+
+   /* compare min length */
+   r = XMEMCMP(A->start, B->start, MIN(A->size, B->size));
+
+   if (r == 0 && A->size != B->size) {
+      if (A->size > B->size) {
+         for (x = B->size; x < A->size; x++) {
+            if (A->start[x]) {
+               return 1;
+            }
+         }
+      } else {
+         for (x = A->size; x < B->size; x++) {
+            if (B->start[x]) {
+               return -1;
+            }
+         }
+      }
+   }
+
+   return r;
+}
+
+/**
+   Encode a SETOF stucture
+   @param list      The list of items to encode
+   @param inlen     The number of items in the list
+   @param out       [out] The destination
+   @param outlen    [in/out] The size of the output
+   @return CRYPT_OK on success
+*/
+int der_encode_setof(ltc_asn1_list *list, unsigned long inlen,
+                     unsigned char *out,  unsigned long *outlen)
+{
+   unsigned long  x, y, z;
+   ptrdiff_t hdrlen;
+   int            err;
+   struct edge   *edges;
+   unsigned char *ptr, *buf;
+
+   /* check that they're all the same type */
+   for (x = 1; x < inlen; x++) {
+      if (list[x].type != list[x-1].type) {
+         return CRYPT_INVALID_ARG;
+      }
+   }
+
+   /* alloc buffer to store copy of output */
+   buf = XCALLOC(1, *outlen);
+   if (buf == NULL) {
+      return CRYPT_MEM;
+   }
+
+   /* encode list */
+   if ((err = der_encode_sequence_ex(list, inlen, buf, outlen, LTC_ASN1_SETOF)) != CRYPT_OK) {
+       XFREE(buf);
+       return err;
+   }
+
+   /* allocate edges */
+   edges = XCALLOC(inlen, sizeof(*edges));
+   if (edges == NULL) {
+      XFREE(buf);
+      return CRYPT_MEM;
+   }
+
+   /* skip header */
+      ptr = buf + 1;
+
+      /* now skip length data */
+      x = *ptr++;
+      if (x >= 0x80) {
+         ptr += (x & 0x7F);
+      }
+
+      /* get the size of the static header */
+      hdrlen = ptr - buf;
+
+
+   /* scan for edges */
+   x = 0;
+   while (ptr < (buf + *outlen)) {
+      /* store start */
+      edges[x].start = ptr;
+
+      /* skip type */
+      z = 1;
+
+      /* parse length */
+      y = ptr[z++];
+      if (y < 128) {
+         edges[x].size = y;
+      } else {
+         y &= 0x7F;
+         edges[x].size = 0;
+         while (y--) {
+            edges[x].size = (edges[x].size << 8) | ((unsigned long)ptr[z++]);
+         }
+      }
+
+      /* skip content */
+      edges[x].size += z;
+      ptr           += edges[x].size;
+      ++x;
+   }
+
+   /* sort based on contents (using edges) */
+   XQSORT(edges, inlen, sizeof(*edges), &qsort_helper);
+
+   /* copy static header */
+   XMEMCPY(out, buf, hdrlen);
+
+   /* copy+sort using edges+indecies to output from buffer */
+   for (y = hdrlen, x = 0; x < inlen; x++) {
+      XMEMCPY(out+y, edges[x].start, edges[x].size);
+      y += edges[x].size;
+   }
+
+#ifdef LTC_CLEAN_STACK
+   zeromem(buf, *outlen);
+#endif
+
+   /* free buffers */
+   XFREE(edges);
+   XFREE(buf);
+
+   return CRYPT_OK;
+}
+
+#endif
+
+/* $Source$ */
+/* $Revision$ */
+/* $Date$ */
diff --git a/core/lib/libtomcrypt/src/pk/asn1/der/set/sub.mk b/core/lib/libtomcrypt/src/pk/asn1/der/set/sub.mk
new file mode 100644
index 0000000..52525d6
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/asn1/der/set/sub.mk
@@ -0,0 +1,2 @@
+srcs-y += der_encode_set.c
+srcs-y += der_encode_setof.c
diff --git a/core/lib/libtomcrypt/src/pk/asn1/der/short_integer/der_decode_short_integer.c b/core/lib/libtomcrypt/src/pk/asn1/der/short_integer/der_decode_short_integer.c
new file mode 100644
index 0000000..462f750
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/asn1/der/short_integer/der_decode_short_integer.c
@@ -0,0 +1,95 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file der_decode_short_integer.c
+  ASN.1 DER, decode an integer, Tom St Denis
+*/
+
+
+#ifdef LTC_DER
+
+/**
+  Read a short integer
+  @param in       The DER encoded data
+  @param inlen    Size of data
+  @param num      [out] The integer to decode
+  @return CRYPT_OK if successful
+*/
+int der_decode_short_integer(const unsigned char *in, unsigned long inlen, unsigned long *num)
+{
+   unsigned long len, x, y;
+
+   LTC_ARGCHK(num    != NULL);
+   LTC_ARGCHK(in     != NULL);
+
+   /* check length */
+   if (inlen < 2) {
+      return CRYPT_INVALID_PACKET;
+   }
+
+   /* check header */
+   x = 0;
+   if ((in[x++] & 0x1F) != 0x02) {
+      return CRYPT_INVALID_PACKET;
+   }
+
+   /* get the packet len */
+   len = in[x++];
+
+   if (x + len > inlen) {
+      return CRYPT_INVALID_PACKET;
+   }
+
+   /* read number */
+   y = 0;
+   while (len--) {
+      y = (y<<8) | (unsigned long)in[x++];
+   }
+   *num = y;
+
+   return CRYPT_OK;
+
+}
+
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/pk/asn1/der/short_integer/der_decode_short_integer.c,v $ */
+/* $Revision: 1.7 $ */
+/* $Date: 2006/12/28 01:27:24 $ */
diff --git a/core/lib/libtomcrypt/src/pk/asn1/der/short_integer/der_encode_short_integer.c b/core/lib/libtomcrypt/src/pk/asn1/der/short_integer/der_encode_short_integer.c
new file mode 100644
index 0000000..2f2cf33
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/asn1/der/short_integer/der_encode_short_integer.c
@@ -0,0 +1,124 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file der_encode_short_integer.c
+  ASN.1 DER, encode an integer, Tom St Denis
+*/
+
+
+#ifdef LTC_DER
+
+/**
+  Store a short integer in the range (0,2^32-1)
+  @param num      The integer to encode
+  @param out      [out] The destination for the DER encoded integers
+  @param outlen   [in/out] The max size and resulting size of the DER encoded integers
+  @return CRYPT_OK if successful
+*/
+int der_encode_short_integer(unsigned long num, unsigned char *out, unsigned long *outlen)
+{  
+   unsigned long len, x, y, z;
+   int           err;
+   
+   LTC_ARGCHK(out    != NULL);
+   LTC_ARGCHK(outlen != NULL);
+
+   /* force to 32 bits */
+   num &= 0xFFFFFFFFUL;
+
+   /* find out how big this will be */
+   if ((err = der_length_short_integer(num, &len)) != CRYPT_OK) {
+      return err;
+   }
+
+   if (*outlen < len) {
+      *outlen = len;
+      return CRYPT_BUFFER_OVERFLOW;
+   }
+
+   /* get len of output */
+   z = 0;
+   y = num;
+   while (y) {
+     ++z;
+     y >>= 8;
+   }
+
+   /* handle zero */
+   if (z == 0) {
+      z = 1;
+   }
+
+   /* see if msb is set */
+   z += (num&(1UL<<((z<<3) - 1))) ? 1 : 0;
+
+   /* adjust the number so the msB is non-zero */
+   for (x = 0; (z <= 4) && (x < (4 - z)); x++) {
+      num <<= 8;
+   }
+
+   /* store header */
+   x = 0;
+   out[x++] = 0x02;
+   out[x++] = (unsigned char)z;
+
+   /* if 31st bit is set output a leading zero and decrement count */
+   if (z == 5) {
+      out[x++] = 0;
+      --z;
+   }
+
+   /* store values */
+   for (y = 0; y < z; y++) {
+      out[x++] = (unsigned char)((num >> 24) & 0xFF);
+      num    <<= 8;
+   }
+
+   /* we good */
+   *outlen = x;
+ 
+   return CRYPT_OK;
+}
+
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/pk/asn1/der/short_integer/der_encode_short_integer.c,v $ */
+/* $Revision: 1.8 $ */
+/* $Date: 2006/12/28 01:27:24 $ */
diff --git a/core/lib/libtomcrypt/src/pk/asn1/der/short_integer/der_length_short_integer.c b/core/lib/libtomcrypt/src/pk/asn1/der/short_integer/der_length_short_integer.c
new file mode 100644
index 0000000..1672697
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/asn1/der/short_integer/der_length_short_integer.c
@@ -0,0 +1,97 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file der_length_short_integer.c
+  ASN.1 DER, get length of encoding, Tom St Denis
+*/
+
+
+#ifdef LTC_DER
+/**
+  Gets length of DER encoding of num 
+  @param num    The integer to get the size of 
+  @param outlen [out] The length of the DER encoding for the given integer
+  @return CRYPT_OK if successful
+*/
+int der_length_short_integer(unsigned long num, unsigned long *outlen)
+{
+   unsigned long z, y, len;
+
+   LTC_ARGCHK(outlen  != NULL);
+
+   /* force to 32 bits */
+   num &= 0xFFFFFFFFUL;
+
+   /* get the number of bytes */
+   z = 0;
+   y = num;
+   while (y) {
+     ++z;
+     y >>= 8;
+   }
+   
+   /* handle zero */
+   if (z == 0) {
+      z = 1;
+   }
+
+   /* we need a 0x02 to indicate it's INTEGER */
+   len = 1;
+
+   /* length byte */
+   ++len;
+
+   /* bytes in value */
+   len += z;
+
+   /* see if msb is set */
+   len += (num&(1UL<<((z<<3) - 1))) ? 1 : 0;
+
+   /* return length */
+   *outlen = len; 
+   
+   return CRYPT_OK;
+}
+
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/pk/asn1/der/short_integer/der_length_short_integer.c,v $ */
+/* $Revision: 1.6 $ */
+/* $Date: 2006/12/28 01:27:24 $ */
diff --git a/core/lib/libtomcrypt/src/pk/asn1/der/short_integer/sub.mk b/core/lib/libtomcrypt/src/pk/asn1/der/short_integer/sub.mk
new file mode 100644
index 0000000..fae1f08
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/asn1/der/short_integer/sub.mk
@@ -0,0 +1,3 @@
+srcs-y += der_decode_short_integer.c
+srcs-y += der_encode_short_integer.c
+srcs-y += der_length_short_integer.c
diff --git a/core/lib/libtomcrypt/src/pk/asn1/der/sub.mk b/core/lib/libtomcrypt/src/pk/asn1/der/sub.mk
new file mode 100644
index 0000000..b704c65
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/asn1/der/sub.mk
@@ -0,0 +1,14 @@
+subdirs-y += bit
+subdirs-y += boolean
+subdirs-y += choice
+subdirs-y += ia5
+subdirs-y += integer
+subdirs-y += object_identifier
+subdirs-y += octet
+subdirs-y += printable_string
+subdirs-y += sequence
+subdirs-y += set
+subdirs-y += short_integer
+subdirs-y += utctime
+subdirs-y += utf8
+subdirs-y += teletex_string
diff --git a/core/lib/libtomcrypt/src/pk/asn1/der/teletex_string/der_decode_teletex_string.c b/core/lib/libtomcrypt/src/pk/asn1/der/teletex_string/der_decode_teletex_string.c
new file mode 100644
index 0000000..a8eaecf
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/asn1/der/teletex_string/der_decode_teletex_string.c
@@ -0,0 +1,122 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file der_decode_teletex_string.c
+  ASN.1 DER, encode a teletex STRING
+*/
+
+#ifdef LTC_DER
+
+/**
+  Store a teletex STRING
+  @param in      The DER encoded teletex STRING
+  @param inlen   The size of the DER teletex STRING
+  @param out     [out] The array of octets stored (one per char)
+  @param outlen  [in/out] The number of octets stored
+  @return CRYPT_OK if successful
+*/
+int der_decode_teletex_string(const unsigned char *in, unsigned long inlen,
+                                unsigned char *out, unsigned long *outlen)
+{
+   unsigned long x, y, len;
+   int           t;
+
+   LTC_ARGCHK(in     != NULL);
+   LTC_ARGCHK(out    != NULL);
+   LTC_ARGCHK(outlen != NULL);
+
+   /* must have header at least */
+   if (inlen < 2) {
+      return CRYPT_INVALID_PACKET;
+   }
+
+   /* check for 0x14 */
+   if ((in[0] & 0x1F) != 0x14) {
+      return CRYPT_INVALID_PACKET;
+   }
+   x = 1;
+
+   /* decode the length */
+   if (in[x] & 0x80) {
+      /* valid # of bytes in length are 1,2,3 */
+      y = in[x] & 0x7F;
+      if ((y == 0) || (y > 3) || ((x + y) > inlen)) {
+         return CRYPT_INVALID_PACKET;
+      }
+
+      /* read the length in */
+      len = 0;
+      ++x;
+      while (y--) {
+         len = (len << 8) | in[x++];
+      }
+   } else {
+      len = in[x++] & 0x7F;
+   }
+
+   /* is it too long? */
+   if (len > *outlen) {
+      *outlen = len;
+      return CRYPT_BUFFER_OVERFLOW;
+   }
+
+   if (len + x > inlen) {
+      return CRYPT_INVALID_PACKET;
+   }
+
+   /* read the data */
+   for (y = 0; y < len; y++) {
+       t = der_teletex_value_decode(in[x++]);
+       if (t == -1) {
+           return CRYPT_INVALID_ARG;
+       }
+       out[y] = t;
+   }
+
+   *outlen = y;
+
+   return CRYPT_OK;
+}
+
+#endif
+
+/* $Source$ */
+/* $Revision$ */
+/* $Date$ */
diff --git a/core/lib/libtomcrypt/src/pk/asn1/der/teletex_string/der_length_teletex_string.c b/core/lib/libtomcrypt/src/pk/asn1/der/teletex_string/der_length_teletex_string.c
new file mode 100644
index 0000000..7435e72
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/asn1/der/teletex_string/der_length_teletex_string.c
@@ -0,0 +1,237 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file der_length_teletex_string.c
+  ASN.1 DER, get length of teletex STRING
+*/
+
+#ifdef LTC_DER
+
+static const struct {
+   int code, value;
+} teletex_table[] = {
+{ '\0',  0 },
+{ '\a',  7 },
+{ '\b',  8 },
+{ '\t',  9 },
+{ '\n', 10 },
+{ '\v', 11 },
+{ '\f', 12 },
+{ '\r', 13 },
+{ ' ',  32 }, 
+{ '!',  33 }, 
+{ '"',  34 }, 
+{ '%',  37 }, 
+{ '&',  38 }, 
+{ '\'', 39 }, 
+{ '(',  40 }, 
+{ ')',  41 }, 
+{ '+',  43 }, 
+{ ',',  44 }, 
+{ '-',  45 }, 
+{ '.',  46 }, 
+{ '/',  47 }, 
+{ '0',  48 }, 
+{ '1',  49 }, 
+{ '2',  50 }, 
+{ '3',  51 }, 
+{ '4',  52 }, 
+{ '5',  53 }, 
+{ '6',  54 }, 
+{ '7',  55 }, 
+{ '8',  56 }, 
+{ '9',  57 }, 
+{ ':',  58 }, 
+{ ';',  59 }, 
+{ '<',  60 }, 
+{ '=',  61 }, 
+{ '>',  62 }, 
+{ '?',  63 }, 
+{ '@',  64 }, 
+{ 'A',  65 }, 
+{ 'B',  66 }, 
+{ 'C',  67 }, 
+{ 'D',  68 }, 
+{ 'E',  69 }, 
+{ 'F',  70 }, 
+{ 'G',  71 }, 
+{ 'H',  72 }, 
+{ 'I',  73 }, 
+{ 'J',  74 }, 
+{ 'K',  75 }, 
+{ 'L',  76 }, 
+{ 'M',  77 }, 
+{ 'N',  78 }, 
+{ 'O',  79 }, 
+{ 'P',  80 }, 
+{ 'Q',  81 }, 
+{ 'R',  82 }, 
+{ 'S',  83 }, 
+{ 'T',  84 }, 
+{ 'U',  85 }, 
+{ 'V',  86 }, 
+{ 'W',  87 }, 
+{ 'X',  88 }, 
+{ 'Y',  89 }, 
+{ 'Z',  90 }, 
+{ '[',  91 }, 
+{ ']',  93 }, 
+{ '_',  95 }, 
+{ 'a',  97 }, 
+{ 'b',  98 }, 
+{ 'c',  99 }, 
+{ 'd',  100 }, 
+{ 'e',  101 }, 
+{ 'f',  102 }, 
+{ 'g',  103 }, 
+{ 'h',  104 }, 
+{ 'i',  105 }, 
+{ 'j',  106 }, 
+{ 'k',  107 }, 
+{ 'l',  108 }, 
+{ 'm',  109 }, 
+{ 'n',  110 }, 
+{ 'o',  111 }, 
+{ 'p',  112 }, 
+{ 'q',  113 }, 
+{ 'r',  114 }, 
+{ 's',  115 }, 
+{ 't',  116 }, 
+{ 'u',  117 }, 
+{ 'v',  118 }, 
+{ 'w',  119 }, 
+{ 'x',  120 }, 
+{ 'y',  121 }, 
+{ 'z',  122 }, 
+{ '|',  124 }, 
+{ ' ',  160 }, 
+{ 0xa1, 161 }, 
+{ 0xa2, 162 }, 
+{ 0xa3, 163 }, 
+{ '$',  164 }, 
+{ 0xa5, 165 }, 
+{ '#',  166 }, 
+{ 0xa7, 167 }, 
+{ 0xa4, 168 }, 
+{ 0xab, 171 }, 
+{ 0xb0, 176 }, 
+{ 0xb1, 177 }, 
+{ 0xb2, 178 }, 
+{ 0xb3, 179 }, 
+{ 0xd7, 180 }, 
+{ 0xb5, 181 }, 
+{ 0xb6, 182 }, 
+{ 0xb7, 183 }, 
+{ 0xf7, 184 }, 
+{ 0xbb, 187 }, 
+{ 0xbc, 188 }, 
+{ 0xbd, 189 }, 
+{ 0xbe, 190 }, 
+{ 0xbf, 191 }, 
+};
+
+int der_teletex_char_encode(int c)
+{
+   int x;
+   for (x = 0; x < (int)(sizeof(teletex_table)/sizeof(teletex_table[0])); x++) {
+       if (teletex_table[x].code == c) {
+          return teletex_table[x].value;
+       }
+   }
+   return -1;
+}
+
+int der_teletex_value_decode(int v)
+{
+   int x;
+   for (x = 0; x < (int)(sizeof(teletex_table)/sizeof(teletex_table[0])); x++) {
+       if (teletex_table[x].value == v) {
+          return teletex_table[x].code;
+       }
+   }
+   return -1;
+}
+   
+/**
+  Gets length of DER encoding of teletex STRING 
+  @param octets   The values you want to encode 
+  @param noctets  The number of octets in the string to encode
+  @param outlen   [out] The length of the DER encoding for the given string
+  @return CRYPT_OK if successful
+*/
+int der_length_teletex_string(const unsigned char *octets, unsigned long noctets, unsigned long *outlen)
+{
+   unsigned long x;
+
+   LTC_ARGCHK(outlen != NULL);
+   LTC_ARGCHK(octets != NULL);
+
+   /* scan string for validity */
+   for (x = 0; x < noctets; x++) {
+       if (der_teletex_char_encode(octets[x]) == -1) {
+          return CRYPT_INVALID_ARG;
+       }
+   }
+
+   if (noctets < 128) {
+      /* 16 LL DD DD DD ... */
+      *outlen = 2 + noctets;
+   } else if (noctets < 256) {
+      /* 16 81 LL DD DD DD ... */
+      *outlen = 3 + noctets;
+   } else if (noctets < 65536UL) {
+      /* 16 82 LL LL DD DD DD ... */
+      *outlen = 4 + noctets;
+   } else if (noctets < 16777216UL) {
+      /* 16 83 LL LL LL DD DD DD ... */
+      *outlen = 5 + noctets;
+   } else {
+      return CRYPT_INVALID_ARG;
+   }
+
+   return CRYPT_OK;
+}
+
+#endif
+
+
+/* $Source$ */
+/* $Revision$ */
+/* $Date$ */
diff --git a/core/lib/libtomcrypt/src/pk/asn1/der/teletex_string/sub.mk b/core/lib/libtomcrypt/src/pk/asn1/der/teletex_string/sub.mk
new file mode 100644
index 0000000..8f47f07
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/asn1/der/teletex_string/sub.mk
@@ -0,0 +1,2 @@
+srcs-y += der_decode_teletex_string.c
+srcs-y += der_length_teletex_string.c
diff --git a/core/lib/libtomcrypt/src/pk/asn1/der/utctime/der_decode_utctime.c b/core/lib/libtomcrypt/src/pk/asn1/der/utctime/der_decode_utctime.c
new file mode 100644
index 0000000..328051d
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/asn1/der/utctime/der_decode_utctime.c
@@ -0,0 +1,156 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file der_decode_utctime.c
+  ASN.1 DER, decode a  UTCTIME, Tom St Denis
+*/
+
+#ifdef LTC_DER
+
+static int char_to_int(unsigned char x)
+{
+   switch (x)  {
+      case '0': return 0;
+      case '1': return 1;
+      case '2': return 2;
+      case '3': return 3;
+      case '4': return 4;
+      case '5': return 5;
+      case '6': return 6;
+      case '7': return 7;
+      case '8': return 8;
+      case '9': return 9;
+      default:
+          return 100;
+   }
+   return 100;
+}
+
+#define DECODE_V(y, max) \
+   y  = char_to_int(buf[x])*10 + char_to_int(buf[x+1]); \
+   if (y >= max) return CRYPT_INVALID_PACKET;           \
+   x += 2;
+
+/**
+  Decodes a UTC time structure in DER format (reads all 6 valid encoding formats)
+  @param in     Input buffer
+  @param inlen  Length of input buffer in octets
+  @param out    [out] Destination of UTC time structure
+  @return CRYPT_OK   if successful
+*/
+int der_decode_utctime(const unsigned char *in, unsigned long *inlen,
+                             ltc_utctime   *out)
+{
+   unsigned char buf[32];
+   unsigned long x;
+   int           y;
+
+   LTC_ARGCHK(in    != NULL);
+   LTC_ARGCHK(inlen != NULL);
+   LTC_ARGCHK(out   != NULL);
+
+   /* check header */
+   if (*inlen < 2UL || (in[1] >= sizeof(buf)) || ((in[1] + 2UL) > *inlen)) {
+      return CRYPT_INVALID_PACKET;
+   }
+
+   /* decode the string */
+   for (x = 0; x < in[1]; x++) {
+       y = der_ia5_value_decode(in[x+2]);
+       if (y == -1) {
+          return CRYPT_INVALID_PACKET;
+       }
+       buf[x] = y;
+   }
+   *inlen = 2 + x;
+
+
+   /* possible encodings are 
+YYMMDDhhmmZ
+YYMMDDhhmm+hh'mm'
+YYMMDDhhmm-hh'mm'
+YYMMDDhhmmssZ
+YYMMDDhhmmss+hh'mm'
+YYMMDDhhmmss-hh'mm'
+
+    So let's do a trivial decode upto [including] mm 
+   */
+
+    x = 0;
+    DECODE_V(out->YY, 100);
+    DECODE_V(out->MM, 13);
+    DECODE_V(out->DD, 32);
+    DECODE_V(out->hh, 24);
+    DECODE_V(out->mm, 60);
+
+    /* clear timezone and seconds info */
+    out->off_dir = out->off_hh = out->off_mm = out->ss = 0;
+
+    /* now is it Z, +, - or 0-9 */
+    if (buf[x] == 'Z') {
+       return CRYPT_OK;
+    } else if (buf[x] == '+' || buf[x] == '-') {
+       out->off_dir = (buf[x++] == '+') ? 0 : 1;
+       DECODE_V(out->off_hh, 24);
+       DECODE_V(out->off_mm, 60);
+       return CRYPT_OK;
+    }
+
+    /* decode seconds */
+    DECODE_V(out->ss, 60);
+
+    /* now is it Z, +, - */
+    if (buf[x] == 'Z') {
+       return CRYPT_OK;
+    } else if (buf[x] == '+' || buf[x] == '-') {
+       out->off_dir = (buf[x++] == '+') ? 0 : 1;
+       DECODE_V(out->off_hh, 24);
+       DECODE_V(out->off_mm, 60);
+       return CRYPT_OK;
+    } else {
+       return CRYPT_INVALID_PACKET;
+    }
+}
+
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/pk/asn1/der/utctime/der_decode_utctime.c,v $ */
+/* $Revision: 1.9 $ */
+/* $Date: 2006/12/28 01:27:24 $ */
diff --git a/core/lib/libtomcrypt/src/pk/asn1/der/utctime/der_encode_utctime.c b/core/lib/libtomcrypt/src/pk/asn1/der/utctime/der_encode_utctime.c
new file mode 100644
index 0000000..e22b294
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/asn1/der/utctime/der_encode_utctime.c
@@ -0,0 +1,110 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file der_encode_utctime.c
+  ASN.1 DER, encode a  UTCTIME, Tom St Denis
+*/
+
+#ifdef LTC_DER
+
+static const char *baseten = "0123456789";
+
+#define STORE_V(y) \
+    out[x++] = der_ia5_char_encode(baseten[(y/10) % 10]); \
+    out[x++] = der_ia5_char_encode(baseten[y % 10]);
+
+/**
+  Encodes a UTC time structure in DER format
+  @param utctime      The UTC time structure to encode
+  @param out          The destination of the DER encoding of the UTC time structure
+  @param outlen       [in/out] The length of the DER encoding
+  @return CRYPT_OK if successful
+*/
+int der_encode_utctime(ltc_utctime *utctime, 
+                       unsigned char *out,   unsigned long *outlen)
+{
+    unsigned long x, tmplen;
+    int           err;
+ 
+    LTC_ARGCHK(utctime != NULL);
+    LTC_ARGCHK(out     != NULL);
+    LTC_ARGCHK(outlen  != NULL);
+
+    if ((err = der_length_utctime(utctime, &tmplen)) != CRYPT_OK) {
+       return err;
+    }
+    if (tmplen > *outlen) {
+        *outlen = tmplen;
+        return CRYPT_BUFFER_OVERFLOW;
+    }
+    
+    /* store header */
+    out[0] = 0x17;
+
+    /* store values */
+    x = 2;
+    STORE_V(utctime->YY);
+    STORE_V(utctime->MM);
+    STORE_V(utctime->DD);
+    STORE_V(utctime->hh);
+    STORE_V(utctime->mm);
+    STORE_V(utctime->ss);
+
+    if (utctime->off_mm || utctime->off_hh) {
+       out[x++] = der_ia5_char_encode(utctime->off_dir ? '-' : '+');
+       STORE_V(utctime->off_hh);
+       STORE_V(utctime->off_mm);
+    } else {
+       out[x++] = der_ia5_char_encode('Z');
+    }
+
+    /* store length */
+    out[1] = (unsigned char)(x - 2);
+   
+    /* all good let's return */
+    *outlen = x;
+    return CRYPT_OK;
+}
+
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/pk/asn1/der/utctime/der_encode_utctime.c,v $ */
+/* $Revision: 1.10 $ */
+/* $Date: 2006/12/28 01:27:24 $ */
diff --git a/core/lib/libtomcrypt/src/pk/asn1/der/utctime/der_length_utctime.c b/core/lib/libtomcrypt/src/pk/asn1/der/utctime/der_length_utctime.c
new file mode 100644
index 0000000..055b726
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/asn1/der/utctime/der_length_utctime.c
@@ -0,0 +1,73 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file der_length_utctime.c
+  ASN.1 DER, get length of UTCTIME, Tom St Denis
+*/
+
+#ifdef LTC_DER
+
+/**
+  Gets length of DER encoding of UTCTIME
+  @param utctime      The UTC time structure to get the size of
+  @param outlen [out] The length of the DER encoding
+  @return CRYPT_OK if successful
+*/
+int der_length_utctime(ltc_utctime *utctime, unsigned long *outlen)
+{
+   LTC_ARGCHK(outlen  != NULL);
+   LTC_ARGCHK(utctime != NULL);
+
+   if (utctime->off_hh == 0 && utctime->off_mm == 0) {
+      /* we encode as YYMMDDhhmmssZ */
+      *outlen = 2 + 13;
+   } else {
+      /* we encode as YYMMDDhhmmss{+|-}hh'mm' */
+      *outlen = 2 + 17;
+   }
+
+   return CRYPT_OK;
+}
+
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/pk/asn1/der/utctime/der_length_utctime.c,v $ */
+/* $Revision: 1.5 $ */
+/* $Date: 2006/12/28 01:27:24 $ */
diff --git a/core/lib/libtomcrypt/src/pk/asn1/der/utctime/sub.mk b/core/lib/libtomcrypt/src/pk/asn1/der/utctime/sub.mk
new file mode 100644
index 0000000..afb3e1b
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/asn1/der/utctime/sub.mk
@@ -0,0 +1,3 @@
+srcs-y += der_decode_utctime.c
+srcs-y += der_encode_utctime.c
+srcs-y += der_length_utctime.c
diff --git a/core/lib/libtomcrypt/src/pk/asn1/der/utf8/der_decode_utf8_string.c b/core/lib/libtomcrypt/src/pk/asn1/der/utf8/der_decode_utf8_string.c
new file mode 100644
index 0000000..6226dd5
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/asn1/der/utf8/der_decode_utf8_string.c
@@ -0,0 +1,138 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file der_decode_utf8_string.c
+  ASN.1 DER, encode a UTF8 STRING, Tom St Denis
+*/
+
+
+#ifdef LTC_DER
+
+/**
+  Store a UTF8 STRING
+  @param in      The DER encoded UTF8 STRING
+  @param inlen   The size of the DER UTF8 STRING
+  @param out     [out] The array of utf8s stored (one per char)
+  @param outlen  [in/out] The number of utf8s stored
+  @return CRYPT_OK if successful
+*/
+int der_decode_utf8_string(const unsigned char *in,  unsigned long inlen,
+                                       wchar_t *out, unsigned long *outlen)
+{
+   wchar_t       tmp;
+   unsigned long x, y, z, len;
+
+   LTC_ARGCHK(in     != NULL);
+   LTC_ARGCHK(out    != NULL);
+   LTC_ARGCHK(outlen != NULL);
+
+   /* must have header at least */
+   if (inlen < 2) {
+      return CRYPT_INVALID_PACKET;
+   }
+
+   /* check for 0x0C */
+   if ((in[0] & 0x1F) != 0x0C) {
+      return CRYPT_INVALID_PACKET;
+   }
+   x = 1;
+
+   /* decode the length */
+   if (in[x] & 0x80) {
+      /* valid # of bytes in length are 1,2,3 */
+      y = in[x] & 0x7F;
+      if ((y == 0) || (y > 3) || ((x + y) > inlen)) {
+         return CRYPT_INVALID_PACKET;
+      }
+
+      /* read the length in */
+      len = 0;
+      ++x;
+      while (y--) {
+         len = (len << 8) | in[x++];
+      }
+   } else {
+      len = in[x++] & 0x7F;
+   }
+
+   if (len + x > inlen) {
+      return CRYPT_INVALID_PACKET;
+   }
+
+   /* proceed to decode */
+   for (y = 0; x < inlen; ) {
+      /* get first byte */
+      tmp = in[x++];
+ 
+      /* count number of bytes */
+      for (z = 0; (tmp & 0x80) && (z <= 4); z++, tmp = (tmp << 1) & 0xFF);
+      
+      if (z > 4 || (x + (z - 1) > inlen)) {
+         return CRYPT_INVALID_PACKET;
+      }
+
+      /* decode, grab upper bits */
+      tmp >>= z;
+
+      /* grab remaining bytes */
+      if (z > 1) { --z; }
+      while (z-- != 0) {
+         if ((in[x] & 0xC0) != 0x80) {
+            return CRYPT_INVALID_PACKET;
+         }
+         tmp = (tmp << 6) | ((wchar_t)in[x++] & 0x3F);
+      }
+
+      if (y > *outlen) {
+         *outlen = y;
+         return CRYPT_BUFFER_OVERFLOW;
+      }
+      out[y++] = tmp;
+   }
+   *outlen = y;
+
+   return CRYPT_OK;
+}
+ 
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/pk/asn1/der/utf8/der_decode_utf8_string.c,v $ */
+/* $Revision: 1.8 $ */
+/* $Date: 2006/12/28 01:27:24 $ */
diff --git a/core/lib/libtomcrypt/src/pk/asn1/der/utf8/der_encode_utf8_string.c b/core/lib/libtomcrypt/src/pk/asn1/der/utf8/der_encode_utf8_string.c
new file mode 100644
index 0000000..684f31b
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/asn1/der/utf8/der_encode_utf8_string.c
@@ -0,0 +1,134 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file der_encode_utf8_string.c
+  ASN.1 DER, encode a UTF8 STRING, Tom St Denis
+*/
+
+
+#ifdef LTC_DER
+
+/**
+  Store an UTF8 STRING
+  @param in       The array of UTF8 to store (one per wchar_t)
+  @param inlen    The number of UTF8 to store
+  @param out      [out] The destination for the DER encoded UTF8 STRING
+  @param outlen   [in/out] The max size and resulting size of the DER UTF8 STRING
+  @return CRYPT_OK if successful
+*/
+int der_encode_utf8_string(const wchar_t *in,  unsigned long inlen,
+                           unsigned char *out, unsigned long *outlen)
+{
+   unsigned long x, y, len;
+
+   LTC_ARGCHK(in     != NULL);
+   LTC_ARGCHK(out    != NULL);
+   LTC_ARGCHK(outlen != NULL);
+
+   /* get the size */
+   for (x = len = 0; x < inlen; x++) {
+       if (in[x] < 0 || in[x] > 0x1FFFF) {
+          return CRYPT_INVALID_ARG;
+       }
+       len += der_utf8_charsize(in[x]);
+   }
+
+   if (len < 128) {
+      y = 2 + len;
+   } else if (len < 256) {
+      y = 3 + len;
+   } else if (len < 65536UL) {
+      y = 4 + len;
+   } else if (len < 16777216UL) {
+      y = 5 + len;
+   } else {
+      return CRYPT_INVALID_ARG;
+   }
+
+   /* too big? */
+   if (y > *outlen) {
+      *outlen = len;
+      return CRYPT_BUFFER_OVERFLOW;
+   }
+
+   /* encode the header+len */
+   x = 0;
+   out[x++] = 0x0C;
+   if (len < 128) {
+      out[x++] = (unsigned char)len;
+   } else if (len < 256) {
+      out[x++] = 0x81;
+      out[x++] = (unsigned char)len;
+   } else if (len < 65536UL) {
+      out[x++] = 0x82;
+      out[x++] = (unsigned char)((len>>8)&255);
+      out[x++] = (unsigned char)(len&255);
+   } else if (len < 16777216UL) {
+      out[x++] = 0x83;
+      out[x++] = (unsigned char)((len>>16)&255);
+      out[x++] = (unsigned char)((len>>8)&255);
+      out[x++] = (unsigned char)(len&255);
+   } else {
+       /* coverity[dead_error_line] */
+      return CRYPT_INVALID_ARG;
+   }
+
+   /* store UTF8 */
+   for (y = 0; y < inlen; y++) {
+       switch (der_utf8_charsize(in[y])) {
+          case 1: out[x++] = (unsigned char)in[y]; break;
+          case 2: out[x++] = 0xC0 | ((in[y] >> 6) & 0x1F);  out[x++] = 0x80 | (in[y] & 0x3F); break;
+          case 3: out[x++] = 0xE0 | ((in[y] >> 12) & 0x0F); out[x++] = 0x80 | ((in[y] >> 6) & 0x3F); out[x++] = 0x80 | (in[y] & 0x3F); break;
+          case 4: out[x++] = 0xF0 | ((in[y] >> 18) & 0x07); out[x++] = 0x80 | ((in[y] >> 12) & 0x3F); out[x++] = 0x80 | ((in[y] >> 6) & 0x3F); out[x++] = 0x80 | (in[y] & 0x3F); break;
+          default: break;
+       }
+   }
+
+   /* retun length */
+   *outlen = x;
+
+   return CRYPT_OK;
+}
+
+#endif
+
+/* $Source$ */
+/* $Revision$ */
+/* $Date$ */
diff --git a/core/lib/libtomcrypt/src/pk/asn1/der/utf8/der_length_utf8_string.c b/core/lib/libtomcrypt/src/pk/asn1/der/utf8/der_length_utf8_string.c
new file mode 100644
index 0000000..17dd672
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/asn1/der/utf8/der_length_utf8_string.c
@@ -0,0 +1,110 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file der_length_utf8_string.c
+  ASN.1 DER, get length of UTF8 STRING, Tom St Denis
+*/
+
+#ifdef LTC_DER
+
+/** Return the size in bytes of a UTF-8 character
+  @param c   The UTF-8 character to measure
+  @return    The size in bytes
+*/
+unsigned long der_utf8_charsize(const wchar_t c)
+{
+   if (c <= 0x7F) {
+      return 1;
+   } else if (c <= 0x7FF) {
+      return 2;
+   } else if (c <= 0xFFFF) {
+      return 3;
+   } else {
+      return 4;
+   }
+}
+
+/**
+  Gets length of DER encoding of UTF8 STRING 
+  @param in       The characters to measure the length of
+  @param noctets  The number of octets in the string to encode
+  @param outlen   [out] The length of the DER encoding for the given string
+  @return CRYPT_OK if successful
+*/
+int der_length_utf8_string(const wchar_t *in, unsigned long noctets, unsigned long *outlen)
+{
+   unsigned long x, len;
+
+   LTC_ARGCHK(in     != NULL);
+   LTC_ARGCHK(outlen != NULL);
+
+   len = 0;
+   for (x = 0; x < noctets; x++) {
+      if (in[x] < 0 || in[x] > 0x10FFFF) {
+         return CRYPT_INVALID_ARG;
+      }
+      len += der_utf8_charsize(in[x]);
+   }
+
+   if (len < 128) {
+      /* 0C LL DD DD DD ... */
+      *outlen = 2 + len;
+   } else if (len < 256) {
+      /* 0C 81 LL DD DD DD ... */
+      *outlen = 3 + len;
+   } else if (len < 65536UL) {
+      /* 0C 82 LL LL DD DD DD ... */
+      *outlen = 4 + len;
+   } else if (len < 16777216UL) {
+      /* 0C 83 LL LL LL DD DD DD ... */
+      *outlen = 5 + len;
+   } else {
+      return CRYPT_INVALID_ARG;
+   }
+
+   return CRYPT_OK;
+}
+
+#endif
+
+
+/* $Source: /cvs/libtom/libtomcrypt/src/pk/asn1/der/utf8/der_length_utf8_string.c,v $ */
+/* $Revision: 1.6 $ */
+/* $Date: 2006/12/28 01:27:24 $ */
diff --git a/core/lib/libtomcrypt/src/pk/asn1/der/utf8/sub.mk b/core/lib/libtomcrypt/src/pk/asn1/der/utf8/sub.mk
new file mode 100644
index 0000000..3538929
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/asn1/der/utf8/sub.mk
@@ -0,0 +1,4 @@
+cflags-remove-y += -Wextra
+srcs-y += der_decode_utf8_string.c
+srcs-y += der_encode_utf8_string.c
+srcs-y += der_length_utf8_string.c
diff --git a/core/lib/libtomcrypt/src/pk/asn1/sub.mk b/core/lib/libtomcrypt/src/pk/asn1/sub.mk
new file mode 100644
index 0000000..c93e1f1
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/asn1/sub.mk
@@ -0,0 +1 @@
+subdirs-y += der
diff --git a/core/lib/libtomcrypt/src/pk/dh/dh.c b/core/lib/libtomcrypt/src/pk/dh/dh.c
new file mode 100644
index 0000000..d97f704
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/dh/dh.c
@@ -0,0 +1,217 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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 <tomcrypt.h>
+
+#ifdef LTC_LINARO_FIX_DH
+
+#include <stdint.h>
+/*
+ * Make a DH key [private key pair]
+ * @param prng     An active PRNG state
+ * @param wprng    The index for the PRNG you desire to use
+ * @param keysize  The key size (octets) desired of the private key
+ * @param q        If not null, then the private key is in the range
+ *                 [2, q-2] where q is called the subprime
+ * @param xbits    If not 0, then the private key has 'xbits' bits
+ * @note           The private key must always be less than p-1
+ * @param key      [in/out] Where the newly created DH key will be stored
+ *                  g and p are provided as input in the key
+ *                  type, x and y are output of this function
+ * @return CRYPT_OK if successful, note: on error all allocated memory will be
+ *         freed automatically.
+*/
+
+int dh_make_key(prng_state *prng, int wprng, void *q, int xbits, dh_key *key)
+{
+	const int limit = 500;	/* number of tries */
+	int err, i;
+	int key_size = 0;	/* max key size, in bytes */
+	int key_size_p = 0;	/* key size of p */
+	int key_size_q = 0;	/* key size of p */
+	void *arg_mod;
+	uint8_t *buf = 0;	/* intermediate buffer to have a raw random  */
+	int found = 0;
+
+	/*
+	 * Check the arguments
+	 */
+	LTC_ARGCHK(key  != NULL);
+	LTC_ARGCHK(key->g  != NULL);
+	LTC_ARGCHK(key->p  != NULL);
+	err = prng_is_valid(wprng);
+	if (err != CRYPT_OK)
+		return err;
+
+	/*
+	 * Set the key size and check constraints
+	 */
+	if (xbits) {
+		LTC_ARGCHK((xbits % 8) == 0);
+		key_size = xbits / 8;
+	}
+	key_size_p = mp_unsigned_bin_size(key->p);
+	if (q)
+		key_size_q = mp_unsigned_bin_size(q);
+	if (key_size) {
+		/* check the constraints */
+		LTC_ARGCHK(key_size <= key_size_p);
+		LTC_ARGCHK((q == NULL) || (key_size <= key_size_q));
+	} else {
+		if (q)
+			key_size = MIN(key_size_p, key_size_q);
+		else
+			key_size =key_size_p;
+	}
+
+	/* Set the argument we will make the modulo against to */
+	if ((q != NULL) && (key_size_q < key_size_p))
+		arg_mod = q;
+	else
+		arg_mod = key->p;
+
+	/* initialize the key */
+	key->x = NULL;
+	key->y = NULL;
+	err = mp_init_multi(&key->x, &key->y, NULL);
+	if (err != CRYPT_OK)
+		goto error;
+
+	/* Initialize the buffer used to store the random number */
+	buf = XMALLOC(key_size);
+	if (buf == NULL) {
+		err = CRYPT_MEM;
+		goto error;
+	}
+
+	for (i = 0; (i < limit) && (!found); i++) {
+		/* generate the private key in a raw-buffer */
+		if (prng_descriptor[wprng]->read(buf, key_size, prng) !=
+		    (unsigned long)key_size) {
+			err = CRYPT_ERROR_READPRNG;
+			goto error;
+		}
+
+		/* make sure it is on the right number of bits */
+		if (xbits)
+		      buf[0] |= 0x80;
+
+		/* transform it as a Big Number */
+		err = mp_read_unsigned_bin(key->x, buf, key_size);
+		if (err != CRYPT_OK)
+			goto error;
+
+		/*
+		 * Transform it as a Big Number compatible with p and q
+		 */
+		err = mp_read_unsigned_bin(key->y, buf, key_size);
+		if (err != CRYPT_OK)
+			goto error;
+		err = mp_mod(key->y, arg_mod, key->x);
+		if (err != CRYPT_OK)
+			goto error;
+
+		/*
+		 * Check the constraints
+		 * - x < p  is ok by construction
+		 * - x < q-1:
+		 * - x contains xbits
+		 */
+		if (xbits) {
+			if (mp_count_bits(key->x) != xbits)
+				continue;
+		}
+
+		/* we found a suitable private key key->x */
+		found = 1;
+	}
+
+	if (!found) {
+		/* key is not found */
+		err = CRYPT_PK_NOT_FOUND;
+		goto error;
+	}
+
+	/* generate the public key key->y */
+	err = mp_exptmod(key->g, key->x, key->p, key->y);
+	if (err != CRYPT_OK)
+		goto error;
+
+	/* no error */
+	err = CRYPT_OK;
+
+error:
+	if (err != CRYPT_OK)
+		mp_clear_multi(key->x, key->y, NULL);
+	if (buf)
+		XFREE(buf);
+
+	return err;
+}
+
+/*
+ * Free the allocated ram for a DH key
+ * @param key   The key which you wish to free
+ */
+void dh_free(dh_key *key)
+{
+	/*
+	 * g and p are not cleared on purpose as they are provided when
+	 * generating the key
+	 */
+	if (key->x) {
+		mp_clear(key->x);
+		key->x = NULL;
+	}
+	if (key->y) {
+		mp_clear(key->y);
+		key->y = NULL;
+	}
+}
+
+/*
+ * Create a DH shared secret.
+ * @param private_key     The private DH key in the pair
+ * @param public_key      The public DH key in the pair, as a big number
+ * @param secret          The secret (as a big number)
+ * @return CRYPT_OK if successful
+ */
+int dh_shared_secret(dh_key *private_key, void *public_key, void *secret)
+{
+	LTC_ARGCHK(private_key != NULL);
+	LTC_ARGCHK(public_key  != NULL);
+	LTC_ARGCHK(secret      != NULL);
+
+	/* types valid? */
+	if (private_key->type != PK_PRIVATE)
+		return CRYPT_PK_NOT_PRIVATE;
+
+	return mp_exptmod(public_key, private_key->x, private_key->p, secret);
+}
+
+#endif
diff --git a/core/lib/libtomcrypt/src/pk/dh/sub.mk b/core/lib/libtomcrypt/src/pk/dh/sub.mk
new file mode 100644
index 0000000..7c36ef1
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/dh/sub.mk
@@ -0,0 +1,2 @@
+srcs-y += dh.c
+cflags-dh.c-y += -Wno-unused-variable
diff --git a/core/lib/libtomcrypt/src/pk/dsa/dsa_decrypt_key.c b/core/lib/libtomcrypt/src/pk/dsa/dsa_decrypt_key.c
new file mode 100644
index 0000000..fde55da
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/dsa/dsa_decrypt_key.c
@@ -0,0 +1,165 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file dsa_decrypt_key.c
+  DSA Crypto, Tom St Denis
+*/  
+
+#ifdef LTC_MDSA
+
+/**
+  Decrypt an DSA encrypted key
+  @param in       The ciphertext
+  @param inlen    The length of the ciphertext (octets)
+  @param out      [out] The plaintext
+  @param outlen   [in/out] The max size and resulting size of the plaintext
+  @param key      The corresponding private DSA key
+  @return CRYPT_OK if successful
+*/
+int dsa_decrypt_key(const unsigned char *in,  unsigned long  inlen,
+                          unsigned char *out, unsigned long *outlen, 
+                          dsa_key *key)
+{
+   unsigned char  *skey, *expt;
+   void           *g_pub;
+   unsigned long  x, y, hashOID[32];
+   int            hash, err;
+   ltc_asn1_list  decode[3];
+
+   LTC_ARGCHK(in     != NULL);
+   LTC_ARGCHK(out    != NULL);
+   LTC_ARGCHK(outlen != NULL);
+   LTC_ARGCHK(key    != NULL);
+
+   /* right key type? */
+   if (key->type != PK_PRIVATE) {
+      return CRYPT_PK_NOT_PRIVATE;
+   }
+   
+   /* decode to find out hash */
+   LTC_SET_ASN1(decode, 0, LTC_ASN1_OBJECT_IDENTIFIER, hashOID, sizeof(hashOID)/sizeof(hashOID[0]));
+ 
+   if ((err = der_decode_sequence(in, inlen, decode, 1)) != CRYPT_OK) {
+      return err;
+   }
+
+   hash = find_hash_oid(hashOID, decode[0].size);                   
+   if (hash_is_valid(hash) != CRYPT_OK) {
+      return CRYPT_INVALID_PACKET;
+   }
+
+   /* we now have the hash! */
+   
+   if ((err = mp_init(&g_pub)) != CRYPT_OK) {
+      return err;
+   }
+
+   /* allocate memory */
+   expt   = XMALLOC(mp_unsigned_bin_size(key->p) + 1);
+   skey   = XMALLOC(MAXBLOCKSIZE);
+   if (expt == NULL || skey == NULL) {
+      if (expt != NULL) {
+         XFREE(expt);
+      }
+      if (skey != NULL) {
+         XFREE(skey);
+      }
+      mp_clear(g_pub);
+      return CRYPT_MEM;
+   }
+   
+   LTC_SET_ASN1(decode, 1, LTC_ASN1_INTEGER,          g_pub,      1UL);
+   LTC_SET_ASN1(decode, 2, LTC_ASN1_OCTET_STRING,      skey,      MAXBLOCKSIZE);
+
+   /* read the structure in now */
+   if ((err = der_decode_sequence(in, inlen, decode, 3)) != CRYPT_OK) {
+      goto LBL_ERR;
+   }
+
+   /* make shared key */
+   x = mp_unsigned_bin_size(key->p) + 1;
+   if ((err = dsa_shared_secret(key->x, g_pub, key, expt, &x)) != CRYPT_OK) {
+      goto LBL_ERR;
+   }
+
+   y = MIN(mp_unsigned_bin_size(key->p) + 1, MAXBLOCKSIZE);
+   if ((err = hash_memory(hash, expt, x, expt, &y)) != CRYPT_OK) {
+      goto LBL_ERR;
+   }
+
+   /* ensure the hash of the shared secret is at least as big as the encrypt itself */
+   if (decode[2].size > y) {
+      err = CRYPT_INVALID_PACKET;
+      goto LBL_ERR;
+   }
+
+   /* avoid buffer overflow */
+   if (*outlen < decode[2].size) {
+      *outlen = decode[2].size;
+      err = CRYPT_BUFFER_OVERFLOW;
+      goto LBL_ERR;
+   }
+
+   /* Decrypt the key */
+   for (x = 0; x < decode[2].size; x++) {
+     out[x] = expt[x] ^ skey[x];
+   }
+   *outlen = x;
+
+   err = CRYPT_OK;
+LBL_ERR:
+#ifdef LTC_CLEAN_STACK
+   zeromem(expt,   mp_unsigned_bin_size(key->p) + 1);
+   zeromem(skey,   MAXBLOCKSIZE);
+#endif
+
+   XFREE(expt);
+   XFREE(skey);
+  
+   mp_clear(g_pub);
+
+   return err;
+}
+
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/pk/dsa/dsa_decrypt_key.c,v $ */
+/* $Revision: 1.11 $ */
+/* $Date: 2007/05/12 14:32:35 $ */
diff --git a/core/lib/libtomcrypt/src/pk/dsa/dsa_encrypt_key.c b/core/lib/libtomcrypt/src/pk/dsa/dsa_encrypt_key.c
new file mode 100644
index 0000000..d69a624
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/dsa/dsa_encrypt_key.c
@@ -0,0 +1,158 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file dsa_encrypt_key.c
+  DSA Crypto, Tom St Denis
+*/  
+
+#ifdef LTC_MDSA
+
+/**
+  Encrypt a symmetric key with DSA
+  @param in         The symmetric key you want to encrypt
+  @param inlen      The length of the key to encrypt (octets)
+  @param out        [out] The destination for the ciphertext
+  @param outlen     [in/out] The max size and resulting size of the ciphertext
+  @param prng       An active PRNG state
+  @param wprng      The index of the PRNG you wish to use 
+  @param hash       The index of the hash you want to use 
+  @param key        The DSA key you want to encrypt to
+  @return CRYPT_OK if successful
+*/
+int dsa_encrypt_key(const unsigned char *in,   unsigned long inlen,
+                          unsigned char *out,  unsigned long *outlen, 
+                          prng_state *prng, int wprng, int hash, 
+                          dsa_key *key)
+{
+    unsigned char *expt, *skey;
+    void          *g_pub, *g_priv;
+    unsigned long  x, y;
+    int            err, qbits;
+
+    LTC_ARGCHK(in      != NULL);
+    LTC_ARGCHK(out     != NULL);
+    LTC_ARGCHK(outlen  != NULL);
+    LTC_ARGCHK(key     != NULL);
+
+    /* check that wprng/cipher/hash are not invalid */
+    if ((err = prng_is_valid(wprng)) != CRYPT_OK) {
+       return err;
+    }
+
+    if ((err = hash_is_valid(hash)) != CRYPT_OK) {
+       return err;
+    }
+
+    if (inlen > hash_descriptor[hash]->hashsize) {
+       return CRYPT_INVALID_HASH;
+    }
+
+    /* make a random key and export the public copy */
+    if ((err = mp_init_multi(&g_pub, &g_priv, NULL)) != CRYPT_OK) {
+       return err;
+    }
+   
+    expt       = XMALLOC(mp_unsigned_bin_size(key->p) + 1);
+    skey       = XMALLOC(MAXBLOCKSIZE);
+    if (expt == NULL  || skey == NULL) {
+       if (expt != NULL) {
+          XFREE(expt);
+       }
+       if (skey != NULL) {
+          XFREE(skey);
+       }
+       mp_clear_multi(g_pub, g_priv, NULL);
+       return CRYPT_MEM;
+    }
+    
+    /* make a random g_priv, g_pub = g^x pair */
+    qbits = mp_count_bits(key->q);
+    do {
+      if ((err = rand_bn_bits(g_priv, qbits, prng, wprng)) != CRYPT_OK) {
+        goto LBL_ERR;
+      }
+      /* private key x should be from range: 1 <= x <= q-1 (see FIPS 186-4 B.1.2) */
+    } while (mp_cmp_d(g_priv, 0) != LTC_MP_GT || mp_cmp(g_priv, key->q) != LTC_MP_LT);
+
+    /* compute y */
+    if ((err = mp_exptmod(key->g, g_priv, key->p, g_pub)) != CRYPT_OK) {
+       goto LBL_ERR;
+    }
+    
+    /* make random key */
+    x        = mp_unsigned_bin_size(key->p) + 1;
+    if ((err = dsa_shared_secret(g_priv, key->y, key, expt, &x)) != CRYPT_OK) {
+       goto LBL_ERR;
+    }
+
+    y = MAXBLOCKSIZE;
+    if ((err = hash_memory(hash, expt, x, skey, &y)) != CRYPT_OK) {
+       goto LBL_ERR;
+    }
+    
+    /* Encrypt key */
+    for (x = 0; x < inlen; x++) {
+      skey[x] ^= in[x];
+    }
+
+    err = der_encode_sequence_multi(out, outlen,
+                                    LTC_ASN1_OBJECT_IDENTIFIER,  hash_descriptor[hash]->OIDlen,   hash_descriptor[hash]->OID,
+                                    LTC_ASN1_INTEGER,            1UL,                            g_pub,
+                                    LTC_ASN1_OCTET_STRING,       inlen,                          skey,
+                                    LTC_ASN1_EOL,                0UL,                            NULL);
+
+LBL_ERR:
+#ifdef LTC_CLEAN_STACK
+    /* clean up */
+    zeromem(expt,   mp_unsigned_bin_size(key->p) + 1);
+    zeromem(skey,   MAXBLOCKSIZE);
+#endif
+
+    XFREE(skey);
+    XFREE(expt);
+    
+    mp_clear_multi(g_pub, g_priv, NULL);
+    return err;
+}
+
+#endif
+/* $Source: /cvs/libtom/libtomcrypt/src/pk/dsa/dsa_encrypt_key.c,v $ */
+/* $Revision: 1.9 $ */
+/* $Date: 2007/05/12 14:32:35 $ */
diff --git a/core/lib/libtomcrypt/src/pk/dsa/dsa_export.c b/core/lib/libtomcrypt/src/pk/dsa/dsa_export.c
new file mode 100644
index 0000000..9424de4
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/dsa/dsa_export.c
@@ -0,0 +1,145 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+   @file dsa_export.c
+   DSA implementation, export key, Tom St Denis
+*/
+
+#ifdef LTC_MDSA
+
+/**
+  Export a DSA key to a binary packet
+  @param out    [out] Where to store the packet
+  @param outlen [in/out] The max size and resulting size of the packet
+  @param type   The type of key to export (PK_PRIVATE or PK_PUBLIC)
+  @param key    The key to export
+  @return CRYPT_OK if successful
+*/
+int dsa_export(unsigned char *out, unsigned long *outlen, int type, dsa_key *key)
+{
+   unsigned long zero=0;
+   int err, std;
+
+   LTC_ARGCHK(out    != NULL);
+   LTC_ARGCHK(outlen != NULL);
+   LTC_ARGCHK(key    != NULL);
+
+   std = type & PK_STD;
+   type &= ~PK_STD;
+
+   /* can we store the static header?  */
+   if (type == PK_PRIVATE && key->type != PK_PRIVATE) {
+      return CRYPT_PK_TYPE_MISMATCH;
+   }
+
+   if (type != PK_PUBLIC && type != PK_PRIVATE) {
+      return CRYPT_INVALID_ARG;
+   }
+
+   if (type == PK_PRIVATE) {
+      if (std) {
+          return der_encode_sequence_multi(out, outlen,
+                                         LTC_ASN1_SHORT_INTEGER, 1UL, &zero,
+                                         LTC_ASN1_INTEGER,      1UL, key->p,
+                                         LTC_ASN1_INTEGER,      1UL, key->q,
+                                         LTC_ASN1_INTEGER,      1UL, key->g,
+                                         LTC_ASN1_INTEGER,      1UL, key->y,
+                                         LTC_ASN1_INTEGER,      1UL, key->x,
+                                         LTC_ASN1_EOL,          0UL, NULL);
+      }
+      else {
+          unsigned char flags[1];
+          flags[0] = 1;
+          return der_encode_sequence_multi(out, outlen,
+                                         LTC_ASN1_BIT_STRING,   1UL, flags,
+                                         LTC_ASN1_INTEGER,      1UL, key->g,
+                                         LTC_ASN1_INTEGER,      1UL, key->p,
+                                         LTC_ASN1_INTEGER,      1UL, key->q,
+                                         LTC_ASN1_INTEGER,      1UL, key->y,
+                                         LTC_ASN1_INTEGER,      1UL, key->x,
+                                         LTC_ASN1_EOL,          0UL, NULL);
+      }
+   } else {
+      if (std) {
+          unsigned long tmplen = (mp_count_bits(key->y) / 8) + 8;
+          unsigned char* tmp = XMALLOC(tmplen);
+          ltc_asn1_list int_list[3];
+
+          if (tmp == NULL) {
+              return CRYPT_MEM;
+          }
+
+          err = der_encode_integer(key->y, tmp, &tmplen);
+          if (err != CRYPT_OK) {
+              goto error;
+          }
+
+          LTC_SET_ASN1(int_list, 0, LTC_ASN1_INTEGER, key->p, 1UL);
+          LTC_SET_ASN1(int_list, 1, LTC_ASN1_INTEGER, key->q, 1UL);
+          LTC_SET_ASN1(int_list, 2, LTC_ASN1_INTEGER, key->g, 1UL);
+
+          err = der_encode_subject_public_key_info(out, outlen, PKA_DSA, tmp,
+              tmplen, LTC_ASN1_SEQUENCE, int_list,
+              sizeof(int_list) / sizeof(int_list[0]));
+
+error:
+          XFREE(tmp);
+          return err;
+      }
+      else {
+          unsigned char flags[1];
+          flags[0] = 0;
+          return der_encode_sequence_multi(out, outlen,
+                                     LTC_ASN1_BIT_STRING,   1UL, flags,
+                                     LTC_ASN1_INTEGER,      1UL, key->g,
+                                     LTC_ASN1_INTEGER,      1UL, key->p,
+                                     LTC_ASN1_INTEGER,      1UL, key->q,
+                                     LTC_ASN1_INTEGER,      1UL, key->y,
+                                     LTC_ASN1_EOL,          0UL, NULL);
+      }
+   }
+}
+
+#endif
+
+
+/* $Source: /cvs/libtom/libtomcrypt/src/pk/dsa/dsa_export.c,v $ */
+/* $Revision: 1.10 $ */
+/* $Date: 2007/05/12 14:32:35 $ */
diff --git a/core/lib/libtomcrypt/src/pk/dsa/dsa_free.c b/core/lib/libtomcrypt/src/pk/dsa/dsa_free.c
new file mode 100644
index 0000000..ca5b887
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/dsa/dsa_free.c
@@ -0,0 +1,61 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+   @file dsa_free.c
+   DSA implementation, free a DSA key, Tom St Denis
+*/
+
+#ifdef LTC_MDSA
+
+/**
+   Free a DSA key
+   @param key   The key to free from memory
+*/
+void dsa_free(dsa_key *key)
+{
+   LTC_ARGCHKVD(key != NULL);
+   mp_clear_multi(key->g, key->q, key->p, key->x, key->y, NULL);
+}
+
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/pk/dsa/dsa_free.c,v $ */
+/* $Revision: 1.8 $ */
+/* $Date: 2007/05/12 14:32:35 $ */
diff --git a/core/lib/libtomcrypt/src/pk/dsa/dsa_import.c b/core/lib/libtomcrypt/src/pk/dsa/dsa_import.c
new file mode 100644
index 0000000..83c8ce5
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/dsa/dsa_import.c
@@ -0,0 +1,164 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+   @file dsa_import.c
+   DSA implementation, import a DSA key, Tom St Denis
+*/
+
+#ifdef LTC_MDSA
+
+/**
+   Import a DSA key 
+   @param in       The binary packet to import from
+   @param inlen    The length of the binary packet
+   @param key      [out] Where to store the imported key
+   @return CRYPT_OK if successful, upon error this function will free all allocated memory
+*/
+int dsa_import(const unsigned char *in, unsigned long inlen, dsa_key *key)
+{
+   int           err;
+   unsigned long zero = 0;
+   unsigned char* tmpbuf = NULL;
+   unsigned char flags[1];
+
+   LTC_ARGCHK(in  != NULL);
+   LTC_ARGCHK(key != NULL);
+   LTC_ARGCHK(ltc_mp.name != NULL);
+
+   /* init key */
+   if (mp_init_multi(&key->p, &key->g, &key->q, &key->x, &key->y, NULL) != CRYPT_OK) {
+      return CRYPT_MEM;
+   }
+
+   /* try to match the old libtomcrypt format */
+   if ((err = der_decode_sequence_multi(in, inlen,
+                                  LTC_ASN1_BIT_STRING, 1UL, flags,
+                                  LTC_ASN1_EOL, 0UL, NULL)) == CRYPT_OK) {
+       /* private key */
+       if (flags[0]) {
+           if ((err = der_decode_sequence_multi(in, inlen,
+                                  LTC_ASN1_BIT_STRING,   1UL, flags,
+                                  LTC_ASN1_INTEGER,      1UL, key->g,
+                                  LTC_ASN1_INTEGER,      1UL, key->p,
+                                  LTC_ASN1_INTEGER,      1UL, key->q,
+                                  LTC_ASN1_INTEGER,      1UL, key->y,
+                                  LTC_ASN1_INTEGER,      1UL, key->x,
+                                  LTC_ASN1_EOL,          0UL, NULL)) != CRYPT_OK) {
+               goto LBL_ERR;
+           }
+           key->type = PK_PRIVATE;
+           goto LBL_OK;
+       }
+       /* public key */
+       else {
+           if ((err = der_decode_sequence_multi(in, inlen,
+                                      LTC_ASN1_BIT_STRING,   1UL, flags,
+                                      LTC_ASN1_INTEGER,      1UL, key->g,
+                                      LTC_ASN1_INTEGER,      1UL, key->p,
+                                      LTC_ASN1_INTEGER,      1UL, key->q,
+                                      LTC_ASN1_INTEGER,      1UL, key->y,
+                                      LTC_ASN1_EOL,          0UL, NULL)) != CRYPT_OK) {
+               goto LBL_ERR;
+           }
+           key->type = PK_PUBLIC;
+           goto LBL_OK;
+       }
+   }
+   /* get key type */
+   if ((err = der_decode_sequence_multi(in, inlen,
+                          LTC_ASN1_SHORT_INTEGER, 1UL, &zero,
+                          LTC_ASN1_INTEGER,      1UL, key->p,
+                          LTC_ASN1_INTEGER,      1UL, key->q,
+                          LTC_ASN1_INTEGER,      1UL, key->g,
+                          LTC_ASN1_INTEGER,      1UL, key->y,
+                          LTC_ASN1_INTEGER,      1UL, key->x,
+                          LTC_ASN1_EOL,          0UL, NULL)) == CRYPT_OK) {
+
+       key->type = PK_PRIVATE;
+   } else { /* public */
+      ltc_asn1_list params[3];
+      unsigned long tmpbuf_len = MAX_RSA_SIZE*8;
+
+      LTC_SET_ASN1(params, 0, LTC_ASN1_INTEGER, key->p, 1UL);
+      LTC_SET_ASN1(params, 1, LTC_ASN1_INTEGER, key->q, 1UL);
+      LTC_SET_ASN1(params, 2, LTC_ASN1_INTEGER, key->g, 1UL);
+
+      tmpbuf = XCALLOC(1, tmpbuf_len);
+      if (tmpbuf == NULL) {
+          err = CRYPT_MEM;
+          goto LBL_ERR;
+      }
+
+      err = der_decode_subject_public_key_info(in, inlen,
+        PKA_DSA, tmpbuf, &tmpbuf_len,
+        LTC_ASN1_SEQUENCE, params, 3);
+      if (err != CRYPT_OK) {
+         goto LBL_ERR;
+      }
+
+      if ((err=der_decode_integer(tmpbuf, tmpbuf_len, key->y)) != CRYPT_OK) {
+         goto LBL_ERR;
+      }
+
+      XFREE(tmpbuf);
+      key->type = PK_PUBLIC;
+  }
+
+LBL_OK:
+  key->qord = mp_unsigned_bin_size(key->q);
+
+  if (key->qord >= LTC_MDSA_MAX_GROUP || key->qord <= 15 ||
+      (unsigned long)key->qord >= mp_unsigned_bin_size(key->p) || (mp_unsigned_bin_size(key->p) - key->qord) >= LTC_MDSA_DELTA) {
+      err = CRYPT_INVALID_PACKET;
+      goto LBL_ERR;
+   }
+
+  return CRYPT_OK;
+LBL_ERR:
+   XFREE(tmpbuf);
+   mp_clear_multi(key->p, key->g, key->q, key->x, key->y, NULL);
+   return err;
+}
+
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/pk/dsa/dsa_import.c,v $ */
+/* $Revision: 1.14 $ */
+/* $Date: 2007/05/12 14:32:35 $ */
diff --git a/core/lib/libtomcrypt/src/pk/dsa/dsa_make_key.c b/core/lib/libtomcrypt/src/pk/dsa/dsa_make_key.c
new file mode 100644
index 0000000..1b02db8
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/dsa/dsa_make_key.c
@@ -0,0 +1,295 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+   @file dsa_make_key.c
+   DSA implementation, generate a DSA key, Tom St Denis
+*/
+
+#ifdef LTC_MDSA
+
+/**
+  Create DSA parameters
+  @param prng          An active PRNG state
+  @param wprng         The index of the PRNG desired
+  @param group_size    Size of the multiplicative group (octets)
+  @param modulus_size  Size of the modulus (octets)
+  @param p             [out] bignum where generated 'p' is stored (must be initialized by caller)
+  @param q             [out] bignum where generated 'q' is stored (must be initialized by caller)
+  @param g             [out] bignum where generated 'g' is stored (must be initialized by caller)
+  @return CRYPT_OK if successful, upon error this function will free all allocated memory
+*/
+static int dsa_make_params(prng_state *prng, int wprng, int group_size, int modulus_size, void *p, void *q, void *g)
+{
+  unsigned long L, N, n, outbytes, seedbytes, counter, j, i;
+  int err, res, mr_tests_q, mr_tests_p, found_p, found_q, hash;
+  unsigned char *wbuf, *sbuf, digest[MAXBLOCKSIZE];
+  void *t2L1, *t2N1, *t2q, *t2seedlen, *U, *W, *X, *c, *h, *e, *seedinc;
+
+  /* check size */
+  if (group_size >= LTC_MDSA_MAX_GROUP || group_size < 1 || group_size >= modulus_size) {
+    return CRYPT_INVALID_ARG;
+  }
+
+ /* FIPS-186-4 A.1.1.2 Generation of the Probable Primes p and q Using an Approved Hash Function
+  *
+  * L = The desired length of the prime p (in bits e.g. L = 1024)
+  * N = The desired length of the prime q (in bits e.g. N = 160)
+  * seedlen = The desired bit length of the domain parameter seed; seedlen shallbe equal to or greater than N
+  * outlen  = The bit length of Hash function
+  *
+  * 1.  Check that the (L, N)
+  * 2.  If (seedlen <N), then return INVALID.
+  * 3.  n = ceil(L / outlen) - 1
+  * 4.  b = L- 1 - (n * outlen)
+  * 5.  domain_parameter_seed = an arbitrary sequence of seedlen bits
+  * 6.  U = Hash (domain_parameter_seed) mod 2^(N-1)
+  * 7.  q = 2^(N-1) + U + 1 - (U mod 2)
+  * 8.  Test whether or not q is prime as specified in Appendix C.3
+  * 9.  If qis not a prime, then go to step 5.
+  * 10. offset = 1
+  * 11. For counter = 0 to (4L- 1) do {
+  *       For j=0 to n do {
+  *         Vj = Hash ((domain_parameter_seed+ offset + j) mod 2^seedlen
+  *       }
+  *       W = V0 + (V1 *2^outlen) + ... + (Vn-1 * 2^((n-1) * outlen)) + ((Vn mod 2^b) * 2^(n * outlen))
+  *       X = W + 2^(L-1)           Comment: 0 <= W < 2^(L-1); hence 2^(L-1) <= X < 2^L
+  *       c = X mod 2*q
+  *       p = X - (c - 1)           Comment: p ~ 1 (mod 2*q)
+  *       If (p >= 2^(L-1)) {
+  *         Test whether or not p is prime as specified in Appendix C.3.
+  *         If p is determined to be prime, then return VALID and the values of p, qand (optionally) the values of domain_parameter_seed and counter
+  *       }
+  *       offset = offset + n + 1   Comment: Increment offset
+  *     }
+  */
+
+  seedbytes = group_size;
+  L = modulus_size * 8;
+  N = group_size * 8;
+
+  /* M-R tests (when followed by one Lucas test) according FIPS-186-4 - Appendix C.3 - table C.1 */
+  mr_tests_p = (L <= 2048) ? 3 : 2;
+  if      (N <= 160)  { mr_tests_q = 19; }
+  else if (N <= 224)  { mr_tests_q = 24; }
+  else                { mr_tests_q = 27; }
+
+  if (N <= 256) {
+    hash = register_hash(&sha256_desc);
+  }
+  else if (N <= 384) {
+    hash = register_hash(&sha384_desc);
+  }
+  else if (N <= 512) {
+    hash = register_hash(&sha512_desc);
+  }
+  else {
+    return CRYPT_INVALID_ARG; /* group_size too big */
+  }
+
+  if ((err = hash_is_valid(hash)) != CRYPT_OK)                                   { return err; }
+  outbytes = hash_descriptor[hash]->hashsize;
+
+  n = ((L + outbytes*8 - 1) / (outbytes*8)) - 1;
+
+  if ((wbuf = XMALLOC((n+1)*outbytes)) == NULL)                                  { err = CRYPT_MEM; goto cleanup3; }
+  if ((sbuf = XMALLOC(seedbytes)) == NULL)                                       { err = CRYPT_MEM; goto cleanup2; }
+
+  err = mp_init_multi(&t2L1, &t2N1, &t2q, &t2seedlen, &U, &W, &X, &c, &h, &e, &seedinc, NULL);
+  if (err != CRYPT_OK)                                                           { goto cleanup1; }
+
+  if ((err = mp_2expt(t2L1, L-1)) != CRYPT_OK)                                   { goto cleanup; }
+  /* t2L1 = 2^(L-1) */
+  if ((err = mp_2expt(t2N1, N-1)) != CRYPT_OK)                                   { goto cleanup; }
+  /* t2N1 = 2^(N-1) */
+  if ((err = mp_2expt(t2seedlen, seedbytes*8)) != CRYPT_OK)                      { goto cleanup; }
+  /* t2seedlen = 2^seedlen */
+
+  for(found_p=0; !found_p;) {
+    /* q */
+    for(found_q=0; !found_q;) {
+      if (prng_descriptor[wprng]->read(sbuf, seedbytes, prng) != seedbytes)       { err = CRYPT_ERROR_READPRNG; goto cleanup; }
+      i = outbytes;
+      if ((err = hash_memory(hash, sbuf, seedbytes, digest, &i)) != CRYPT_OK)    { goto cleanup; }
+      if ((err = mp_read_unsigned_bin(U, digest, outbytes)) != CRYPT_OK)         { goto cleanup; }
+      if ((err = mp_mod(U, t2N1, U)) != CRYPT_OK)                                { goto cleanup; }
+      if ((err = mp_add(t2N1, U, q)) != CRYPT_OK)                                { goto cleanup; }
+      if (!mp_isodd(q)) mp_add_d(q, 1, q);
+      if ((err = mp_prime_is_prime(q, mr_tests_q, &res)) != CRYPT_OK)            { goto cleanup; }       /* XXX-TODO rounds are ignored; no Lucas test */
+      if (res == LTC_MP_YES) found_q = 1;
+    }
+
+    /* p */
+    if ((err = mp_read_unsigned_bin(seedinc, sbuf, seedbytes)) != CRYPT_OK)      { goto cleanup; }
+    if ((err = mp_add(q, q, t2q)) != CRYPT_OK)                                   { goto cleanup; }
+    for(counter=0; counter < 4*L && !found_p; counter++) {
+      for(j=0; j<=n; j++) {
+        if ((err = mp_add_d(seedinc, 1, seedinc)) != CRYPT_OK)                   { goto cleanup; }
+        if ((err = mp_mod(seedinc, t2seedlen, seedinc)) != CRYPT_OK)             { goto cleanup; }
+        /* seedinc = (seedinc+1) % 2^seed_bitlen */
+        if ((i = mp_unsigned_bin_size(seedinc)) > seedbytes)                     { err = CRYPT_INVALID_ARG; goto cleanup; }
+        zeromem(sbuf, seedbytes);
+        if ((err = mp_to_unsigned_bin(seedinc, sbuf + seedbytes-i)) != CRYPT_OK) { goto cleanup; }
+        i = outbytes;
+        err = hash_memory(hash, sbuf, seedbytes, wbuf+(n-j)*outbytes, &i);
+        if (err != CRYPT_OK)                                                     { goto cleanup; }
+      }
+      if ((err = mp_read_unsigned_bin(W, wbuf, (n+1)*outbytes)) != CRYPT_OK)     { goto cleanup; }
+      if ((err = mp_mod(W, t2L1, W)) != CRYPT_OK)                                { goto cleanup; }
+      if ((err = mp_add(W, t2L1, X)) != CRYPT_OK)                                { goto cleanup; }
+      if ((err = mp_mod(X, t2q, c))  != CRYPT_OK)                                { goto cleanup; }
+      if ((err = mp_sub_d(c, 1, p))  != CRYPT_OK)                                { goto cleanup; }
+      if ((err = mp_sub(X, p, p))    != CRYPT_OK)                                { goto cleanup; }
+      if (mp_cmp(p, t2L1) != LTC_MP_LT) {
+        /* p >= 2^(L-1) */
+        if ((err = mp_prime_is_prime(p, mr_tests_p, &res)) != CRYPT_OK)          { goto cleanup; }       /* XXX-TODO rounds are ignored; no Lucas test */
+        if (res == LTC_MP_YES) {
+          found_p = 1;
+        }
+      }
+    }
+  }
+
+ /* FIPS-186-4 A.2.1 Unverifiable Generation of the Generator g
+  * 1. e = (p - 1)/q
+  * 2. h = any integer satisfying: 1 < h < (p - 1)
+  *    h could be obtained from a random number generator or from a counter that changes after each use
+  * 3. g = h^e mod p
+  * 4. if (g == 1), then go to step 2.
+  *
+  */
+
+  if ((err = mp_sub_d(p, 1, e)) != CRYPT_OK)                                     { goto cleanup; }
+  if ((err = mp_div(e, q, e, c)) != CRYPT_OK)                                    { goto cleanup; }
+  /* e = (p - 1)/q */
+  i = mp_count_bits(p);
+  do {
+    do {
+      if ((err = rand_bn_bits(h, i, prng, wprng)) != CRYPT_OK)                   { goto cleanup; }
+    } while (mp_cmp(h, p) != LTC_MP_LT || mp_cmp_d(h, 2) != LTC_MP_GT);
+    if ((err = mp_sub_d(h, 1, h)) != CRYPT_OK)                                   { goto cleanup; }
+    /* h is randon and 1 < h < (p-1) */
+    if ((err = mp_exptmod(h, e, p, g)) != CRYPT_OK)                              { goto cleanup; }
+  } while (mp_cmp_d(g, 1) == LTC_MP_EQ);
+
+  err = CRYPT_OK;
+cleanup:
+  mp_clear_multi(t2L1, t2N1, t2q, t2seedlen, U, W, X, c, h, e, seedinc, NULL);
+cleanup1:
+  XFREE(sbuf);
+cleanup2:
+  XFREE(wbuf);
+cleanup3:
+  return err;
+}
+
+/**
+  Create a DSA key (with given params)
+  @param prng          An active PRNG state
+  @param wprng         The index of the PRNG desired
+  @param group_size    Size of the multiplicative group (octets)
+  @param modulus_size  Size of the modulus (octets)
+  @param key           [out] Where to store the created key
+  @param p_hex         Hexadecimal string 'p'
+  @param q_hex         Hexadecimal string 'q'
+  @param g_hex         Hexadecimal string 'g'
+  @return CRYPT_OK if successful, upon error this function will free all allocated memory
+*/
+static int dsa_make_key_ex(prng_state *prng, int wprng, int group_size, int modulus_size, dsa_key *key, char* p_hex, char* q_hex, char* g_hex)
+{
+  int err, qbits;
+
+  LTC_ARGCHK(key  != NULL);
+
+  /* init mp_ints */
+  if ((err = mp_init_multi(&key->g, &key->q, &key->p, &key->x, &key->y, NULL)) != CRYPT_OK) {
+    return err;
+  }
+
+  if (p_hex == NULL || q_hex == NULL || g_hex == NULL) {
+    /* generate params */
+    err = dsa_make_params(prng, wprng, group_size, modulus_size, key->p, key->q, key->g);
+    if (err != CRYPT_OK)                                                         { goto cleanup; }
+  }
+  else {
+    /* read params */
+    if ((err = mp_read_radix(key->p, p_hex, 16)) != CRYPT_OK)                    { goto cleanup; }
+    if ((err = mp_read_radix(key->q, q_hex, 16)) != CRYPT_OK)                    { goto cleanup; }
+    if ((err = mp_read_radix(key->g, g_hex, 16)) != CRYPT_OK)                    { goto cleanup; }
+    /* XXX-TODO maybe do some validity check for p, q, g */
+  }
+
+  /* so now we have our DH structure, generator g, order q, modulus p
+     Now we need a random exponent [mod q] and it's power g^x mod p
+   */
+  qbits = mp_count_bits(key->q);
+  do {
+     if ((err = rand_bn_bits(key->x, qbits, prng, wprng)) != CRYPT_OK)                  { goto cleanup; }
+     /* private key x should be from range: 1 <= x <= q-1 (see FIPS 186-4 B.1.2) */
+  } while (mp_cmp_d(key->x, 0) != LTC_MP_GT || mp_cmp(key->x, key->q) != LTC_MP_LT);
+  if ((err = mp_exptmod(key->g, key->x, key->p, key->y)) != CRYPT_OK)                   { goto cleanup; }
+  key->type = PK_PRIVATE;
+  key->qord = group_size;
+
+  return CRYPT_OK;
+
+cleanup:
+  mp_clear_multi(key->g, key->q, key->p, key->x, key->y, NULL);
+  return err;
+}
+
+/**
+  Create a DSA key
+  @param prng          An active PRNG state
+  @param wprng         The index of the PRNG desired
+  @param group_size    Size of the multiplicative group (octets)
+  @param modulus_size  Size of the modulus (octets)
+  @param key           [out] Where to store the created key
+  @return CRYPT_OK if successful, upon error this function will free all allocated memory
+*/
+int dsa_make_key(prng_state *prng, int wprng, int group_size, int modulus_size, dsa_key *key)
+{
+  return dsa_make_key_ex(prng, wprng, group_size, modulus_size, key, NULL, NULL, NULL);
+}
+
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/pk/dsa/dsa_make_key.c,v $ */
+/* $Revision: 1.12 $ */
+/* $Date: 2007/05/12 14:32:35 $ */
diff --git a/core/lib/libtomcrypt/src/pk/dsa/dsa_shared_secret.c b/core/lib/libtomcrypt/src/pk/dsa/dsa_shared_secret.c
new file mode 100644
index 0000000..449af93
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/dsa/dsa_shared_secret.c
@@ -0,0 +1,98 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file dsa_shared_secret.c
+  DSA Crypto, Tom St Denis
+*/  
+
+#ifdef LTC_MDSA
+
+/**
+  Create a DSA shared secret between two keys
+  @param private_key      The private DSA key (the exponent)
+  @param base             The base of the exponentiation (allows this to be used for both encrypt and decrypt) 
+  @param public_key       The public key
+  @param out              [out] Destination of the shared secret
+  @param outlen           [in/out] The max size and resulting size of the shared secret
+  @return CRYPT_OK if successful
+*/
+int dsa_shared_secret(void          *private_key, void *base,
+                      dsa_key       *public_key,
+                      unsigned char *out,         unsigned long *outlen)
+{
+   unsigned long  x;
+   void          *res;
+   int            err;
+
+   LTC_ARGCHK(private_key != NULL);
+   LTC_ARGCHK(public_key  != NULL);
+   LTC_ARGCHK(out         != NULL);
+   LTC_ARGCHK(outlen      != NULL);
+
+   /* make new point */
+   if ((err = mp_init(&res)) != CRYPT_OK) {
+      return err;
+   }
+
+   if ((err = mp_exptmod(base, private_key, public_key->p, res)) != CRYPT_OK) {
+      mp_clear(res);
+      return err;
+   }
+   
+   x = (unsigned long)mp_unsigned_bin_size(res);
+   if (*outlen < x) {
+      *outlen = x;
+      err = CRYPT_BUFFER_OVERFLOW;
+      goto done;
+   }
+   zeromem(out, x);
+   if ((err = mp_to_unsigned_bin(res, out + (x - mp_unsigned_bin_size(res))))   != CRYPT_OK)          { goto done; }
+
+   err     = CRYPT_OK;
+   *outlen = x;
+done:
+   mp_clear(res);
+   return err;
+}
+
+#endif
+/* $Source: /cvs/libtom/libtomcrypt/src/pk/dsa/dsa_shared_secret.c,v $ */
+/* $Revision: 1.9 $ */
+/* $Date: 2007/05/12 14:32:35 $ */
diff --git a/core/lib/libtomcrypt/src/pk/dsa/dsa_sign_hash.c b/core/lib/libtomcrypt/src/pk/dsa/dsa_sign_hash.c
new file mode 100644
index 0000000..0c89967
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/dsa/dsa_sign_hash.c
@@ -0,0 +1,178 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+   @file dsa_sign_hash.c
+   DSA implementation, sign a hash, Tom St Denis
+*/
+
+#ifdef LTC_MDSA
+
+/**
+  Sign a hash with DSA
+  @param in       The hash to sign
+  @param inlen    The length of the hash to sign
+  @param r        The "r" integer of the signature (caller must initialize with mp_init() first)
+  @param s        The "s" integer of the signature (caller must initialize with mp_init() first)
+  @param prng     An active PRNG state
+  @param wprng    The index of the PRNG desired
+  @param key      A private DSA key
+  @return CRYPT_OK if successful
+*/
+int dsa_sign_hash_raw(const unsigned char *in,  unsigned long inlen,
+                                   void   *r,   void *s,
+                               prng_state *prng, int wprng, dsa_key *key)
+{
+   void         *k, *kinv, *tmp;
+   unsigned char *buf;
+   int            err, qbits;
+
+   LTC_ARGCHK(in  != NULL);
+   LTC_ARGCHK(r   != NULL);
+   LTC_ARGCHK(s   != NULL);
+   LTC_ARGCHK(key != NULL);
+
+   if ((err = prng_is_valid(wprng)) != CRYPT_OK) {
+      return err;
+   }
+   if (key->type != PK_PRIVATE) {
+      return CRYPT_PK_NOT_PRIVATE;
+   }
+
+   /* check group order size  */
+   if (key->qord >= LTC_MDSA_MAX_GROUP) {
+      return CRYPT_INVALID_ARG;
+   }
+
+   buf = XMALLOC(LTC_MDSA_MAX_GROUP);
+   if (buf == NULL) {
+      return CRYPT_MEM;
+   }
+
+   /* Init our temps */
+   if ((err = mp_init_multi(&k, &kinv, &tmp, NULL)) != CRYPT_OK)                       { goto ERRBUF; }
+
+   qbits = mp_count_bits(key->q);
+retry:
+
+   do {
+      /* gen random k */
+      if ((err = rand_bn_bits(k, qbits, prng, wprng)) != CRYPT_OK)                     { goto error; }
+
+      /* k should be from range: 1 <= k <= q-1 (see FIPS 186-4 B.2.2) */
+      if (mp_cmp_d(k, 0) != LTC_MP_GT || mp_cmp(k, key->q) != LTC_MP_LT)               { goto retry; }
+
+      /* test gcd */
+      if ((err = mp_gcd(k, key->q, tmp)) != CRYPT_OK)                                  { goto error; }
+   } while (mp_cmp_d(tmp, 1) != LTC_MP_EQ);
+
+   /* now find 1/k mod q */
+   if ((err = mp_invmod(k, key->q, kinv)) != CRYPT_OK)                                 { goto error; }
+
+   /* now find r = g^k mod p mod q */
+   if ((err = mp_exptmod(key->g, k, key->p, r)) != CRYPT_OK)                           { goto error; }
+   if ((err = mp_mod(r, key->q, r)) != CRYPT_OK)                                       { goto error; }
+
+   if (mp_iszero(r) == LTC_MP_YES)                                                     { goto retry; }
+
+   /* now find s = (in + xr)/k mod q */
+   if ((err = mp_read_unsigned_bin(tmp, (unsigned char *)in, inlen)) != CRYPT_OK)      { goto error; }
+   if ((err = mp_mul(key->x, r, s)) != CRYPT_OK)                                       { goto error; }
+   if ((err = mp_add(s, tmp, s)) != CRYPT_OK)                                          { goto error; }
+   if ((err = mp_mulmod(s, kinv, key->q, s)) != CRYPT_OK)                              { goto error; }
+
+   if (mp_iszero(s) == LTC_MP_YES)                                                     { goto retry; }
+
+   err = CRYPT_OK;
+error: 
+   mp_clear_multi(k, kinv, tmp, NULL);
+ERRBUF:
+#ifdef LTC_CLEAN_STACK
+   zeromem(buf, LTC_MDSA_MAX_GROUP);
+#endif
+   XFREE(buf);
+   return err;
+}
+
+/**
+  Sign a hash with DSA
+  @param in       The hash to sign
+  @param inlen    The length of the hash to sign
+  @param out      [out] Where to store the signature
+  @param outlen   [in/out] The max size and resulting size of the signature
+  @param prng     An active PRNG state
+  @param wprng    The index of the PRNG desired
+  @param key      A private DSA key
+  @return CRYPT_OK if successful
+*/
+int dsa_sign_hash(const unsigned char *in,  unsigned long inlen,
+                        unsigned char *out, unsigned long *outlen,
+                        prng_state *prng, int wprng, dsa_key *key)
+{
+   void         *r, *s;
+   int           err;
+
+   LTC_ARGCHK(in      != NULL);
+   LTC_ARGCHK(out     != NULL);
+   LTC_ARGCHK(outlen  != NULL);
+   LTC_ARGCHK(key     != NULL);
+
+   if (mp_init_multi(&r, &s, NULL) != CRYPT_OK) {
+      return CRYPT_MEM;
+   }
+
+   if ((err = dsa_sign_hash_raw(in, inlen, r, s, prng, wprng, key)) != CRYPT_OK) {
+      goto error;
+   }
+
+   err = der_encode_sequence_multi(out, outlen, 
+                             LTC_ASN1_INTEGER, 1UL, r, 
+                             LTC_ASN1_INTEGER, 1UL, s, 
+                             LTC_ASN1_EOL,     0UL, NULL);
+
+error:
+   mp_clear_multi(r, s, NULL);
+   return err;
+}
+
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/pk/dsa/dsa_sign_hash.c,v $ */
+/* $Revision: 1.14 $ */
+/* $Date: 2007/05/12 14:32:35 $ */
diff --git a/core/lib/libtomcrypt/src/pk/dsa/dsa_verify_hash.c b/core/lib/libtomcrypt/src/pk/dsa/dsa_verify_hash.c
new file mode 100644
index 0000000..50ccaa7
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/dsa/dsa_verify_hash.c
@@ -0,0 +1,154 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+   @file dsa_verify_hash.c
+   DSA implementation, verify a signature, Tom St Denis
+*/
+
+
+#ifdef LTC_MDSA
+/**
+  Verify a DSA signature
+  @param r        DSA "r" parameter
+  @param s        DSA "s" parameter
+  @param hash     The hash that was signed
+  @param hashlen  The length of the hash that was signed
+  @param stat     [out] The result of the signature verification, 1==valid, 0==invalid
+  @param key      The corresponding public DH key
+  @return CRYPT_OK if successful (even if the signature is invalid)
+*/
+int dsa_verify_hash_raw(         void   *r,          void   *s,
+                    const unsigned char *hash, unsigned long hashlen, 
+                                    int *stat,      dsa_key *key)
+{
+   void          *w, *v, *u1, *u2;
+   int           err;
+
+   LTC_ARGCHK(r    != NULL);
+   LTC_ARGCHK(s    != NULL);
+   LTC_ARGCHK(hash != NULL);
+   LTC_ARGCHK(stat != NULL);
+   LTC_ARGCHK(key  != NULL);
+
+   /* default to invalid signature */
+   *stat = 0;
+
+   /* init our variables */
+   if ((err = mp_init_multi(&w, &v, &u1, &u2, NULL)) != CRYPT_OK) {
+      return err;
+   }
+
+   /* neither r or s can be null or >q*/
+   if (mp_iszero(r) == LTC_MP_YES || mp_iszero(s) == LTC_MP_YES || mp_cmp(r, key->q) != LTC_MP_LT || mp_cmp(s, key->q) != LTC_MP_LT) {
+      err = CRYPT_INVALID_PACKET;
+      goto error;
+   }
+   
+   /* w = 1/s mod q */
+   if ((err = mp_invmod(s, key->q, w)) != CRYPT_OK)                                       { goto error; }
+
+   /* u1 = m * w mod q */
+   if ((err = mp_read_unsigned_bin(u1, (unsigned char *)hash, hashlen)) != CRYPT_OK)      { goto error; }
+
+   if ((err = mp_mulmod(u1, w, key->q, u1)) != CRYPT_OK)                                  { goto error; }
+
+   /* u2 = r*w mod q */
+   if ((err = mp_mulmod(r, w, key->q, u2)) != CRYPT_OK)                                   { goto error; } 
+
+   /* v = g^u1 * y^u2 mod p mod q */
+   if ((err = mp_exptmod(key->g, u1, key->p, u1)) != CRYPT_OK)                            { goto error; }
+   if ((err = mp_exptmod(key->y, u2, key->p, u2)) != CRYPT_OK)                            { goto error; }
+   if ((err = mp_mulmod(u1, u2, key->p, v)) != CRYPT_OK)                                  { goto error; }
+   if ((err = mp_mod(v, key->q, v)) != CRYPT_OK)                                          { goto error; }
+
+   /* if r = v then we're set */
+   if (mp_cmp(r, v) == LTC_MP_EQ) {
+      *stat = 1;
+   }
+
+   err = CRYPT_OK;
+error:
+   mp_clear_multi(w, v, u1, u2, NULL);
+   return err;
+}
+
+/**
+  Verify a DSA signature
+  @param sig      The signature
+  @param siglen   The length of the signature (octets)
+  @param hash     The hash that was signed
+  @param hashlen  The length of the hash that was signed
+  @param stat     [out] The result of the signature verification, 1==valid, 0==invalid
+  @param key      The corresponding public DH key
+  @return CRYPT_OK if successful (even if the signature is invalid)
+*/
+int dsa_verify_hash(const unsigned char *sig, unsigned long siglen,
+                    const unsigned char *hash, unsigned long hashlen, 
+                    int *stat, dsa_key *key)
+{
+   int    err;
+   void   *r, *s;
+
+   if ((err = mp_init_multi(&r, &s, NULL)) != CRYPT_OK) {
+      return CRYPT_MEM;
+   }
+
+   /* decode the sequence */
+   if ((err = der_decode_sequence_multi(sig, siglen,
+                                  LTC_ASN1_INTEGER, 1UL, r, 
+                                  LTC_ASN1_INTEGER, 1UL, s, 
+                                  LTC_ASN1_EOL,     0UL, NULL)) != CRYPT_OK) {
+      goto LBL_ERR;
+   }
+
+   /* do the op */
+   err = dsa_verify_hash_raw(r, s, hash, hashlen, stat, key);
+
+LBL_ERR:
+   mp_clear_multi(r, s, NULL);
+   return err;
+}
+
+#endif
+
+
+/* $Source: /cvs/libtom/libtomcrypt/src/pk/dsa/dsa_verify_hash.c,v $ */
+/* $Revision: 1.15 $ */
+/* $Date: 2007/05/12 14:32:35 $ */
diff --git a/core/lib/libtomcrypt/src/pk/dsa/dsa_verify_key.c b/core/lib/libtomcrypt/src/pk/dsa/dsa_verify_key.c
new file mode 100644
index 0000000..c413a4e
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/dsa/dsa_verify_key.c
@@ -0,0 +1,127 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+   @file dsa_verify_key.c
+   DSA implementation, verify a key, Tom St Denis
+*/
+
+#ifdef LTC_MDSA
+
+/**
+   Verify a DSA key for validity
+   @param key   The key to verify
+   @param stat  [out]  Result of test, 1==valid, 0==invalid
+   @return CRYPT_OK if successful
+*/
+int dsa_verify_key(dsa_key *key, int *stat)
+{
+   void   *tmp, *tmp2;
+   int    res, err;
+
+   LTC_ARGCHK(key  != NULL);
+   LTC_ARGCHK(stat != NULL);
+
+   /* default to an invalid key */
+   *stat = 0;
+
+   /* first make sure key->q and key->p are prime */
+   if ((err = mp_prime_is_prime(key->q, 8, &res)) != CRYPT_OK) {
+      return err;
+   }
+   if (res == 0) {
+      return CRYPT_OK;
+   }
+
+   if ((err = mp_prime_is_prime(key->p, 8, &res)) != CRYPT_OK) {
+      return err;
+   }
+   if (res == 0) {
+      return CRYPT_OK;
+   }
+
+   /* now make sure that g is not -1, 0 or 1 and <p */
+   if (mp_cmp_d(key->g, 0) == LTC_MP_EQ || mp_cmp_d(key->g, 1) == LTC_MP_EQ) {
+      return CRYPT_OK;
+   }
+   if ((err = mp_init_multi(&tmp, &tmp2, NULL)) != CRYPT_OK)               { return err; }
+   if ((err = mp_sub_d(key->p, 1, tmp)) != CRYPT_OK)                       { goto error; }
+   if (mp_cmp(tmp, key->g) == LTC_MP_EQ || mp_cmp(key->g, key->p) != LTC_MP_LT) {
+      err = CRYPT_OK;
+      goto error;
+   }
+
+   /* 1 < y < p-1 */
+   if (!(mp_cmp_d(key->y, 1) == LTC_MP_GT && mp_cmp(key->y, tmp) == LTC_MP_LT)) {
+      err = CRYPT_OK;
+      goto error;
+   }
+
+   /* now we have to make sure that g^q = 1, and that p-1/q gives 0 remainder */
+   if ((err = mp_div(tmp, key->q, tmp, tmp2)) != CRYPT_OK)             { goto error; }
+   if (mp_iszero(tmp2) != LTC_MP_YES) {
+      err = CRYPT_OK;
+      goto error;
+   }
+
+   if ((err = mp_exptmod(key->g, key->q, key->p, tmp)) != CRYPT_OK)    { goto error; }
+   if (mp_cmp_d(tmp, 1) != LTC_MP_EQ) {
+      err = CRYPT_OK;
+      goto error;
+   }
+
+   /* now we have to make sure that y^q = 1, this makes sure y \in g^x mod p */
+   if ((err = mp_exptmod(key->y, key->q, key->p, tmp)) != CRYPT_OK)       { goto error; }
+   if (mp_cmp_d(tmp, 1) != LTC_MP_EQ) {
+      err = CRYPT_OK;
+      goto error;
+   }
+
+   /* at this point we are out of tests ;-( */
+   err   = CRYPT_OK;
+   *stat = 1;
+error: 
+   mp_clear_multi(tmp, tmp2, NULL);
+   return err;
+}
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/pk/dsa/dsa_verify_key.c,v $ */
+/* $Revision: 1.8 $ */
+/* $Date: 2007/05/12 14:32:35 $ */
diff --git a/core/lib/libtomcrypt/src/pk/dsa/sub.mk b/core/lib/libtomcrypt/src/pk/dsa/sub.mk
new file mode 100644
index 0000000..bd1d2df
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/dsa/sub.mk
@@ -0,0 +1,10 @@
+srcs-y += dsa_decrypt_key.c
+srcs-y += dsa_encrypt_key.c
+srcs-y += dsa_export.c
+srcs-y += dsa_free.c
+srcs-y += dsa_import.c
+srcs-y += dsa_make_key.c
+srcs-y += dsa_shared_secret.c
+srcs-y += dsa_sign_hash.c
+srcs-y += dsa_verify_hash.c
+srcs-y += dsa_verify_key.c
diff --git a/core/lib/libtomcrypt/src/pk/ecc/ecc.c b/core/lib/libtomcrypt/src/pk/ecc/ecc.c
new file mode 100644
index 0000000..5b321b7
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/ecc/ecc.c
@@ -0,0 +1,153 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+
+/* Implements ECC over Z/pZ for curve y^2 = x^3 - 3x + b
+ *
+ * All curves taken from NIST recommendation paper of July 1999
+ * Available at http://csrc.nist.gov/cryptval/dss.htm
+ */
+#include "tomcrypt.h"
+
+/**
+  @file ecc.c
+  ECC Crypto, Tom St Denis
+*/
+
+#ifdef LTC_MECC
+
+/* This holds the key settings.  ***MUST*** be organized by size from smallest to largest. */
+const ltc_ecc_set_type ltc_ecc_sets[] = {
+#ifdef LTC_ECC112
+{
+        14,
+        "SECP112R1",
+        "DB7C2ABF62E35E668076BEAD208B",
+        "659EF8BA043916EEDE8911702B22",
+        "DB7C2ABF62E35E7628DFAC6561C5",
+        "09487239995A5EE76B55F9C2F098",
+        "A89CE5AF8724C0A23E0E0FF77500"
+},
+#endif
+#ifdef LTC_ECC128
+{
+        16,
+        "SECP128R1",
+        "FFFFFFFDFFFFFFFFFFFFFFFFFFFFFFFF",
+        "E87579C11079F43DD824993C2CEE5ED3",
+        "FFFFFFFE0000000075A30D1B9038A115",
+        "161FF7528B899B2D0C28607CA52C5B86",
+        "CF5AC8395BAFEB13C02DA292DDED7A83",
+},
+#endif
+#ifdef LTC_ECC160
+{
+        20,
+        "SECP160R1",
+        "FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF7FFFFFFF",
+        "1C97BEFC54BD7A8B65ACF89F81D4D4ADC565FA45",
+        "0100000000000000000001F4C8F927AED3CA752257",
+        "4A96B5688EF573284664698968C38BB913CBFC82",
+        "23A628553168947D59DCC912042351377AC5FB32",
+},
+#endif
+#ifdef LTC_ECC192
+{
+        24,
+        "ECC-192",
+        "FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFEFFFFFFFFFFFFFFFF",
+        "64210519E59C80E70FA7E9AB72243049FEB8DEECC146B9B1",
+        "FFFFFFFFFFFFFFFFFFFFFFFF99DEF836146BC9B1B4D22831",
+        "188DA80EB03090F67CBF20EB43A18800F4FF0AFD82FF1012",
+        "7192B95FFC8DA78631011ED6B24CDD573F977A11E794811",
+},
+#endif
+#ifdef LTC_ECC224
+{
+        28,
+        "ECC-224",
+        "FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF000000000000000000000001",
+        "B4050A850C04B3ABF54132565044B0B7D7BFD8BA270B39432355FFB4",
+        "FFFFFFFFFFFFFFFFFFFFFFFFFFFF16A2E0B8F03E13DD29455C5C2A3D",
+        "B70E0CBD6BB4BF7F321390B94A03C1D356C21122343280D6115C1D21",
+        "BD376388B5F723FB4C22DFE6CD4375A05A07476444D5819985007E34",
+},
+#endif
+#ifdef LTC_ECC256
+{
+        32,
+        "ECC-256",
+        "FFFFFFFF00000001000000000000000000000000FFFFFFFFFFFFFFFFFFFFFFFF",
+        "5AC635D8AA3A93E7B3EBBD55769886BC651D06B0CC53B0F63BCE3C3E27D2604B",
+        "FFFFFFFF00000000FFFFFFFFFFFFFFFFBCE6FAADA7179E84F3B9CAC2FC632551",
+        "6B17D1F2E12C4247F8BCE6E563A440F277037D812DEB33A0F4A13945D898C296",
+        "4FE342E2FE1A7F9B8EE7EB4A7C0F9E162BCE33576B315ECECBB6406837BF51F5",
+},
+#endif
+#ifdef LTC_ECC384
+{
+        48,
+        "ECC-384",
+        "FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFEFFFFFFFF0000000000000000FFFFFFFF",
+        "B3312FA7E23EE7E4988E056BE3F82D19181D9C6EFE8141120314088F5013875AC656398D8A2ED19D2A85C8EDD3EC2AEF",
+        "FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFC7634D81F4372DDF581A0DB248B0A77AECEC196ACCC52973",
+        "AA87CA22BE8B05378EB1C71EF320AD746E1D3B628BA79B9859F741E082542A385502F25DBF55296C3A545E3872760AB7",
+        "3617DE4A96262C6F5D9E98BF9292DC29F8F41DBD289A147CE9DA3113B5F0B8C00A60B1CE1D7E819D7A431D7C90EA0E5F",
+},
+#endif
+#ifdef LTC_ECC521
+{
+        66,
+        "ECC-521",
+        "1FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF",
+        "51953EB9618E1C9A1F929A21A0B68540EEA2DA725B99B315F3B8B489918EF109E156193951EC7E937B1652C0BD3BB1BF073573DF883D2C34F1EF451FD46B503F00",
+        "1FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFA51868783BF2F966B7FCC0148F709A5D03BB5C9B8899C47AEBB6FB71E91386409",
+        "C6858E06B70404E9CD9E3ECB662395B4429C648139053FB521F828AF606B4D3DBAA14B5E77EFE75928FE1DC127A2FFA8DE3348B3C1856A429BF97E7E31C2E5BD66",
+        "11839296A789A3BC0045C8A5FB42C7D1BD998F54449579B446817AFBD17273E662C97EE72995EF42640C550B9013FAD0761353C7086A272C24088BE94769FD16650",
+},
+#endif
+{
+   0,
+   NULL, NULL, NULL, NULL, NULL, NULL
+}
+};
+
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/pk/ecc/ecc.c,v $ */
+/* $Revision: 1.40 $ */
+/* $Date: 2007/05/12 14:32:35 $ */
diff --git a/core/lib/libtomcrypt/src/pk/ecc/ecc_ansi_x963_export.c b/core/lib/libtomcrypt/src/pk/ecc/ecc_ansi_x963_export.c
new file mode 100644
index 0000000..b35b041
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/ecc/ecc_ansi_x963_export.c
@@ -0,0 +1,106 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+
+/* Implements ECC over Z/pZ for curve y^2 = x^3 - 3x + b
+ *
+ * All curves taken from NIST recommendation paper of July 1999
+ * Available at http://csrc.nist.gov/cryptval/dss.htm
+ */
+#include "tomcrypt.h"
+
+/**
+  @file ecc_ansi_x963_export.c
+  ECC Crypto, Tom St Denis
+*/  
+
+#ifdef LTC_MECC
+
+/** ECC X9.63 (Sec. 4.3.6) uncompressed export
+  @param key     Key to export
+  @param out     [out] destination of export
+  @param outlen  [in/out]  Length of destination and final output size
+  Return CRYPT_OK on success
+*/
+int ecc_ansi_x963_export(ecc_key *key, unsigned char *out, unsigned long *outlen)
+{
+   unsigned char buf[ECC_BUF_SIZE];
+   unsigned long numlen, xlen, ylen;
+
+   LTC_ARGCHK(key    != NULL);
+   LTC_ARGCHK(outlen != NULL);
+
+   if (ltc_ecc_is_valid_idx(key->idx) == 0) {
+      return CRYPT_INVALID_ARG;
+   }
+   numlen = key->dp->size;
+   xlen = mp_unsigned_bin_size(key->pubkey.x);
+   ylen = mp_unsigned_bin_size(key->pubkey.y);
+
+   if (xlen > numlen || ylen > numlen || sizeof(buf) < numlen) {
+      return CRYPT_BUFFER_OVERFLOW;
+   }
+
+   if (*outlen < (1 + 2*numlen)) {
+      *outlen = 1 + 2*numlen;
+      return CRYPT_BUFFER_OVERFLOW;
+   }
+
+   LTC_ARGCHK(out    != NULL);
+
+   /* store byte 0x04 */
+   out[0] = 0x04;
+
+   /* pad and store x */
+   zeromem(buf, sizeof(buf));
+   mp_to_unsigned_bin(key->pubkey.x, buf + (numlen - xlen));
+   XMEMCPY(out+1, buf, numlen);
+
+   /* pad and store y */
+   zeromem(buf, sizeof(buf));
+   mp_to_unsigned_bin(key->pubkey.y, buf + (numlen - ylen));
+   XMEMCPY(out+1+numlen, buf, numlen);
+
+   *outlen = 1 + 2*numlen;
+   return CRYPT_OK;
+}
+
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/pk/ecc/ecc_ansi_x963_export.c,v $ */
+/* $Revision: 1.6 $ */
+/* $Date: 2007/05/12 14:32:35 $ */
diff --git a/core/lib/libtomcrypt/src/pk/ecc/ecc_ansi_x963_import.c b/core/lib/libtomcrypt/src/pk/ecc/ecc_ansi_x963_import.c
new file mode 100644
index 0000000..cafbd07
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/ecc/ecc_ansi_x963_import.c
@@ -0,0 +1,131 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+
+/* Implements ECC over Z/pZ for curve y^2 = x^3 - 3x + b
+ *
+ * All curves taken from NIST recommendation paper of July 1999
+ * Available at http://csrc.nist.gov/cryptval/dss.htm
+ */
+#include "tomcrypt.h"
+
+/**
+  @file ecc_ansi_x963_import.c
+  ECC Crypto, Tom St Denis
+*/  
+
+#ifdef LTC_MECC
+
+/** Import an ANSI X9.63 format public key 
+  @param in      The input data to read
+  @param inlen   The length of the input data
+  @param key     [out] destination to store imported key \
+*/
+int ecc_ansi_x963_import(const unsigned char *in, unsigned long inlen, ecc_key *key)
+{
+   return ecc_ansi_x963_import_ex(in, inlen, key, NULL);
+}
+
+int ecc_ansi_x963_import_ex(const unsigned char *in, unsigned long inlen, ecc_key *key, ltc_ecc_set_type *dp)
+{
+   int x, err;
+ 
+   LTC_ARGCHK(in  != NULL);
+   LTC_ARGCHK(key != NULL);
+   
+   /* must be odd */
+   if ((inlen & 1) == 0) {
+      return CRYPT_INVALID_ARG;
+   }
+
+   /* init key */
+   if (mp_init_multi(&key->pubkey.x, &key->pubkey.y, &key->pubkey.z, &key->k, NULL) != CRYPT_OK) {
+      return CRYPT_MEM;
+   }
+
+   /* check for 4, 6 or 7 */
+   if (in[0] != 4 && in[0] != 6 && in[0] != 7) {
+      err = CRYPT_INVALID_PACKET;
+      goto error;
+   }
+
+   /* read data */
+   if ((err = mp_read_unsigned_bin(key->pubkey.x, (unsigned char *)in+1, (inlen-1)>>1)) != CRYPT_OK) {
+      goto error;
+   }
+
+   if ((err = mp_read_unsigned_bin(key->pubkey.y, (unsigned char *)in+1+((inlen-1)>>1), (inlen-1)>>1)) != CRYPT_OK) {
+      goto error;
+   }
+   if ((err = mp_set(key->pubkey.z, 1)) != CRYPT_OK) { goto error; }
+
+   if (dp == NULL) {
+     /* determine the idx */
+      for (x = 0; ltc_ecc_sets[x].size != 0; x++) {
+         if ((unsigned)ltc_ecc_sets[x].size >= ((inlen-1)>>1)) {
+            break;
+         }
+      }
+      if (ltc_ecc_sets[x].size == 0) {
+         err = CRYPT_INVALID_PACKET;
+         goto error;
+      }
+      /* set the idx */
+      key->idx  = x;
+      key->dp = &ltc_ecc_sets[x];
+   } else {
+      if (((inlen-1)>>1) != (unsigned long) dp->size) {
+         err = CRYPT_INVALID_PACKET;
+         goto error;
+      }
+      key->idx = -1;
+      key->dp  = dp;
+   }
+   key->type = PK_PUBLIC;
+
+   /* we're done */
+   return CRYPT_OK;
+error:
+   mp_clear_multi(key->pubkey.x, key->pubkey.y, key->pubkey.z, key->k, NULL);
+   return err;
+}
+
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/pk/ecc/ecc_ansi_x963_import.c,v $ */
+/* $Revision: 1.11 $ */
+/* $Date: 2007/05/12 14:32:35 $ */
diff --git a/core/lib/libtomcrypt/src/pk/ecc/ecc_decrypt_key.c b/core/lib/libtomcrypt/src/pk/ecc/ecc_decrypt_key.c
new file mode 100644
index 0000000..b928dce
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/ecc/ecc_decrypt_key.c
@@ -0,0 +1,176 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+
+/* Implements ECC over Z/pZ for curve y^2 = x^3 - 3x + b
+ *
+ * All curves taken from NIST recommendation paper of July 1999
+ * Available at http://csrc.nist.gov/cryptval/dss.htm
+ */
+#include "tomcrypt.h"
+
+/**
+  @file ecc_decrypt_key.c
+  ECC Crypto, Tom St Denis
+*/  
+
+#ifdef LTC_MECC
+
+/**
+  Decrypt an ECC encrypted key
+  @param in       The ciphertext
+  @param inlen    The length of the ciphertext (octets)
+  @param out      [out] The plaintext
+  @param outlen   [in/out] The max size and resulting size of the plaintext
+  @param key      The corresponding private ECC key
+  @return CRYPT_OK if successful
+*/
+int ecc_decrypt_key(const unsigned char *in,  unsigned long  inlen,
+                          unsigned char *out, unsigned long *outlen, 
+                          ecc_key *key)
+{
+   unsigned char *ecc_shared, *skey, *pub_expt;
+   unsigned long  x, y, hashOID[32];
+   int            hash, err;
+   ecc_key        pubkey;
+   ltc_asn1_list  decode[3];
+
+   LTC_ARGCHK(in     != NULL);
+   LTC_ARGCHK(out    != NULL);
+   LTC_ARGCHK(outlen != NULL);
+   LTC_ARGCHK(key    != NULL);
+
+   /* right key type? */
+   if (key->type != PK_PRIVATE) {
+      return CRYPT_PK_NOT_PRIVATE;
+   }
+   
+   /* decode to find out hash */
+   LTC_SET_ASN1(decode, 0, LTC_ASN1_OBJECT_IDENTIFIER, hashOID, sizeof(hashOID)/sizeof(hashOID[0]));
+ 
+   if ((err = der_decode_sequence(in, inlen, decode, 1)) != CRYPT_OK) {
+      return err;
+   }
+
+   hash = find_hash_oid(hashOID, decode[0].size);                   
+   if (hash_is_valid(hash) != CRYPT_OK) {
+      return CRYPT_INVALID_PACKET;
+   }
+
+   /* we now have the hash! */
+
+   /* allocate memory */
+   pub_expt   = XMALLOC(ECC_BUF_SIZE);
+   ecc_shared = XMALLOC(ECC_BUF_SIZE);
+   skey       = XMALLOC(MAXBLOCKSIZE);
+   if (pub_expt == NULL || ecc_shared == NULL || skey == NULL) {
+      if (pub_expt != NULL) {
+         XFREE(pub_expt);
+      }
+      if (ecc_shared != NULL) {
+         XFREE(ecc_shared);
+      }
+      if (skey != NULL) {
+         XFREE(skey);
+      }
+      return CRYPT_MEM;
+   }
+   LTC_SET_ASN1(decode, 1, LTC_ASN1_OCTET_STRING,      pub_expt,  ECC_BUF_SIZE);
+   LTC_SET_ASN1(decode, 2, LTC_ASN1_OCTET_STRING,      skey,      MAXBLOCKSIZE);
+
+   /* read the structure in now */
+   if ((err = der_decode_sequence(in, inlen, decode, 3)) != CRYPT_OK) {
+      goto LBL_ERR;
+   }
+
+   /* import ECC key from packet */
+   if ((err = ecc_import(decode[1].data, decode[1].size, &pubkey)) != CRYPT_OK) {
+      goto LBL_ERR;
+   }
+
+   /* make shared key */
+   x = ECC_BUF_SIZE;
+   if ((err = ecc_shared_secret(key, &pubkey, ecc_shared, &x)) != CRYPT_OK) {
+      ecc_free(&pubkey);
+      goto LBL_ERR;
+   }
+   ecc_free(&pubkey);
+
+   y = MIN(ECC_BUF_SIZE, MAXBLOCKSIZE);
+   if ((err = hash_memory(hash, ecc_shared, x, ecc_shared, &y)) != CRYPT_OK) {
+      goto LBL_ERR;
+   }
+
+   /* ensure the hash of the shared secret is at least as big as the encrypt itself */
+   if (decode[2].size > y) {
+      err = CRYPT_INVALID_PACKET;
+      goto LBL_ERR;
+   }
+
+   /* avoid buffer overflow */
+   if (*outlen < decode[2].size) {
+      *outlen = decode[2].size;
+      err = CRYPT_BUFFER_OVERFLOW;
+      goto LBL_ERR;
+   }
+
+   /* Decrypt the key */
+   for (x = 0; x < decode[2].size; x++) {
+     out[x] = skey[x] ^ ecc_shared[x];
+   }
+   *outlen = x;
+
+   err = CRYPT_OK;
+LBL_ERR:
+#ifdef LTC_CLEAN_STACK
+   zeromem(pub_expt,   ECC_BUF_SIZE);
+   zeromem(ecc_shared, ECC_BUF_SIZE);
+   zeromem(skey,       MAXBLOCKSIZE);
+#endif
+
+   XFREE(pub_expt);
+   XFREE(ecc_shared);
+   XFREE(skey);
+
+   return err;
+}
+
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/pk/ecc/ecc_decrypt_key.c,v $ */
+/* $Revision: 1.7 $ */
+/* $Date: 2007/05/12 14:32:35 $ */
diff --git a/core/lib/libtomcrypt/src/pk/ecc/ecc_encrypt_key.c b/core/lib/libtomcrypt/src/pk/ecc/ecc_encrypt_key.c
new file mode 100644
index 0000000..f37f374
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/ecc/ecc_encrypt_key.c
@@ -0,0 +1,162 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+
+/* Implements ECC over Z/pZ for curve y^2 = x^3 - 3x + b
+ *
+ * All curves taken from NIST recommendation paper of July 1999
+ * Available at http://csrc.nist.gov/cryptval/dss.htm
+ */
+#include "tomcrypt.h"
+
+/**
+  @file ecc_encrypt_key.c
+  ECC Crypto, Tom St Denis
+*/  
+
+#ifdef LTC_MECC
+
+/**
+  Encrypt a symmetric key with ECC 
+  @param in         The symmetric key you want to encrypt
+  @param inlen      The length of the key to encrypt (octets)
+  @param out        [out] The destination for the ciphertext
+  @param outlen     [in/out] The max size and resulting size of the ciphertext
+  @param prng       An active PRNG state
+  @param wprng      The index of the PRNG you wish to use 
+  @param hash       The index of the hash you want to use 
+  @param key        The ECC key you want to encrypt to
+  @return CRYPT_OK if successful
+*/
+int ecc_encrypt_key(const unsigned char *in,   unsigned long inlen,
+                          unsigned char *out,  unsigned long *outlen, 
+                          prng_state *prng, int wprng, int hash, 
+                          ecc_key *key)
+{
+    unsigned char *pub_expt, *ecc_shared, *skey;
+    ecc_key        pubkey;
+    unsigned long  x, y, pubkeysize;
+    int            err;
+
+    LTC_ARGCHK(in      != NULL);
+    LTC_ARGCHK(out     != NULL);
+    LTC_ARGCHK(outlen  != NULL);
+    LTC_ARGCHK(key     != NULL);
+
+    /* check that wprng/cipher/hash are not invalid */
+    if ((err = prng_is_valid(wprng)) != CRYPT_OK) {
+       return err;
+    }
+
+    if ((err = hash_is_valid(hash)) != CRYPT_OK) {
+       return err;
+    }
+
+    if (inlen > hash_descriptor[hash].hashsize) {
+       return CRYPT_INVALID_HASH;
+    }
+
+    /* make a random key and export the public copy */
+    if ((err = ecc_make_key_ex(prng, wprng, &pubkey, key->dp)) != CRYPT_OK) {
+       return err;
+    }
+
+    pub_expt   = XMALLOC(ECC_BUF_SIZE);
+    ecc_shared = XMALLOC(ECC_BUF_SIZE);
+    skey       = XMALLOC(MAXBLOCKSIZE);
+    if (pub_expt == NULL || ecc_shared == NULL || skey == NULL) {
+       if (pub_expt != NULL) {
+          XFREE(pub_expt);
+       }
+       if (ecc_shared != NULL) {
+          XFREE(ecc_shared);
+       }
+       if (skey != NULL) {
+          XFREE(skey);
+       }
+       ecc_free(&pubkey);
+       return CRYPT_MEM;
+    }
+
+    pubkeysize = ECC_BUF_SIZE;
+    if ((err = ecc_export(pub_expt, &pubkeysize, PK_PUBLIC, &pubkey)) != CRYPT_OK) {
+       ecc_free(&pubkey);
+       goto LBL_ERR;
+    }
+    
+    /* make random key */
+    x        = ECC_BUF_SIZE;
+    if ((err = ecc_shared_secret(&pubkey, key, ecc_shared, &x)) != CRYPT_OK) {
+       ecc_free(&pubkey);
+       goto LBL_ERR;
+    }
+    ecc_free(&pubkey);
+    y = MAXBLOCKSIZE;
+    if ((err = hash_memory(hash, ecc_shared, x, skey, &y)) != CRYPT_OK) {
+       goto LBL_ERR;
+    }
+    
+    /* Encrypt key */
+    for (x = 0; x < inlen; x++) {
+      skey[x] ^= in[x];
+    }
+
+    err = der_encode_sequence_multi(out, outlen,
+                                    LTC_ASN1_OBJECT_IDENTIFIER,  hash_descriptor[hash].OIDlen,   hash_descriptor[hash].OID,
+                                    LTC_ASN1_OCTET_STRING,       pubkeysize,                     pub_expt,
+                                    LTC_ASN1_OCTET_STRING,       inlen,                          skey,
+                                    LTC_ASN1_EOL,                0UL,                            NULL);
+
+LBL_ERR:
+#ifdef LTC_CLEAN_STACK
+    /* clean up */
+    zeromem(pub_expt,   ECC_BUF_SIZE);
+    zeromem(ecc_shared, ECC_BUF_SIZE);
+    zeromem(skey,       MAXBLOCKSIZE);
+#endif
+
+    XFREE(skey);
+    XFREE(ecc_shared);
+    XFREE(pub_expt);
+
+    return err;
+}
+
+#endif
+/* $Source: /cvs/libtom/libtomcrypt/src/pk/ecc/ecc_encrypt_key.c,v $ */
+/* $Revision: 1.6 $ */
+/* $Date: 2007/05/12 14:32:35 $ */
diff --git a/core/lib/libtomcrypt/src/pk/ecc/ecc_export.c b/core/lib/libtomcrypt/src/pk/ecc/ecc_export.c
new file mode 100644
index 0000000..70baedd
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/ecc/ecc_export.c
@@ -0,0 +1,108 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+
+/* Implements ECC over Z/pZ for curve y^2 = x^3 - 3x + b
+ *
+ * All curves taken from NIST recommendation paper of July 1999
+ * Available at http://csrc.nist.gov/cryptval/dss.htm
+ */
+#include "tomcrypt.h"
+
+/**
+  @file ecc_export.c
+  ECC Crypto, Tom St Denis
+*/  
+
+#ifdef LTC_MECC
+
+/**
+  Export an ECC key as a binary packet
+  @param out     [out] Destination for the key
+  @param outlen  [in/out] Max size and resulting size of the exported key
+  @param type    The type of key you want to export (PK_PRIVATE or PK_PUBLIC)
+  @param key     The key to export
+  @return CRYPT_OK if successful
+*/
+int ecc_export(unsigned char *out, unsigned long *outlen, int type, ecc_key *key)
+{
+   int           err;
+   unsigned char flags[1];
+   unsigned long key_size;
+
+   LTC_ARGCHK(out    != NULL);
+   LTC_ARGCHK(outlen != NULL);
+   LTC_ARGCHK(key    != NULL);
+   
+   /* type valid? */
+   if (key->type != PK_PRIVATE && type == PK_PRIVATE) {
+      return CRYPT_PK_TYPE_MISMATCH;
+   }
+
+   if (ltc_ecc_is_valid_idx(key->idx) == 0) {
+      return CRYPT_INVALID_ARG;
+   }
+
+   /* we store the NIST byte size */
+   key_size = key->dp->size;
+
+   if (type == PK_PRIVATE) {
+       flags[0] = 1;
+       err = der_encode_sequence_multi(out, outlen,
+                                 LTC_ASN1_BIT_STRING,      1UL, flags,
+                                 LTC_ASN1_SHORT_INTEGER,   1UL, &key_size,
+                                 LTC_ASN1_INTEGER,         1UL, key->pubkey.x,
+                                 LTC_ASN1_INTEGER,         1UL, key->pubkey.y,
+                                 LTC_ASN1_INTEGER,         1UL, key->k,
+                                 LTC_ASN1_EOL,             0UL, NULL);
+   } else {
+       flags[0] = 0;
+       err = der_encode_sequence_multi(out, outlen,
+                                 LTC_ASN1_BIT_STRING,      1UL, flags,
+                                 LTC_ASN1_SHORT_INTEGER,   1UL, &key_size,
+                                 LTC_ASN1_INTEGER,         1UL, key->pubkey.x,
+                                 LTC_ASN1_INTEGER,         1UL, key->pubkey.y,
+                                 LTC_ASN1_EOL,             0UL, NULL);
+   }
+
+   return err;
+}
+
+#endif
+/* $Source: /cvs/libtom/libtomcrypt/src/pk/ecc/ecc_export.c,v $ */
+/* $Revision: 1.6 $ */
+/* $Date: 2007/05/12 14:32:35 $ */
diff --git a/core/lib/libtomcrypt/src/pk/ecc/ecc_free.c b/core/lib/libtomcrypt/src/pk/ecc/ecc_free.c
new file mode 100644
index 0000000..5de73ca
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/ecc/ecc_free.c
@@ -0,0 +1,66 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+
+/* Implements ECC over Z/pZ for curve y^2 = x^3 - 3x + b
+ *
+ * All curves taken from NIST recommendation paper of July 1999
+ * Available at http://csrc.nist.gov/cryptval/dss.htm
+ */
+#include "tomcrypt.h"
+
+/**
+  @file ecc_free.c
+  ECC Crypto, Tom St Denis
+*/  
+
+#ifdef LTC_MECC
+
+/**
+  Free an ECC key from memory
+  @param key   The key you wish to free
+*/
+void ecc_free(ecc_key *key)
+{
+   LTC_ARGCHKVD(key != NULL);
+   mp_clear_multi(key->pubkey.x, key->pubkey.y, key->pubkey.z, key->k, NULL);
+}
+
+#endif
+/* $Source: /cvs/libtom/libtomcrypt/src/pk/ecc/ecc_free.c,v $ */
+/* $Revision: 1.6 $ */
+/* $Date: 2007/05/12 14:32:35 $ */
diff --git a/core/lib/libtomcrypt/src/pk/ecc/ecc_get_size.c b/core/lib/libtomcrypt/src/pk/ecc/ecc_get_size.c
new file mode 100644
index 0000000..5e4e382
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/ecc/ecc_get_size.c
@@ -0,0 +1,70 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+
+/* Implements ECC over Z/pZ for curve y^2 = x^3 - 3x + b
+ *
+ * All curves taken from NIST recommendation paper of July 1999
+ * Available at http://csrc.nist.gov/cryptval/dss.htm
+ */
+#include "tomcrypt.h"
+
+/**
+  @file ecc_get_size.c
+  ECC Crypto, Tom St Denis
+*/  
+
+#ifdef LTC_MECC
+
+/**
+  Get the size of an ECC key
+  @param key    The key to get the size of 
+  @return The size (octets) of the key or INT_MAX on error
+*/
+int ecc_get_size(ecc_key *key)
+{
+   LTC_ARGCHK(key != NULL);
+   if (ltc_ecc_is_valid_idx(key->idx))
+      return key->dp->size;
+   else
+      return INT_MAX; /* large value known to cause it to fail when passed to ecc_make_key() */
+}
+
+#endif
+/* $Source: /cvs/libtom/libtomcrypt/src/pk/ecc/ecc_get_size.c,v $ */
+/* $Revision: 1.6 $ */
+/* $Date: 2007/05/12 14:32:35 $ */
diff --git a/core/lib/libtomcrypt/src/pk/ecc/ecc_import.c b/core/lib/libtomcrypt/src/pk/ecc/ecc_import.c
new file mode 100644
index 0000000..41b3fe5
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/ecc/ecc_import.c
@@ -0,0 +1,198 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+
+/* Implements ECC over Z/pZ for curve y^2 = x^3 - 3x + b
+ *
+ * All curves taken from NIST recommendation paper of July 1999
+ * Available at http://csrc.nist.gov/cryptval/dss.htm
+ */
+#include "tomcrypt.h"
+
+/**
+  @file ecc_import.c
+  ECC Crypto, Tom St Denis
+*/  
+
+#ifdef LTC_MECC
+
+static int is_point(ecc_key *key)
+{
+   void *prime, *b, *t1, *t2;
+   int err;
+   
+   if ((err = mp_init_multi(&prime, &b, &t1, &t2, NULL)) != CRYPT_OK) {
+      return err;
+   }
+   
+   /* load prime and b */
+   if ((err = mp_read_radix(prime, key->dp->prime, 16)) != CRYPT_OK)                          { goto error; }
+   if ((err = mp_read_radix(b, key->dp->B, 16)) != CRYPT_OK)                                  { goto error; }
+   
+   /* compute y^2 */
+   if ((err = mp_sqr(key->pubkey.y, t1)) != CRYPT_OK)                                         { goto error; }
+   
+   /* compute x^3 */
+   if ((err = mp_sqr(key->pubkey.x, t2)) != CRYPT_OK)                                         { goto error; }
+   if ((err = mp_mod(t2, prime, t2)) != CRYPT_OK)                                             { goto error; }
+   if ((err = mp_mul(key->pubkey.x, t2, t2)) != CRYPT_OK)                                     { goto error; }
+   
+   /* compute y^2 - x^3 */
+   if ((err = mp_sub(t1, t2, t1)) != CRYPT_OK)                                                { goto error; }
+   
+   /* compute y^2 - x^3 + 3x */
+   if ((err = mp_add(t1, key->pubkey.x, t1)) != CRYPT_OK)                                     { goto error; }
+   if ((err = mp_add(t1, key->pubkey.x, t1)) != CRYPT_OK)                                     { goto error; }
+   if ((err = mp_add(t1, key->pubkey.x, t1)) != CRYPT_OK)                                     { goto error; }
+   if ((err = mp_mod(t1, prime, t1)) != CRYPT_OK)                                             { goto error; }
+   while (mp_cmp_d(t1, 0) == LTC_MP_LT) {
+      if ((err = mp_add(t1, prime, t1)) != CRYPT_OK)                                          { goto error; }
+   }
+   while (mp_cmp(t1, prime) != LTC_MP_LT) {
+      if ((err = mp_sub(t1, prime, t1)) != CRYPT_OK)                                          { goto error; }
+   }
+   
+   /* compare to b */
+   if (mp_cmp(t1, b) != LTC_MP_EQ) {
+      err = CRYPT_INVALID_PACKET;
+   } else {
+      err = CRYPT_OK;
+   }
+   
+error:
+   mp_clear_multi(prime, b, t1, t2, NULL);
+   return err;
+}
+
+/**
+  Import an ECC key from a binary packet
+  @param in      The packet to import
+  @param inlen   The length of the packet
+  @param key     [out] The destination of the import
+  @return CRYPT_OK if successful, upon error all allocated memory will be freed
+*/
+int ecc_import(const unsigned char *in, unsigned long inlen, ecc_key *key)
+{
+   return ecc_import_ex(in, inlen, key, NULL);
+}
+
+/**
+  Import an ECC key from a binary packet, using user supplied domain params rather than one of the NIST ones
+  @param in      The packet to import
+  @param inlen   The length of the packet
+  @param key     [out] The destination of the import
+  @param dp      pointer to user supplied params; must be the same as the params used when exporting
+  @return CRYPT_OK if successful, upon error all allocated memory will be freed
+*/
+int ecc_import_ex(const unsigned char *in, unsigned long inlen, ecc_key *key, const ltc_ecc_set_type *dp)
+{
+   unsigned long key_size;
+   unsigned char flags[1];
+   int           err;
+
+   LTC_ARGCHK(in  != NULL);
+   LTC_ARGCHK(key != NULL);
+   LTC_ARGCHK(ltc_mp.name != NULL);
+
+   /* init key */
+   if (mp_init_multi(&key->pubkey.x, &key->pubkey.y, &key->pubkey.z, &key->k, NULL) != CRYPT_OK) {
+      return CRYPT_MEM;
+   }
+
+   /* find out what type of key it is */
+   if ((err = der_decode_sequence_multi(in, inlen, 
+                                  LTC_ASN1_BIT_STRING, 1UL, &flags,
+                                  LTC_ASN1_EOL,        0UL, NULL)) != CRYPT_OK) {
+      goto done;
+   }
+
+
+   if (flags[0] == 1) {
+      /* private key */
+      key->type = PK_PRIVATE;
+      if ((err = der_decode_sequence_multi(in, inlen,
+                                     LTC_ASN1_BIT_STRING,      1UL, flags,
+                                     LTC_ASN1_SHORT_INTEGER,   1UL, &key_size,
+                                     LTC_ASN1_INTEGER,         1UL, key->pubkey.x,
+                                     LTC_ASN1_INTEGER,         1UL, key->pubkey.y,
+                                     LTC_ASN1_INTEGER,         1UL, key->k,
+                                     LTC_ASN1_EOL,             0UL, NULL)) != CRYPT_OK) {
+         goto done;
+      }
+   } else {
+      /* public key */
+      key->type = PK_PUBLIC;
+      if ((err = der_decode_sequence_multi(in, inlen,
+                                     LTC_ASN1_BIT_STRING,      1UL, flags,
+                                     LTC_ASN1_SHORT_INTEGER,   1UL, &key_size,
+                                     LTC_ASN1_INTEGER,         1UL, key->pubkey.x,
+                                     LTC_ASN1_INTEGER,         1UL, key->pubkey.y,
+                                     LTC_ASN1_EOL,             0UL, NULL)) != CRYPT_OK) {
+         goto done;
+      }
+   }
+
+   if (dp == NULL) {
+     /* find the idx */
+     for (key->idx = 0; ltc_ecc_sets[key->idx].size && (unsigned long)ltc_ecc_sets[key->idx].size != key_size; ++key->idx);
+     if (ltc_ecc_sets[key->idx].size == 0) {
+       err = CRYPT_INVALID_PACKET;
+       goto done;
+     }
+     key->dp = &ltc_ecc_sets[key->idx];
+   } else {
+     key->idx = -1;
+     key->dp = dp;
+   }
+   /* set z */
+   if ((err = mp_set(key->pubkey.z, 1)) != CRYPT_OK) { goto done; }
+   
+   /* is it a point on the curve?  */
+   if ((err = is_point(key)) != CRYPT_OK) {
+      goto done;
+   }
+
+   /* we're good */
+   return CRYPT_OK;
+done:
+   mp_clear_multi(key->pubkey.x, key->pubkey.y, key->pubkey.z, key->k, NULL);
+   return err;
+}
+#endif
+/* $Source: /cvs/libtom/libtomcrypt/src/pk/ecc/ecc_import.c,v $ */
+/* $Revision: 1.13 $ */
+/* $Date: 2007/05/12 14:32:35 $ */
diff --git a/core/lib/libtomcrypt/src/pk/ecc/ecc_make_key.c b/core/lib/libtomcrypt/src/pk/ecc/ecc_make_key.c
new file mode 100644
index 0000000..4e9f547
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/ecc/ecc_make_key.c
@@ -0,0 +1,156 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+
+/* Implements ECC over Z/pZ for curve y^2 = x^3 - 3x + b
+ *
+ * All curves taken from NIST recommendation paper of July 1999
+ * Available at http://csrc.nist.gov/cryptval/dss.htm
+ */
+#include "tomcrypt.h"
+
+/**
+  @file ecc_make_key.c
+  ECC Crypto, Tom St Denis
+*/  
+
+#ifdef LTC_MECC
+
+/**
+  Make a new ECC key 
+  @param prng         An active PRNG state
+  @param wprng        The index of the PRNG you wish to use
+  @param keysize      The keysize for the new key (in octets from 20 to 65 bytes)
+  @param key          [out] Destination of the newly created key
+  @return CRYPT_OK if successful, upon error all allocated memory will be freed
+*/
+int ecc_make_key(prng_state *prng, int wprng, int keysize, ecc_key *key)
+{
+   int x, err;
+
+   /* find key size */
+   for (x = 0; (keysize > ltc_ecc_sets[x].size) && (ltc_ecc_sets[x].size != 0); x++);
+   keysize = ltc_ecc_sets[x].size;
+
+   if (keysize > ECC_MAXSIZE || ltc_ecc_sets[x].size == 0) {
+      return CRYPT_INVALID_KEYSIZE;
+   }
+   err = ecc_make_key_ex(prng, wprng, key, &ltc_ecc_sets[x]);
+   key->idx = x;
+   return err;
+}
+
+int ecc_make_key_ex(prng_state *prng, int wprng, ecc_key *key, const ltc_ecc_set_type *dp)
+{
+   int            err;
+   ecc_point     *base;
+   void          *prime, *order;
+   unsigned char *buf;
+   int            keysize;
+
+   LTC_ARGCHK(key         != NULL);
+   LTC_ARGCHK(ltc_mp.name != NULL);
+   LTC_ARGCHK(dp          != NULL);
+
+   /* good prng? */
+   if ((err = prng_is_valid(wprng)) != CRYPT_OK) {
+      return err;
+   }
+
+   key->idx = -1;
+   key->dp  = dp;
+   keysize  = dp->size;
+
+   /* allocate ram */
+   base = NULL;
+   buf  = XMALLOC(ECC_MAXSIZE);
+   if (buf == NULL) {
+      return CRYPT_MEM;
+   }
+
+   /* make up random string */
+   if (prng_descriptor[wprng]->read(buf, (unsigned long)keysize, prng) != (unsigned long)keysize) {
+      err = CRYPT_ERROR_READPRNG;
+      goto ERR_BUF;
+   }
+
+   /* setup the key variables */
+   if ((err = mp_init_multi(&key->pubkey.x, &key->pubkey.y, &key->pubkey.z, &key->k, &prime, &order, NULL)) != CRYPT_OK) {
+      goto ERR_BUF;
+   }
+   base = ltc_ecc_new_point();
+   if (base == NULL) {
+      err = CRYPT_MEM;
+      goto errkey;
+   }
+
+   /* read in the specs for this key */
+   if ((err = mp_read_radix(prime,   (char *)key->dp->prime, 16)) != CRYPT_OK)                  { goto errkey; }
+   if ((err = mp_read_radix(order,   (char *)key->dp->order, 16)) != CRYPT_OK)                  { goto errkey; }
+   if ((err = mp_read_radix(base->x, (char *)key->dp->Gx, 16)) != CRYPT_OK)                     { goto errkey; }
+   if ((err = mp_read_radix(base->y, (char *)key->dp->Gy, 16)) != CRYPT_OK)                     { goto errkey; }
+   if ((err = mp_set(base->z, 1)) != CRYPT_OK)                                                  { goto errkey; }
+   if ((err = mp_read_unsigned_bin(key->k, (unsigned char *)buf, keysize)) != CRYPT_OK)         { goto errkey; }
+
+   /* the key should be smaller than the order of base point */
+   if (mp_cmp(key->k, order) != LTC_MP_LT) {
+       if((err = mp_mod(key->k, order, key->k)) != CRYPT_OK)                                    { goto errkey; }
+   }
+   /* make the public key */
+   if ((err = ltc_mp.ecc_ptmul(key->k, base, &key->pubkey, prime, 1)) != CRYPT_OK)              { goto errkey; }
+   key->type = PK_PRIVATE;
+
+   /* free up ram */
+   err = CRYPT_OK;
+   goto cleanup;
+errkey:
+   mp_clear_multi(key->pubkey.x, key->pubkey.y, key->pubkey.z, key->k, NULL);
+cleanup:
+   ltc_ecc_del_point(base);
+   mp_clear_multi(prime, order, NULL);
+ERR_BUF:
+#ifdef LTC_CLEAN_STACK
+   zeromem(buf, ECC_MAXSIZE);
+#endif
+   XFREE(buf);
+   return err;
+}
+
+#endif
+/* $Source: /cvs/libtom/libtomcrypt/src/pk/ecc/ecc_make_key.c,v $ */
+/* $Revision: 1.13 $ */
+/* $Date: 2007/05/12 14:32:35 $ */
diff --git a/core/lib/libtomcrypt/src/pk/ecc/ecc_shared_secret.c b/core/lib/libtomcrypt/src/pk/ecc/ecc_shared_secret.c
new file mode 100644
index 0000000..5c0b563
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/ecc/ecc_shared_secret.c
@@ -0,0 +1,121 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+
+/* Implements ECC over Z/pZ for curve y^2 = x^3 - 3x + b
+ *
+ * All curves taken from NIST recommendation paper of July 1999
+ * Available at http://csrc.nist.gov/cryptval/dss.htm
+ */
+#include "tomcrypt.h"
+
+/**
+  @file ecc_shared_secret.c
+  ECC Crypto, Tom St Denis
+*/  
+
+#ifdef LTC_MECC
+
+/**
+  Create an ECC shared secret between two keys
+  @param private_key      The private ECC key
+  @param public_key       The public key
+  @param out              [out] Destination of the shared secret (Conforms to EC-DH from ANSI X9.63)
+  @param outlen           [in/out] The max size and resulting size of the shared secret
+  @return CRYPT_OK if successful
+*/
+int ecc_shared_secret(ecc_key *private_key, ecc_key *public_key,
+                      unsigned char *out, unsigned long *outlen)
+{
+   unsigned long  x;
+   ecc_point     *result;
+   void          *prime;
+   int            err;
+
+   LTC_ARGCHK(private_key != NULL);
+   LTC_ARGCHK(public_key  != NULL);
+   LTC_ARGCHK(out         != NULL);
+   LTC_ARGCHK(outlen      != NULL);
+
+   /* type valid? */
+   if (private_key->type != PK_PRIVATE) {
+      return CRYPT_PK_NOT_PRIVATE;
+   }
+
+   if (ltc_ecc_is_valid_idx(private_key->idx) == 0 || ltc_ecc_is_valid_idx(public_key->idx) == 0) {
+      return CRYPT_INVALID_ARG;
+   }
+
+   if (XSTRCMP(private_key->dp->name, public_key->dp->name) != 0) {
+      return CRYPT_PK_TYPE_MISMATCH;
+   }
+
+   /* make new point */
+   result = ltc_ecc_new_point();
+   if (result == NULL) {
+      return CRYPT_MEM;
+   }
+
+   if ((err = mp_init(&prime)) != CRYPT_OK) {
+      ltc_ecc_del_point(result);
+      return err;
+   }
+
+   if ((err = mp_read_radix(prime, (char *)private_key->dp->prime, 16)) != CRYPT_OK)                               { goto done; }
+   if ((err = ltc_mp.ecc_ptmul(private_key->k, &public_key->pubkey, result, prime, 1)) != CRYPT_OK)                { goto done; }
+
+   x = (unsigned long)mp_unsigned_bin_size(prime);
+   if (*outlen < x) {
+      *outlen = x;
+      err = CRYPT_BUFFER_OVERFLOW;
+      goto done;
+   }
+   zeromem(out, x);
+   if ((err = mp_to_unsigned_bin(result->x, out + (x - mp_unsigned_bin_size(result->x))))   != CRYPT_OK)           { goto done; }
+
+   err     = CRYPT_OK;
+   *outlen = x;
+done:
+   mp_clear(prime);
+   ltc_ecc_del_point(result);
+   return err;
+}
+
+#endif
+/* $Source: /cvs/libtom/libtomcrypt/src/pk/ecc/ecc_shared_secret.c,v $ */
+/* $Revision: 1.10 $ */
+/* $Date: 2007/05/12 14:32:35 $ */
diff --git a/core/lib/libtomcrypt/src/pk/ecc/ecc_sign_hash.c b/core/lib/libtomcrypt/src/pk/ecc/ecc_sign_hash.c
new file mode 100644
index 0000000..0f5f3ae
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/ecc/ecc_sign_hash.c
@@ -0,0 +1,179 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+
+/* Implements ECC over Z/pZ for curve y^2 = x^3 - 3x + b
+ *
+ * All curves taken from NIST recommendation paper of July 1999
+ * Available at http://csrc.nist.gov/cryptval/dss.htm
+ */
+#include "tomcrypt.h"
+
+/**
+  @file ecc_sign_hash.c
+  ECC Crypto, Tom St Denis
+*/  
+
+#ifdef LTC_MECC
+
+/**
+  Sign a hash with ECC
+  @param in       The hash to sign
+  @param inlen    The length of the hash to sign
+  @param r        The "r" integer of the signature (caller must initialize with mp_init() first)
+  @param s        The "s" integer of the signature (caller must initialize with mp_init() first)
+  @param prng     An active PRNG state
+  @param wprng    The index of the PRNG desired
+  @param key      A private ECC key
+  @return CRYPT_OK if successful
+*/
+int ecc_sign_hash_raw(const unsigned char *in,  unsigned long inlen,
+                            void *r, void *s,
+                            prng_state *prng, int wprng, ecc_key *key)
+{
+   ecc_key       pubkey;
+   void          *e, *p;
+   int           err;
+
+   LTC_ARGCHK(in  != NULL);
+   LTC_ARGCHK(r   != NULL);
+   LTC_ARGCHK(s   != NULL);
+   LTC_ARGCHK(key != NULL);
+
+   /* is this a private key? */
+   if (key->type != PK_PRIVATE) {
+      return CRYPT_PK_NOT_PRIVATE;
+   }
+   
+   /* is the IDX valid ?  */
+   if (ltc_ecc_is_valid_idx(key->idx) != 1) {
+      return CRYPT_PK_INVALID_TYPE;
+   }
+   
+   if ((err = prng_is_valid(wprng)) != CRYPT_OK) {
+      return err;
+   }
+
+   /* get the hash and load it as a bignum into 'e' */
+   /* init the bignums */
+   if ((err = mp_init_multi(&p, &e, NULL)) != CRYPT_OK) {
+      return err;
+   }
+   if ((err = mp_read_radix(p, (char *)key->dp->order, 16)) != CRYPT_OK)                      { goto errnokey; }
+   if ((err = mp_read_unsigned_bin(e, (unsigned char *)in, (int)inlen)) != CRYPT_OK)          { goto errnokey; }
+
+   /* make up a key and export the public copy */
+   for (;;) {
+      if ((err = ecc_make_key_ex(prng, wprng, &pubkey, key->dp)) != CRYPT_OK) {
+         goto errnokey;
+      }
+
+      /* find r = x1 mod n */
+      if ((err = mp_mod(pubkey.pubkey.x, p, r)) != CRYPT_OK)                 { goto error; }
+
+      if (mp_iszero(r) == LTC_MP_YES) {
+         ecc_free(&pubkey);
+      } else { 
+        /* find s = (e + xr)/k */
+        if ((err = mp_invmod(pubkey.k, p, pubkey.k)) != CRYPT_OK)            { goto error; } /* k = 1/k */
+        if ((err = mp_mulmod(key->k, r, p, s)) != CRYPT_OK)                  { goto error; } /* s = xr */
+        if ((err = mp_add(e, s, s)) != CRYPT_OK)                             { goto error; } /* s = e +  xr */
+        if ((err = mp_mod(s, p, s)) != CRYPT_OK)                             { goto error; } /* s = e +  xr */
+        if ((err = mp_mulmod(s, pubkey.k, p, s)) != CRYPT_OK)                { goto error; } /* s = (e + xr)/k */
+        ecc_free(&pubkey);
+        if (mp_iszero(s) == LTC_MP_NO) {
+           break;
+        }
+      }
+   }
+
+   err = CRYPT_OK;
+   goto errnokey;
+
+error:
+   ecc_free(&pubkey);
+errnokey:
+   mp_clear_multi(p, e, NULL);
+   return err;
+}
+
+/**
+  Sign a message digest
+  @param in        The message digest to sign
+  @param inlen     The length of the digest
+  @param out       [out] The destination for the signature
+  @param outlen    [in/out] The max size and resulting size of the signature
+  @param prng      An active PRNG state
+  @param wprng     The index of the PRNG you wish to use
+  @param key       A private ECC key
+  @return CRYPT_OK if successful
+*/
+int ecc_sign_hash(const unsigned char *in,  unsigned long inlen,
+                        unsigned char *out, unsigned long *outlen,
+                        prng_state *prng, int wprng, ecc_key *key)
+{
+   void          *r, *s;
+   int           err;
+
+   LTC_ARGCHK(in     != NULL);
+   LTC_ARGCHK(out    != NULL);
+   LTC_ARGCHK(outlen != NULL);
+   LTC_ARGCHK(key    != NULL);
+
+   if (mp_init_multi(&r, &s, NULL) != CRYPT_OK) {
+      return CRYPT_MEM;
+   }
+
+   if ((err = ecc_sign_hash_raw(in, inlen, r, s, prng, wprng, key)) != CRYPT_OK) {
+      goto error;
+   }
+
+   /* store as SEQUENCE { r, s -- integer } */
+   err = der_encode_sequence_multi(out, outlen,
+                             LTC_ASN1_INTEGER, 1UL, r,
+                             LTC_ASN1_INTEGER, 1UL, s,
+                             LTC_ASN1_EOL, 0UL, NULL);
+
+error:
+   mp_clear_multi(r, s, NULL);
+   return err;   
+}
+
+#endif
+/* $Source: /cvs/libtom/libtomcrypt/src/pk/ecc/ecc_sign_hash.c,v $ */
+/* $Revision: 1.11 $ */
+/* $Date: 2007/05/12 14:32:35 $ */
diff --git a/core/lib/libtomcrypt/src/pk/ecc/ecc_sizes.c b/core/lib/libtomcrypt/src/pk/ecc/ecc_sizes.c
new file mode 100644
index 0000000..da8a45d
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/ecc/ecc_sizes.c
@@ -0,0 +1,74 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+
+/* Implements ECC over Z/pZ for curve y^2 = x^3 - 3x + b
+ *
+ * All curves taken from NIST recommendation paper of July 1999
+ * Available at http://csrc.nist.gov/cryptval/dss.htm
+ */
+#include "tomcrypt.h"
+
+/**
+  @file ecc_sizes.c
+  ECC Crypto, Tom St Denis
+*/  
+
+#ifdef LTC_MECC
+
+void ecc_sizes(int *low, int *high)
+{
+ int i;
+ LTC_ARGCHKVD(low  != NULL);
+ LTC_ARGCHKVD(high != NULL);
+
+ *low = INT_MAX;
+ *high = 0;
+ for (i = 0; ltc_ecc_sets[i].size != 0; i++) {
+     if (ltc_ecc_sets[i].size < *low)  {
+        *low  = ltc_ecc_sets[i].size;
+     }
+     if (ltc_ecc_sets[i].size > *high) {
+        *high = ltc_ecc_sets[i].size;
+     }
+ }
+}
+
+#endif
+/* $Source: /cvs/libtom/libtomcrypt/src/pk/ecc/ecc_sizes.c,v $ */
+/* $Revision: 1.6 $ */
+/* $Date: 2007/05/12 14:32:35 $ */
diff --git a/core/lib/libtomcrypt/src/pk/ecc/ecc_verify_hash.c b/core/lib/libtomcrypt/src/pk/ecc/ecc_verify_hash.c
new file mode 100644
index 0000000..2ee3609
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/ecc/ecc_verify_hash.c
@@ -0,0 +1,228 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+
+/* Implements ECC over Z/pZ for curve y^2 = x^3 - 3x + b
+ *
+ * All curves taken from NIST recommendation paper of July 1999
+ * Available at http://csrc.nist.gov/cryptval/dss.htm
+ */
+#include "tomcrypt.h"
+
+/**
+  @file ecc_verify_hash.c
+  ECC Crypto, Tom St Denis
+*/  
+
+#ifdef LTC_MECC
+
+/* verify 
+ *
+ * w  = s^-1 mod n
+ * u1 = xw 
+ * u2 = rw
+ * X = u1*G + u2*Q
+ * v = X_x1 mod n
+ * accept if v == r
+ */
+
+/**
+  Verify a ECC signature
+  @param r        ECC "r" parameter
+  @param s        ECC "s" parameter
+  @param hash     The hash that was signed
+  @param hashlen  The length of the hash that was signed
+  @param stat     [out] The result of the signature verification, 1==valid, 0==invalid
+  @param key      The corresponding public DH key
+  @return CRYPT_OK if successful (even if the signature is invalid)
+*/
+int ecc_verify_hash_raw(      void   *r, void   *s,
+                        const unsigned char *hash, unsigned long hashlen,
+                        int *stat, ecc_key *key)
+{
+   ecc_point    *mG, *mQ;
+   void          *v, *w, *u1, *u2, *e, *p, *m;
+   void          *mp = NULL;
+   int           err;
+
+   LTC_ARGCHK(r    != NULL);
+   LTC_ARGCHK(s    != NULL);
+   LTC_ARGCHK(hash != NULL);
+   LTC_ARGCHK(stat != NULL);
+   LTC_ARGCHK(key  != NULL);
+
+   /* default to invalid signature */
+   *stat = 0;
+   mp    = NULL;
+
+   /* is the IDX valid ?  */
+   if (ltc_ecc_is_valid_idx(key->idx) != 1) {
+      return CRYPT_PK_INVALID_TYPE;
+   }
+
+   /* allocate ints */
+   if ((err = mp_init_multi(&v, &w, &u1, &u2, &p, &e, &m, NULL)) != CRYPT_OK) {
+      return CRYPT_MEM;
+   }
+
+   /* allocate points */
+   mG = ltc_ecc_new_point();
+   mQ = ltc_ecc_new_point();
+   if (mQ  == NULL || mG == NULL) {
+      err = CRYPT_MEM;
+      goto error;
+   }
+
+   /* get the order */
+   if ((err = mp_read_radix(p, (char *)key->dp->order, 16)) != CRYPT_OK)                                { goto error; }
+
+   /* get the modulus */
+   if ((err = mp_read_radix(m, (char *)key->dp->prime, 16)) != CRYPT_OK)                                { goto error; }
+
+   /* check for zero */
+   if (mp_iszero(r) || mp_iszero(s) || mp_cmp(r, p) != LTC_MP_LT || mp_cmp(s, p) != LTC_MP_LT) {
+      err = CRYPT_INVALID_PACKET;
+      goto error;
+   }
+
+   /* read hash */
+   if ((err = mp_read_unsigned_bin(e, (unsigned char *)hash, (int)hashlen)) != CRYPT_OK)                { goto error; }
+
+   /*  w  = s^-1 mod n */
+   if ((err = mp_invmod(s, p, w)) != CRYPT_OK)                                                          { goto error; }
+
+   /* u1 = ew */
+   if ((err = mp_mulmod(e, w, p, u1)) != CRYPT_OK)                                                      { goto error; }
+
+   /* u2 = rw */
+   if ((err = mp_mulmod(r, w, p, u2)) != CRYPT_OK)                                                      { goto error; }
+
+   /* find mG and mQ */
+   if ((err = mp_read_radix(mG->x, (char *)key->dp->Gx, 16)) != CRYPT_OK)                               { goto error; }
+   if ((err = mp_read_radix(mG->y, (char *)key->dp->Gy, 16)) != CRYPT_OK)                               { goto error; }
+   if ((err = mp_set(mG->z, 1)) != CRYPT_OK)                                                            { goto error; }
+
+   if ((err = mp_copy(key->pubkey.x, mQ->x)) != CRYPT_OK)                                               { goto error; }
+   if ((err = mp_copy(key->pubkey.y, mQ->y)) != CRYPT_OK)                                               { goto error; }
+   if ((err = mp_copy(key->pubkey.z, mQ->z)) != CRYPT_OK)                                               { goto error; }
+
+   /* compute u1*mG + u2*mQ = mG */
+   if (ltc_mp.ecc_mul2add == NULL) {
+      if ((err = ltc_mp.ecc_ptmul(u1, mG, mG, m, 0)) != CRYPT_OK)                                       { goto error; }
+      if ((err = ltc_mp.ecc_ptmul(u2, mQ, mQ, m, 0)) != CRYPT_OK)                                       { goto error; }
+  
+      /* find the montgomery mp */
+      if ((err = mp_montgomery_setup(m, &mp)) != CRYPT_OK)                                              { goto error; }
+
+      /* add them */
+      if ((err = ltc_mp.ecc_ptadd(mQ, mG, mG, m, mp)) != CRYPT_OK)                                      { goto error; }
+   
+      /* reduce */
+      if ((err = ltc_mp.ecc_map(mG, m, mp)) != CRYPT_OK)                                                { goto error; }
+   } else {
+      /* use Shamir's trick to compute u1*mG + u2*mQ using half of the doubles */
+      if ((err = ltc_mp.ecc_mul2add(mG, u1, mQ, u2, mG, m)) != CRYPT_OK)                                { goto error; }
+   }
+
+   /* v = X_x1 mod n */
+   if ((err = mp_mod(mG->x, p, v)) != CRYPT_OK)                                                         { goto error; }
+
+   /* does v == r */
+   if (mp_cmp(v, r) == LTC_MP_EQ) {
+      *stat = 1;
+   }
+
+   /* clear up and return */
+   err = CRYPT_OK;
+error:
+   ltc_ecc_del_point(mG);
+   ltc_ecc_del_point(mQ);
+   mp_clear_multi(v, w, u1, u2, p, e, m, NULL);
+   if (mp != NULL) { 
+      mp_montgomery_free(mp);
+   }
+   return err;
+}
+
+/**
+   Verify an ECC signature
+   @param sig         The signature to verify
+   @param siglen      The length of the signature (octets)
+   @param hash        The hash (message digest) that was signed
+   @param hashlen     The length of the hash (octets)
+   @param stat        Result of signature, 1==valid, 0==invalid
+   @param key         The corresponding public ECC key
+   @return CRYPT_OK if successful (even if the signature is not valid)
+*/
+
+int ecc_verify_hash(const unsigned char *sig,  unsigned long siglen,
+                    const unsigned char *hash, unsigned long hashlen,
+                    int *stat, ecc_key *key)
+{
+   void          *r, *s;
+   int           err;
+
+   LTC_ARGCHK(sig  != NULL);
+   LTC_ARGCHK(hash != NULL);
+   LTC_ARGCHK(stat != NULL);
+   LTC_ARGCHK(key  != NULL);
+
+   /* allocate ints */
+   if ((err = mp_init_multi(&r, &s, NULL)) != CRYPT_OK) {
+      return CRYPT_MEM;
+   }
+
+   /* parse header */
+   if ((err = der_decode_sequence_multi(sig, siglen,
+                                  LTC_ASN1_INTEGER, 1UL, r,
+                                  LTC_ASN1_INTEGER, 1UL, s,
+                                  LTC_ASN1_EOL, 0UL, NULL)) != CRYPT_OK) {
+      goto error;
+   }
+
+   /* do the op */
+   err = ecc_verify_hash_raw(r, s, hash, hashlen, stat, key);
+
+error:
+   mp_clear_multi(r, s, NULL);
+   return err;
+}
+
+#endif
+/* $Source: /cvs/libtom/libtomcrypt/src/pk/ecc/ecc_verify_hash.c,v $ */
+/* $Revision: 1.14 $ */
+/* $Date: 2007/05/12 14:32:35 $ */
diff --git a/core/lib/libtomcrypt/src/pk/ecc/ltc_ecc_is_valid_idx.c b/core/lib/libtomcrypt/src/pk/ecc/ltc_ecc_is_valid_idx.c
new file mode 100644
index 0000000..2509273
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/ecc/ltc_ecc_is_valid_idx.c
@@ -0,0 +1,72 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+
+/* Implements ECC over Z/pZ for curve y^2 = x^3 - 3x + b
+ *
+ * All curves taken from NIST recommendation paper of July 1999
+ * Available at http://csrc.nist.gov/cryptval/dss.htm
+ */
+#include "tomcrypt.h"
+
+/**
+  @file ltc_ecc_is_valid_idx.c
+  ECC Crypto, Tom St Denis
+*/  
+
+#ifdef LTC_MECC
+
+/** Returns whether an ECC idx is valid or not
+  @param n   The idx number to check
+  @return 1 if valid, 0 if not
+*/  
+int ltc_ecc_is_valid_idx(int n)
+{
+   int x;
+
+   for (x = 0; ltc_ecc_sets[x].size != 0; x++);
+   /* -1 is a valid index --- indicating that the domain params were supplied by the user */
+   if ((n >= -1) && (n < x)) {
+      return 1;
+   }
+   return 0;
+}
+
+#endif
+/* $Source: /cvs/libtom/libtomcrypt/src/pk/ecc/ltc_ecc_is_valid_idx.c,v $ */
+/* $Revision: 1.7 $ */
+/* $Date: 2007/05/12 14:32:35 $ */
diff --git a/core/lib/libtomcrypt/src/pk/ecc/ltc_ecc_map.c b/core/lib/libtomcrypt/src/pk/ecc/ltc_ecc_map.c
new file mode 100644
index 0000000..7f5447a
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/ecc/ltc_ecc_map.c
@@ -0,0 +1,102 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+
+/* Implements ECC over Z/pZ for curve y^2 = x^3 - 3x + b
+ *
+ * All curves taken from NIST recommendation paper of July 1999
+ * Available at http://csrc.nist.gov/cryptval/dss.htm
+ */
+#include "tomcrypt.h"
+
+/**
+  @file ltc_ecc_map.c
+  ECC Crypto, Tom St Denis
+*/  
+
+#ifdef LTC_MECC
+
+/**
+  Map a projective jacbobian point back to affine space
+  @param P        [in/out] The point to map
+  @param modulus  The modulus of the field the ECC curve is in
+  @param mp       The "b" value from montgomery_setup()
+  @return CRYPT_OK on success
+*/
+int ltc_ecc_map(ecc_point *P, void *modulus, void *mp)
+{
+   void *t1, *t2;
+   int   err;
+
+   LTC_ARGCHK(P       != NULL);
+   LTC_ARGCHK(modulus != NULL);
+   LTC_ARGCHK(mp      != NULL);
+
+   if ((err = mp_init_multi(&t1, &t2, NULL)) != CRYPT_OK) {
+      return CRYPT_MEM;
+   }
+
+   /* first map z back to normal */
+   if ((err = mp_montgomery_reduce(P->z, modulus, mp)) != CRYPT_OK)           { goto done; }
+
+   /* get 1/z */
+   if ((err = mp_invmod(P->z, modulus, t1)) != CRYPT_OK)                      { goto done; }
+ 
+   /* get 1/z^2 and 1/z^3 */
+   if ((err = mp_sqr(t1, t2)) != CRYPT_OK)                                    { goto done; }
+   if ((err = mp_mod(t2, modulus, t2)) != CRYPT_OK)                           { goto done; }
+   if ((err = mp_mul(t1, t2, t1)) != CRYPT_OK)                                { goto done; }
+   if ((err = mp_mod(t1, modulus, t1)) != CRYPT_OK)                           { goto done; }
+
+   /* multiply against x/y */
+   if ((err = mp_mul(P->x, t2, P->x)) != CRYPT_OK)                            { goto done; }
+   if ((err = mp_montgomery_reduce(P->x, modulus, mp)) != CRYPT_OK)           { goto done; }
+   if ((err = mp_mul(P->y, t1, P->y)) != CRYPT_OK)                            { goto done; }
+   if ((err = mp_montgomery_reduce(P->y, modulus, mp)) != CRYPT_OK)           { goto done; }
+   if ((err = mp_set(P->z, 1)) != CRYPT_OK)                                   { goto done; }
+
+   err = CRYPT_OK;
+done:
+   mp_clear_multi(t1, t2, NULL);
+   return err;
+}
+
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/pk/ecc/ltc_ecc_map.c,v $ */
+/* $Revision: 1.7 $ */
+/* $Date: 2007/05/12 14:32:35 $ */
diff --git a/core/lib/libtomcrypt/src/pk/ecc/ltc_ecc_mul2add.c b/core/lib/libtomcrypt/src/pk/ecc/ltc_ecc_mul2add.c
new file mode 100644
index 0000000..dc4cb6b
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/ecc/ltc_ecc_mul2add.c
@@ -0,0 +1,234 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+
+/* Implements ECC over Z/pZ for curve y^2 = x^3 - 3x + b
+ *
+ * All curves taken from NIST recommendation paper of July 1999
+ * Available at http://csrc.nist.gov/cryptval/dss.htm
+ */
+#include "tomcrypt.h"
+
+/**
+  @file ltc_ecc_mul2add.c
+  ECC Crypto, Shamir's Trick, Tom St Denis
+*/  
+
+#ifdef LTC_MECC
+
+#ifdef LTC_ECC_SHAMIR
+
+/** Computes kA*A + kB*B = C using Shamir's Trick
+  @param A        First point to multiply
+  @param kA       What to multiple A by
+  @param B        Second point to multiply
+  @param kB       What to multiple B by
+  @param C        [out] Destination point (can overlap with A or B
+  @param modulus  Modulus for curve 
+  @return CRYPT_OK on success
+*/ 
+int ltc_ecc_mul2add(ecc_point *A, void *kA,
+                    ecc_point *B, void *kB,
+                    ecc_point *C,
+                         void *modulus)
+{
+  ecc_point     *precomp[16];
+  unsigned       bitbufA, bitbufB, lenA, lenB, len, x, y, nA, nB, nibble;
+  unsigned char *tA, *tB;
+  int            err, first;
+  void          *mp, *mu;
+ 
+  /* argchks */
+  LTC_ARGCHK(A       != NULL);
+  LTC_ARGCHK(B       != NULL);
+  LTC_ARGCHK(C       != NULL);
+  LTC_ARGCHK(kA      != NULL);
+  LTC_ARGCHK(kB      != NULL);
+  LTC_ARGCHK(modulus != NULL);
+
+  /* allocate memory */
+  tA = XCALLOC(1, ECC_BUF_SIZE);
+  if (tA == NULL) {
+     return CRYPT_MEM;
+  }
+  tB = XCALLOC(1, ECC_BUF_SIZE);
+  if (tB == NULL) {
+     XFREE(tA);
+     return CRYPT_MEM;
+  }
+
+  /* get sizes */
+  lenA = mp_unsigned_bin_size(kA);
+  lenB = mp_unsigned_bin_size(kB);
+  len  = MAX(lenA, lenB);
+
+  /* sanity check */
+  if ((lenA > ECC_BUF_SIZE) || (lenB > ECC_BUF_SIZE)) {
+     err = CRYPT_INVALID_ARG;
+     goto ERR_T;
+  }
+
+  /* extract and justify kA */
+  mp_to_unsigned_bin(kA, (len - lenA) + tA);
+
+  /* extract and justify kB */
+  mp_to_unsigned_bin(kB, (len - lenB) + tB);
+
+  /* allocate the table */
+  for (x = 0; x < 16; x++) {
+     precomp[x] = ltc_ecc_new_point();
+     if (precomp[x] == NULL) {
+         for (y = 0; y < x; ++y) {
+            ltc_ecc_del_point(precomp[y]);
+         }
+         err = CRYPT_MEM;
+         goto ERR_T;
+     }
+  }
+
+   /* init montgomery reduction */
+   if ((err = mp_montgomery_setup(modulus, &mp)) != CRYPT_OK) {
+      goto ERR_P;
+   }
+   if ((err = mp_init(&mu)) != CRYPT_OK) {
+      goto ERR_MP;
+   }
+   if ((err = mp_montgomery_normalization(mu, modulus)) != CRYPT_OK) {
+      goto ERR_MU;
+   }
+
+  /* copy ones ... */
+  if ((err = mp_mulmod(A->x, mu, modulus, precomp[1]->x)) != CRYPT_OK)                                         { goto ERR_MU; }
+  if ((err = mp_mulmod(A->y, mu, modulus, precomp[1]->y)) != CRYPT_OK)                                         { goto ERR_MU; }
+  if ((err = mp_mulmod(A->z, mu, modulus, precomp[1]->z)) != CRYPT_OK)                                         { goto ERR_MU; }
+
+  if ((err = mp_mulmod(B->x, mu, modulus, precomp[1<<2]->x)) != CRYPT_OK)                                      { goto ERR_MU; }
+  if ((err = mp_mulmod(B->y, mu, modulus, precomp[1<<2]->y)) != CRYPT_OK)                                      { goto ERR_MU; }
+  if ((err = mp_mulmod(B->z, mu, modulus, precomp[1<<2]->z)) != CRYPT_OK)                                      { goto ERR_MU; }
+
+  /* precomp [i,0](A + B) table */
+  if ((err = ltc_mp.ecc_ptdbl(precomp[1], precomp[2], modulus, mp)) != CRYPT_OK)                               { goto ERR_MU; }
+  if ((err = ltc_mp.ecc_ptadd(precomp[1], precomp[2], precomp[3], modulus, mp)) != CRYPT_OK)                   { goto ERR_MU; }
+
+  /* precomp [0,i](A + B) table */
+  if ((err = ltc_mp.ecc_ptdbl(precomp[1<<2], precomp[2<<2], modulus, mp)) != CRYPT_OK)                         { goto ERR_MU; }
+  if ((err = ltc_mp.ecc_ptadd(precomp[1<<2], precomp[2<<2], precomp[3<<2], modulus, mp)) != CRYPT_OK)          { goto ERR_MU; }
+
+  /* precomp [i,j](A + B) table (i != 0, j != 0) */
+  for (x = 1; x < 4; x++) {
+     for (y = 1; y < 4; y++) {
+        if ((err = ltc_mp.ecc_ptadd(precomp[x], precomp[(y<<2)], precomp[x+(y<<2)], modulus, mp)) != CRYPT_OK) { goto ERR_MU; }
+     }
+  }   
+
+  nibble  = 3;
+  first   = 1;
+  bitbufA = tA[0];
+  bitbufB = tB[0];
+
+  /* for every byte of the multiplicands */
+  for (x = -1;; ) {
+     /* grab a nibble */
+     if (++nibble == 4) {
+        ++x; if (x == len) break;
+        bitbufA = tA[x];
+        bitbufB = tB[x];
+        nibble  = 0;
+     }
+
+     /* extract two bits from both, shift/update */
+     nA = (bitbufA >> 6) & 0x03;
+     nB = (bitbufB >> 6) & 0x03;
+     bitbufA = (bitbufA << 2) & 0xFF;   
+     bitbufB = (bitbufB << 2) & 0xFF;   
+
+     /* if both zero, if first, continue */
+     if ((nA == 0) && (nB == 0) && (first == 1)) {
+        continue;
+     }
+
+     /* double twice, only if this isn't the first */
+     if (first == 0) {
+        /* double twice */
+        if ((err = ltc_mp.ecc_ptdbl(C, C, modulus, mp)) != CRYPT_OK)                  { goto ERR_MU; }
+        if ((err = ltc_mp.ecc_ptdbl(C, C, modulus, mp)) != CRYPT_OK)                  { goto ERR_MU; }
+     }
+
+     /* if not both zero */
+     if ((nA != 0) || (nB != 0)) {
+        if (first == 1) {
+           /* if first, copy from table */
+           first = 0;
+           if ((err = mp_copy(precomp[nA + (nB<<2)]->x, C->x)) != CRYPT_OK)           { goto ERR_MU; }
+           if ((err = mp_copy(precomp[nA + (nB<<2)]->y, C->y)) != CRYPT_OK)           { goto ERR_MU; }
+           if ((err = mp_copy(precomp[nA + (nB<<2)]->z, C->z)) != CRYPT_OK)           { goto ERR_MU; }
+        } else {
+           /* if not first, add from table */
+           if ((err = ltc_mp.ecc_ptadd(C, precomp[nA + (nB<<2)], C, modulus, mp)) != CRYPT_OK) { goto ERR_MU; }
+        }
+     }
+  }
+
+  /* reduce to affine */
+  err = ltc_ecc_map(C, modulus, mp);
+
+  /* clean up */
+ERR_MU:
+   mp_clear(mu);
+ERR_MP:
+   mp_montgomery_free(mp);
+ERR_P:
+   for (x = 0; x < 16; x++) {
+       ltc_ecc_del_point(precomp[x]);
+   }
+ERR_T:
+#ifdef LTC_CLEAN_STACK
+   zeromem(tA, ECC_BUF_SIZE);
+   zeromem(tB, ECC_BUF_SIZE);
+#endif
+   XFREE(tA);
+   XFREE(tB);
+
+   return err;
+}
+
+#endif
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/pk/ecc/ltc_ecc_mul2add.c,v $ */
+/* $Revision: 1.8 $ */
+/* $Date: 2007/05/12 14:32:35 $ */
diff --git a/core/lib/libtomcrypt/src/pk/ecc/ltc_ecc_mulmod.c b/core/lib/libtomcrypt/src/pk/ecc/ltc_ecc_mulmod.c
new file mode 100644
index 0000000..dfd59aa
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/ecc/ltc_ecc_mulmod.c
@@ -0,0 +1,251 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+
+/* Implements ECC over Z/pZ for curve y^2 = x^3 - 3x + b
+ *
+ * All curves taken from NIST recommendation paper of July 1999
+ * Available at http://csrc.nist.gov/cryptval/dss.htm
+ */
+#include "tomcrypt.h"
+
+/**
+  @file ltc_ecc_mulmod.c
+  ECC Crypto, Tom St Denis
+*/
+
+#ifdef LTC_MECC
+#ifndef LTC_ECC_TIMING_RESISTANT
+
+/* size of sliding window, don't change this! MUST BE A POWER OF TWO */
+#define WINSIZE 4
+#define WINVARS (1 << (WINSIZE - 1))
+
+/**
+   Perform a point multiplication
+   @param k    The scalar to multiply by
+   @param G    The base point
+   @param R    [out] Destination for kG
+   @param modulus  The modulus of the field the ECC curve is in
+   @param map      Boolean whether to map back to affine or not (1==map, 0 == leave in projective)
+   @return CRYPT_OK on success
+*/
+int ltc_ecc_mulmod(void *k, ecc_point *G, ecc_point *R, void *modulus, int map)
+{
+   ecc_point *tG, *M[WINVARS];
+   int        i, j, err;
+   void       *mu, *mp;
+   ltc_mp_digit buf;
+   int        first, bitbuf, bitcpy, bitcnt, mode, digidx;
+
+   LTC_ARGCHK(k       != NULL);
+   LTC_ARGCHK(G       != NULL);
+   LTC_ARGCHK(R       != NULL);
+   LTC_ARGCHK(modulus != NULL);
+
+   /* init montgomery reduction */
+   if ((err = mp_montgomery_setup(modulus, &mp)) != CRYPT_OK) {
+      return err;
+   }
+   if ((err = mp_init(&mu)) != CRYPT_OK) {
+      mp_montgomery_free(mp);
+      return err;
+   }
+   if ((err = mp_montgomery_normalization(mu, modulus)) != CRYPT_OK) {
+      mp_montgomery_free(mp);
+      mp_clear(mu);
+      return err;
+   }
+
+  /* alloc ram for window temps */
+  for (i = 0; i < WINVARS; i++) {
+      M[i] = ltc_ecc_new_point();
+      if (M[i] == NULL) {
+         for (j = 0; j < i; j++) {
+             ltc_ecc_del_point(M[j]);
+         }
+         mp_montgomery_free(mp);
+         mp_clear(mu);
+         return CRYPT_MEM;
+      }
+  }
+
+   /* make a copy of G incase R==G */
+   tG = ltc_ecc_new_point();
+   if (tG == NULL)                                                                   { err = CRYPT_MEM; goto done; }
+
+   /* tG = G  and convert to montgomery */
+   if (mp_cmp_d(mu, 1) == LTC_MP_EQ) {
+      if ((err = mp_copy(G->x, tG->x)) != CRYPT_OK)                                  { goto done; }
+      if ((err = mp_copy(G->y, tG->y)) != CRYPT_OK)                                  { goto done; }
+      if ((err = mp_copy(G->z, tG->z)) != CRYPT_OK)                                  { goto done; }
+   } else {
+      if ((err = mp_mulmod(G->x, mu, modulus, tG->x)) != CRYPT_OK)                   { goto done; }
+      if ((err = mp_mulmod(G->y, mu, modulus, tG->y)) != CRYPT_OK)                   { goto done; }
+      if ((err = mp_mulmod(G->z, mu, modulus, tG->z)) != CRYPT_OK)                   { goto done; }
+   }
+   mp_clear(mu);
+   mu = NULL;
+
+   /* calc the M tab, which holds kG for k==8..15 */
+   /* M[0] == [WINVARS].G */
+   if ((err = ltc_mp.ecc_ptdbl(tG, M[0], modulus, mp)) != CRYPT_OK)                 { goto done; }
+   for (j = 1; j < (WINSIZE-1); ++j) {
+       if ((err = ltc_mp.ecc_ptdbl(M[0], M[0], modulus, mp)) != CRYPT_OK)               { goto done; }
+   }
+
+   /* now find (WINVARS+k)G for k=1..(WINVARS-1) */
+   for (j = (WINVARS + 1); j < (2*WINVARS); j++) {
+       if ((err = ltc_mp.ecc_ptadd(M[j-(WINVARS + 1)], tG, M[j-WINVARS], modulus, mp)) != CRYPT_OK)   { goto done; }
+   }
+
+   /* setup sliding window */
+   mode   = 0;
+   bitcnt = 1;
+   buf    = 0;
+   digidx = mp_get_digit_count(k) - 1;
+   bitcpy = bitbuf = 0;
+   first  = 1;
+
+   /* perform ops */
+   for (;;) {
+     /* grab next digit as required */
+     if (--bitcnt == 0) {
+       if (digidx == -1) {
+          break;
+       }
+       buf    = mp_get_digit(k, digidx);
+       bitcnt = (int) ltc_mp.bits_per_digit;
+       --digidx;
+     }
+
+     /* grab the next msb from the ltiplicand */
+     i = (buf >> (ltc_mp.bits_per_digit - 1)) & 1;
+     buf <<= 1;
+
+     /* skip leading zero bits */
+     if (mode == 0 && i == 0) {
+        continue;
+     }
+
+     /* if the bit is zero and mode == 1 then we double */
+     if (mode == 1 && i == 0) {
+        if ((err = ltc_mp.ecc_ptdbl(R, R, modulus, mp)) != CRYPT_OK)                 { goto done; }
+        continue;
+     }
+
+     /* else we add it to the window */
+     bitbuf |= (i << (WINSIZE - ++bitcpy));
+     mode = 2;
+
+     if (bitcpy == WINSIZE) {
+       /* if this is the first window we do a simple copy */
+       if (first == 1) {
+          /* R = kG [k = first window] */
+          if ((err = mp_copy(M[bitbuf-WINVARS]->x, R->x)) != CRYPT_OK)                     { goto done; }
+          if ((err = mp_copy(M[bitbuf-WINVARS]->y, R->y)) != CRYPT_OK)                     { goto done; }
+          if ((err = mp_copy(M[bitbuf-WINVARS]->z, R->z)) != CRYPT_OK)                     { goto done; }
+          first = 0;
+       } else {
+         /* normal window */
+         /* ok window is filled so double as required and add  */
+         /* double first */
+         for (j = 0; j < WINSIZE; j++) {
+           if ((err = ltc_mp.ecc_ptdbl(R, R, modulus, mp)) != CRYPT_OK)              { goto done; }
+         }
+
+         /* then add, bitbuf will be WINVARS..(2*WINVARS - 1) guaranteed */
+         if ((err = ltc_mp.ecc_ptadd(R, M[bitbuf-WINVARS], R, modulus, mp)) != CRYPT_OK)   { goto done; }
+       }
+       /* empty window and reset */
+       bitcpy = bitbuf = 0;
+       mode = 1;
+    }
+  }
+
+   /* if bits remain then double/add */
+   if (mode == 2 && bitcpy > 0) {
+     /* double then add */
+     for (j = 0; j < bitcpy; j++) {
+       /* only double if we have had at least one add first */
+       if (first == 0) {
+          if ((err = ltc_mp.ecc_ptdbl(R, R, modulus, mp)) != CRYPT_OK)              { goto done; }
+       }
+
+       bitbuf <<= 1;
+       if ((bitbuf & (1 << WINSIZE)) != 0) {
+         if (first == 1){
+            /* first add, so copy */
+            if ((err = mp_copy(tG->x, R->x)) != CRYPT_OK)                           { goto done; }
+            if ((err = mp_copy(tG->y, R->y)) != CRYPT_OK)                           { goto done; }
+            if ((err = mp_copy(tG->z, R->z)) != CRYPT_OK)                           { goto done; }
+            first = 0;
+         } else {
+            /* then add */
+            if ((err = ltc_mp.ecc_ptadd(R, tG, R, modulus, mp)) != CRYPT_OK)        { goto done; }
+         }
+       }
+     }
+   }
+
+   /* map R back from projective space */
+   if (map) {
+      err = ltc_ecc_map(R, modulus, mp);
+   } else {
+      err = CRYPT_OK;
+   }
+done:
+   if (mu != NULL) {
+      mp_clear(mu);
+   }
+   mp_montgomery_free(mp);
+   ltc_ecc_del_point(tG);
+   for (i = 0; i < WINVARS; i++) {
+       ltc_ecc_del_point(M[i]);
+   }
+   return err;
+}
+
+#endif
+
+#undef WINSIZE
+
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/pk/ecc/ltc_ecc_mulmod.c,v $ */
+/* $Revision: 1.26 $ */
+/* $Date: 2007/05/12 14:32:35 $ */
diff --git a/core/lib/libtomcrypt/src/pk/ecc/ltc_ecc_mulmod_timing.c b/core/lib/libtomcrypt/src/pk/ecc/ltc_ecc_mulmod_timing.c
new file mode 100644
index 0000000..38af150
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/ecc/ltc_ecc_mulmod_timing.c
@@ -0,0 +1,191 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+
+/* Implements ECC over Z/pZ for curve y^2 = x^3 - 3x + b
+ *
+ * All curves taken from NIST recommendation paper of July 1999
+ * Available at http://csrc.nist.gov/cryptval/dss.htm
+ */
+#include "tomcrypt.h"
+
+/**
+  @file ltc_ecc_mulmod_timing.c
+  ECC Crypto, Tom St Denis
+*/
+
+#ifdef LTC_MECC
+
+#ifdef LTC_ECC_TIMING_RESISTANT
+
+/**
+   Perform a point multiplication  (timing resistant)
+   @param k    The scalar to multiply by
+   @param G    The base point
+   @param R    [out] Destination for kG
+   @param modulus  The modulus of the field the ECC curve is in
+   @param map      Boolean whether to map back to affine or not (1==map, 0 == leave in projective)
+   @return CRYPT_OK on success
+*/
+int ltc_ecc_mulmod(void *k, ecc_point *G, ecc_point *R, void *modulus, int map)
+{
+   ecc_point *tG, *M[3];
+   int        i, j, err;
+   void       *mu, *mp;
+   ltc_mp_digit buf;
+   int        bitcnt, mode, digidx;
+
+   LTC_ARGCHK(k       != NULL);
+   LTC_ARGCHK(G       != NULL);
+   LTC_ARGCHK(R       != NULL);
+   LTC_ARGCHK(modulus != NULL);
+
+   /* init montgomery reduction */
+   if ((err = mp_montgomery_setup(modulus, &mp)) != CRYPT_OK) {
+      return err;
+   }
+   if ((err = mp_init(&mu)) != CRYPT_OK) {
+      mp_montgomery_free(mp);
+      return err;
+   }
+   if ((err = mp_montgomery_normalization(mu, modulus)) != CRYPT_OK) {
+      mp_clear(mu);
+      mp_montgomery_free(mp);
+      return err;
+   }
+
+  /* alloc ram for window temps */
+  for (i = 0; i < 3; i++) {
+      M[i] = ltc_ecc_new_point();
+      if (M[i] == NULL) {
+         for (j = 0; j < i; j++) {
+             ltc_ecc_del_point(M[j]);
+         }
+         mp_clear(mu);
+         mp_montgomery_free(mp);
+         return CRYPT_MEM;
+      }
+  }
+
+   /* make a copy of G incase R==G */
+   tG = ltc_ecc_new_point();
+   if (tG == NULL)                                                                   { err = CRYPT_MEM; goto done; }
+
+   /* tG = G  and convert to montgomery */
+   if ((err = mp_mulmod(G->x, mu, modulus, tG->x)) != CRYPT_OK)                      { goto done; }
+   if ((err = mp_mulmod(G->y, mu, modulus, tG->y)) != CRYPT_OK)                      { goto done; }
+   if ((err = mp_mulmod(G->z, mu, modulus, tG->z)) != CRYPT_OK)                      { goto done; }
+   mp_clear(mu);
+   mu = NULL;
+
+   /* calc the M tab */
+   /* M[0] == G */
+   if ((err = mp_copy(tG->x, M[0]->x)) != CRYPT_OK)                                  { goto done; }
+   if ((err = mp_copy(tG->y, M[0]->y)) != CRYPT_OK)                                  { goto done; }
+   if ((err = mp_copy(tG->z, M[0]->z)) != CRYPT_OK)                                  { goto done; }
+   /* M[1] == 2G */
+   if ((err = ltc_mp.ecc_ptdbl(tG, M[1], modulus, mp)) != CRYPT_OK)                  { goto done; }
+
+   /* setup sliding window */
+   mode   = 0;
+   bitcnt = 1;
+   buf    = 0;
+   digidx = mp_get_digit_count(k) - 1;
+
+   /* perform ops */
+   for (;;) {
+     /* grab next digit as required */
+      if (--bitcnt == 0) {
+         if (digidx == -1) {
+            break;
+         }
+         buf    = mp_get_digit(k, digidx);
+         bitcnt = (int) MP_DIGIT_BIT;
+         --digidx;
+      }
+
+      /* grab the next msb from the ltiplicand */
+      i = (buf >> (MP_DIGIT_BIT - 1)) & 1;
+      buf <<= 1;
+
+      if (mode == 0 && i == 0) {
+         /* dummy operations */
+         if ((err = ltc_mp.ecc_ptadd(M[0], M[1], M[2], modulus, mp)) != CRYPT_OK)    { goto done; }
+         if ((err = ltc_mp.ecc_ptdbl(M[1], M[2], modulus, mp)) != CRYPT_OK)          { goto done; }
+         continue;
+      }
+
+      if (mode == 0 && i == 1) {
+         mode = 1;
+         /* dummy operations */
+         if ((err = ltc_mp.ecc_ptadd(M[0], M[1], M[2], modulus, mp)) != CRYPT_OK)    { goto done; }
+         if ((err = ltc_mp.ecc_ptdbl(M[1], M[2], modulus, mp)) != CRYPT_OK)          { goto done; }
+         continue;
+      }
+
+      if ((err = ltc_mp.ecc_ptadd(M[0], M[1], M[i^1], modulus, mp)) != CRYPT_OK)     { goto done; }
+      if ((err = ltc_mp.ecc_ptdbl(M[i], M[i], modulus, mp)) != CRYPT_OK)             { goto done; }
+   }
+
+   /* copy result out */
+   if ((err = mp_copy(M[0]->x, R->x)) != CRYPT_OK)                                   { goto done; }
+   if ((err = mp_copy(M[0]->y, R->y)) != CRYPT_OK)                                   { goto done; }
+   if ((err = mp_copy(M[0]->z, R->z)) != CRYPT_OK)                                   { goto done; }
+
+   /* map R back from projective space */
+   if (map) {
+      err = ltc_ecc_map(R, modulus, mp);
+   } else {
+      err = CRYPT_OK;
+   }
+done:
+   if (mu != NULL) {
+      mp_clear(mu);
+   }
+   mp_montgomery_free(mp);
+   ltc_ecc_del_point(tG);
+   for (i = 0; i < 3; i++) {
+       ltc_ecc_del_point(M[i]);
+   }
+   return err;
+}
+
+#endif
+#endif
+/* $Source: /cvs/libtom/libtomcrypt/src/pk/ecc/ltc_ecc_mulmod_timing.c,v $ */
+/* $Revision: 1.13 $ */
+/* $Date: 2007/05/12 14:32:35 $ */
diff --git a/core/lib/libtomcrypt/src/pk/ecc/ltc_ecc_points.c b/core/lib/libtomcrypt/src/pk/ecc/ltc_ecc_points.c
new file mode 100644
index 0000000..1f4a002
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/ecc/ltc_ecc_points.c
@@ -0,0 +1,87 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+
+/* Implements ECC over Z/pZ for curve y^2 = x^3 - 3x + b
+ *
+ * All curves taken from NIST recommendation paper of July 1999
+ * Available at http://csrc.nist.gov/cryptval/dss.htm
+ */
+#include "tomcrypt.h"
+
+/**
+  @file ltc_ecc_points.c
+  ECC Crypto, Tom St Denis
+*/  
+
+#ifdef LTC_MECC
+
+/**
+   Allocate a new ECC point
+   @return A newly allocated point or NULL on error 
+*/
+ecc_point *ltc_ecc_new_point(void)
+{
+   ecc_point *p;
+   p = XCALLOC(1, sizeof(*p));
+   if (p == NULL) {
+      return NULL;
+   }
+   if (mp_init_multi_size(LTC_MAX_ECC * 2,
+			  &p->x, &p->y, &p->z, NULL) != CRYPT_OK) {
+      XFREE(p);
+      return NULL;
+   }
+   return p;
+}
+
+/** Free an ECC point from memory
+  @param p   The point to free
+*/
+void ltc_ecc_del_point(ecc_point *p)
+{
+   /* prevents free'ing null arguments */
+   if (p != NULL) {
+      mp_clear_multi(p->x, p->y, p->z, NULL); /* note: p->z may be NULL but that's ok with this function anyways */
+      XFREE(p);
+   }
+}
+
+#endif
+/* $Source: /cvs/libtom/libtomcrypt/src/pk/ecc/ltc_ecc_points.c,v $ */
+/* $Revision: 1.7 $ */
+/* $Date: 2007/05/12 14:32:35 $ */
diff --git a/core/lib/libtomcrypt/src/pk/ecc/ltc_ecc_projective_add_point.c b/core/lib/libtomcrypt/src/pk/ecc/ltc_ecc_projective_add_point.c
new file mode 100644
index 0000000..7c89a89
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/ecc/ltc_ecc_projective_add_point.c
@@ -0,0 +1,222 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+
+/* Implements ECC over Z/pZ for curve y^2 = x^3 - 3x + b
+ *
+ * All curves taken from NIST recommendation paper of July 1999
+ * Available at http://csrc.nist.gov/cryptval/dss.htm
+ */
+#include "tomcrypt.h"
+
+/**
+  @file ltc_ecc_projective_add_point.c
+  ECC Crypto, Tom St Denis
+*/
+
+#if defined(LTC_MECC) && (!defined(LTC_MECC_ACCEL) || defined(LTM_DESC))
+
+/**
+   Add two ECC points
+   @param P        The point to add
+   @param Q        The point to add
+   @param R        [out] The destination of the double
+   @param modulus  The modulus of the field the ECC curve is in
+   @param mp       The "b" value from montgomery_setup()
+   @return CRYPT_OK on success
+*/
+int ltc_ecc_projective_add_point(ecc_point *P, ecc_point *Q, ecc_point *R, void *modulus, void *mp)
+{
+   void  *t1, *t2, *x, *y, *z;
+   int    err;
+
+   LTC_ARGCHK(P       != NULL);
+   LTC_ARGCHK(Q       != NULL);
+   LTC_ARGCHK(R       != NULL);
+   LTC_ARGCHK(modulus != NULL);
+   LTC_ARGCHK(mp      != NULL);
+
+   if ((err = mp_init_multi(&t1, &t2, &x, &y, &z, NULL)) != CRYPT_OK) {
+      return err;
+   }
+
+   /* should we dbl instead? */
+   if ((err = mp_sub(modulus, Q->y, t1)) != CRYPT_OK)                          { goto done; }
+
+   if ( (mp_cmp(P->x, Q->x) == LTC_MP_EQ) &&
+        (Q->z != NULL && mp_cmp(P->z, Q->z) == LTC_MP_EQ) &&
+        (mp_cmp(P->y, Q->y) == LTC_MP_EQ || mp_cmp(P->y, t1) == LTC_MP_EQ)) {
+        mp_clear_multi(t1, t2, x, y, z, NULL);
+        return ltc_ecc_projective_dbl_point(P, R, modulus, mp);
+   }
+
+   if ((err = mp_copy(P->x, x)) != CRYPT_OK)                                   { goto done; }
+   if ((err = mp_copy(P->y, y)) != CRYPT_OK)                                   { goto done; }
+   if ((err = mp_copy(P->z, z)) != CRYPT_OK)                                   { goto done; }
+
+   /* if Z is one then these are no-operations */
+   if (Q->z != NULL) {
+      /* T1 = Z' * Z' */
+      if ((err = mp_sqr(Q->z, t1)) != CRYPT_OK)                                { goto done; }
+      if ((err = mp_montgomery_reduce(t1, modulus, mp)) != CRYPT_OK)           { goto done; }
+      /* X = X * T1 */
+      if ((err = mp_mul(t1, x, x)) != CRYPT_OK)                                { goto done; }
+      if ((err = mp_montgomery_reduce(x, modulus, mp)) != CRYPT_OK)            { goto done; }
+      /* T1 = Z' * T1 */
+      if ((err = mp_mul(Q->z, t1, t1)) != CRYPT_OK)                            { goto done; }
+      if ((err = mp_montgomery_reduce(t1, modulus, mp)) != CRYPT_OK)           { goto done; }
+      /* Y = Y * T1 */
+      if ((err = mp_mul(t1, y, y)) != CRYPT_OK)                                { goto done; }
+      if ((err = mp_montgomery_reduce(y, modulus, mp)) != CRYPT_OK)            { goto done; }
+   }
+
+   /* T1 = Z*Z */
+   if ((err = mp_sqr(z, t1)) != CRYPT_OK)                                      { goto done; }
+   if ((err = mp_montgomery_reduce(t1, modulus, mp)) != CRYPT_OK)              { goto done; }
+   /* T2 = X' * T1 */
+   if ((err = mp_mul(Q->x, t1, t2)) != CRYPT_OK)                               { goto done; }
+   if ((err = mp_montgomery_reduce(t2, modulus, mp)) != CRYPT_OK)              { goto done; }
+   /* T1 = Z * T1 */
+   if ((err = mp_mul(z, t1, t1)) != CRYPT_OK)                                  { goto done; }
+   if ((err = mp_montgomery_reduce(t1, modulus, mp)) != CRYPT_OK)              { goto done; }
+   /* T1 = Y' * T1 */
+   if ((err = mp_mul(Q->y, t1, t1)) != CRYPT_OK)                               { goto done; }
+   if ((err = mp_montgomery_reduce(t1, modulus, mp)) != CRYPT_OK)              { goto done; }
+
+   /* Y = Y - T1 */
+   if ((err = mp_sub(y, t1, y)) != CRYPT_OK)                                   { goto done; }
+   if (mp_cmp_d(y, 0) == LTC_MP_LT) {
+      if ((err = mp_add(y, modulus, y)) != CRYPT_OK)                           { goto done; }
+   }
+   /* T1 = 2T1 */
+   if ((err = mp_add(t1, t1, t1)) != CRYPT_OK)                                 { goto done; }
+   if (mp_cmp(t1, modulus) != LTC_MP_LT) {
+      if ((err = mp_sub(t1, modulus, t1)) != CRYPT_OK)                         { goto done; }
+   }
+   /* T1 = Y + T1 */
+   if ((err = mp_add(t1, y, t1)) != CRYPT_OK)                                  { goto done; }
+   if (mp_cmp(t1, modulus) != LTC_MP_LT) {
+      if ((err = mp_sub(t1, modulus, t1)) != CRYPT_OK)                         { goto done; }
+   }
+   /* X = X - T2 */
+   if ((err = mp_sub(x, t2, x)) != CRYPT_OK)                                   { goto done; }
+   if (mp_cmp_d(x, 0) == LTC_MP_LT) {
+      if ((err = mp_add(x, modulus, x)) != CRYPT_OK)                           { goto done; }
+   }
+   /* T2 = 2T2 */
+   if ((err = mp_add(t2, t2, t2)) != CRYPT_OK)                                 { goto done; }
+   if (mp_cmp(t2, modulus) != LTC_MP_LT) {
+      if ((err = mp_sub(t2, modulus, t2)) != CRYPT_OK)                         { goto done; }
+   }
+   /* T2 = X + T2 */
+   if ((err = mp_add(t2, x, t2)) != CRYPT_OK)                                  { goto done; }
+   if (mp_cmp(t2, modulus) != LTC_MP_LT) {
+      if ((err = mp_sub(t2, modulus, t2)) != CRYPT_OK)                         { goto done; }
+   }
+
+   /* if Z' != 1 */
+   if (Q->z != NULL) {
+      /* Z = Z * Z' */
+      if ((err = mp_mul(z, Q->z, z)) != CRYPT_OK)                              { goto done; }
+      if ((err = mp_montgomery_reduce(z, modulus, mp)) != CRYPT_OK)            { goto done; }
+   }
+
+   /* Z = Z * X */
+   if ((err = mp_mul(z, x, z)) != CRYPT_OK)                                    { goto done; }
+   if ((err = mp_montgomery_reduce(z, modulus, mp)) != CRYPT_OK)               { goto done; }
+
+   /* T1 = T1 * X  */
+   if ((err = mp_mul(t1, x, t1)) != CRYPT_OK)                                  { goto done; }
+   if ((err = mp_montgomery_reduce(t1, modulus, mp)) != CRYPT_OK)              { goto done; }
+   /* X = X * X */
+   if ((err = mp_sqr(x, x)) != CRYPT_OK)                                       { goto done; }
+   if ((err = mp_montgomery_reduce(x, modulus, mp)) != CRYPT_OK)               { goto done; }
+   /* T2 = T2 * x */
+   if ((err = mp_mul(t2, x, t2)) != CRYPT_OK)                                  { goto done; }
+   if ((err = mp_montgomery_reduce(t2, modulus, mp)) != CRYPT_OK)              { goto done; }
+   /* T1 = T1 * X  */
+   if ((err = mp_mul(t1, x, t1)) != CRYPT_OK)                                  { goto done; }
+   if ((err = mp_montgomery_reduce(t1, modulus, mp)) != CRYPT_OK)              { goto done; }
+
+   /* X = Y*Y */
+   if ((err = mp_sqr(y, x)) != CRYPT_OK)                                       { goto done; }
+   if ((err = mp_montgomery_reduce(x, modulus, mp)) != CRYPT_OK)               { goto done; }
+   /* X = X - T2 */
+   if ((err = mp_sub(x, t2, x)) != CRYPT_OK)                                   { goto done; }
+   if (mp_cmp_d(x, 0) == LTC_MP_LT) {
+      if ((err = mp_add(x, modulus, x)) != CRYPT_OK)                           { goto done; }
+   }
+
+   /* T2 = T2 - X */
+   if ((err = mp_sub(t2, x, t2)) != CRYPT_OK)                                  { goto done; }
+   if (mp_cmp_d(t2, 0) == LTC_MP_LT) {
+      if ((err = mp_add(t2, modulus, t2)) != CRYPT_OK)                         { goto done; }
+   }
+   /* T2 = T2 - X */
+   if ((err = mp_sub(t2, x, t2)) != CRYPT_OK)                                  { goto done; }
+   if (mp_cmp_d(t2, 0) == LTC_MP_LT) {
+      if ((err = mp_add(t2, modulus, t2)) != CRYPT_OK)                         { goto done; }
+   }
+   /* T2 = T2 * Y */
+   if ((err = mp_mul(t2, y, t2)) != CRYPT_OK)                                  { goto done; }
+   if ((err = mp_montgomery_reduce(t2, modulus, mp)) != CRYPT_OK)              { goto done; }
+   /* Y = T2 - T1 */
+   if ((err = mp_sub(t2, t1, y)) != CRYPT_OK)                                  { goto done; }
+   if (mp_cmp_d(y, 0) == LTC_MP_LT) {
+      if ((err = mp_add(y, modulus, y)) != CRYPT_OK)                           { goto done; }
+   }
+   /* Y = Y/2 */
+   if (mp_isodd(y)) {
+      if ((err = mp_add(y, modulus, y)) != CRYPT_OK)                           { goto done; }
+   }
+   if ((err = mp_div_2(y, y)) != CRYPT_OK)                                     { goto done; }
+
+   if ((err = mp_copy(x, R->x)) != CRYPT_OK)                                   { goto done; }
+   if ((err = mp_copy(y, R->y)) != CRYPT_OK)                                   { goto done; }
+   if ((err = mp_copy(z, R->z)) != CRYPT_OK)                                   { goto done; }
+
+   err = CRYPT_OK;
+done:
+   mp_clear_multi(t1, t2, x, y, z, NULL);
+   return err;
+}
+
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/pk/ecc/ltc_ecc_projective_add_point.c,v $ */
+/* $Revision: 1.16 $ */
+/* $Date: 2007/05/12 14:32:35 $ */
diff --git a/core/lib/libtomcrypt/src/pk/ecc/ltc_ecc_projective_dbl_point.c b/core/lib/libtomcrypt/src/pk/ecc/ltc_ecc_projective_dbl_point.c
new file mode 100644
index 0000000..6cd0da9
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/ecc/ltc_ecc_projective_dbl_point.c
@@ -0,0 +1,173 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+
+/* Implements ECC over Z/pZ for curve y^2 = x^3 - 3x + b
+ *
+ * All curves taken from NIST recommendation paper of July 1999
+ * Available at http://csrc.nist.gov/cryptval/dss.htm
+ */
+#include "tomcrypt.h"
+
+/**
+  @file ltc_ecc_projective_dbl_point.c
+  ECC Crypto, Tom St Denis
+*/
+
+#if defined(LTC_MECC) && (!defined(LTC_MECC_ACCEL) || defined(LTM_DESC))
+
+/**
+   Double an ECC point
+   @param P   The point to double
+   @param R   [out] The destination of the double
+   @param modulus  The modulus of the field the ECC curve is in
+   @param mp       The "b" value from montgomery_setup()
+   @return CRYPT_OK on success
+*/
+int ltc_ecc_projective_dbl_point(ecc_point *P, ecc_point *R, void *modulus, void *mp)
+{
+   void *t1, *t2;
+   int   err;
+
+   LTC_ARGCHK(P       != NULL);
+   LTC_ARGCHK(R       != NULL);
+   LTC_ARGCHK(modulus != NULL);
+   LTC_ARGCHK(mp      != NULL);
+
+   if ((err = mp_init_multi(&t1, &t2, NULL)) != CRYPT_OK) {
+      return err;
+   }
+
+   if (P != R) {
+      if ((err = mp_copy(P->x, R->x)) != CRYPT_OK)                                { goto done; }
+      if ((err = mp_copy(P->y, R->y)) != CRYPT_OK)                                { goto done; }
+      if ((err = mp_copy(P->z, R->z)) != CRYPT_OK)                                { goto done; }
+   }
+
+   /* t1 = Z * Z */
+   if ((err = mp_sqr(R->z, t1)) != CRYPT_OK)                                      { goto done; }
+   if ((err = mp_montgomery_reduce(t1, modulus, mp)) != CRYPT_OK)                 { goto done; }
+   /* Z = Y * Z */
+   if ((err = mp_mul(R->z, R->y, R->z)) != CRYPT_OK)                              { goto done; }
+   if ((err = mp_montgomery_reduce(R->z, modulus, mp)) != CRYPT_OK)               { goto done; }
+   /* Z = 2Z */
+   if ((err = mp_add(R->z, R->z, R->z)) != CRYPT_OK)                              { goto done; }
+   if (mp_cmp(R->z, modulus) != LTC_MP_LT) {
+      if ((err = mp_sub(R->z, modulus, R->z)) != CRYPT_OK)                        { goto done; }
+   }
+
+   /* T2 = X - T1 */
+   if ((err = mp_sub(R->x, t1, t2)) != CRYPT_OK)                                  { goto done; }
+   if (mp_cmp_d(t2, 0) == LTC_MP_LT) {
+      if ((err = mp_add(t2, modulus, t2)) != CRYPT_OK)                            { goto done; }
+   }
+   /* T1 = X + T1 */
+   if ((err = mp_add(t1, R->x, t1)) != CRYPT_OK)                                  { goto done; }
+   if (mp_cmp(t1, modulus) != LTC_MP_LT) {
+      if ((err = mp_sub(t1, modulus, t1)) != CRYPT_OK)                            { goto done; }
+   }
+   /* T2 = T1 * T2 */
+   if ((err = mp_mul(t1, t2, t2)) != CRYPT_OK)                                    { goto done; }
+   if ((err = mp_montgomery_reduce(t2, modulus, mp)) != CRYPT_OK)                 { goto done; }
+   /* T1 = 2T2 */
+   if ((err = mp_add(t2, t2, t1)) != CRYPT_OK)                                    { goto done; }
+   if (mp_cmp(t1, modulus) != LTC_MP_LT) {
+      if ((err = mp_sub(t1, modulus, t1)) != CRYPT_OK)                            { goto done; }
+   }
+   /* T1 = T1 + T2 */
+   if ((err = mp_add(t1, t2, t1)) != CRYPT_OK)                                    { goto done; }
+   if (mp_cmp(t1, modulus) != LTC_MP_LT) {
+      if ((err = mp_sub(t1, modulus, t1)) != CRYPT_OK)                            { goto done; }
+   }
+
+   /* Y = 2Y */
+   if ((err = mp_add(R->y, R->y, R->y)) != CRYPT_OK)                              { goto done; }
+   if (mp_cmp(R->y, modulus) != LTC_MP_LT) {
+      if ((err = mp_sub(R->y, modulus, R->y)) != CRYPT_OK)                        { goto done; }
+   }
+   /* Y = Y * Y */
+   if ((err = mp_sqr(R->y, R->y)) != CRYPT_OK)                                    { goto done; }
+   if ((err = mp_montgomery_reduce(R->y, modulus, mp)) != CRYPT_OK)               { goto done; }
+   /* T2 = Y * Y */
+   if ((err = mp_sqr(R->y, t2)) != CRYPT_OK)                                      { goto done; }
+   if ((err = mp_montgomery_reduce(t2, modulus, mp)) != CRYPT_OK)                 { goto done; }
+   /* T2 = T2/2 */
+   if (mp_isodd(t2)) {
+      if ((err = mp_add(t2, modulus, t2)) != CRYPT_OK)                            { goto done; }
+   }
+   if ((err = mp_div_2(t2, t2)) != CRYPT_OK)                                      { goto done; }
+   /* Y = Y * X */
+   if ((err = mp_mul(R->y, R->x, R->y)) != CRYPT_OK)                              { goto done; }
+   if ((err = mp_montgomery_reduce(R->y, modulus, mp)) != CRYPT_OK)               { goto done; }
+
+   /* X  = T1 * T1 */
+   if ((err = mp_sqr(t1, R->x)) != CRYPT_OK)                                      { goto done; }
+   if ((err = mp_montgomery_reduce(R->x, modulus, mp)) != CRYPT_OK)               { goto done; }
+   /* X = X - Y */
+   if ((err = mp_sub(R->x, R->y, R->x)) != CRYPT_OK)                              { goto done; }
+   if (mp_cmp_d(R->x, 0) == LTC_MP_LT) {
+      if ((err = mp_add(R->x, modulus, R->x)) != CRYPT_OK)                        { goto done; }
+   }
+   /* X = X - Y */
+   if ((err = mp_sub(R->x, R->y, R->x)) != CRYPT_OK)                              { goto done; }
+   if (mp_cmp_d(R->x, 0) == LTC_MP_LT) {
+      if ((err = mp_add(R->x, modulus, R->x)) != CRYPT_OK)                        { goto done; }
+   }
+
+   /* Y = Y - X */
+   if ((err = mp_sub(R->y, R->x, R->y)) != CRYPT_OK)                              { goto done; }
+   if (mp_cmp_d(R->y, 0) == LTC_MP_LT) {
+      if ((err = mp_add(R->y, modulus, R->y)) != CRYPT_OK)                        { goto done; }
+   }
+   /* Y = Y * T1 */
+   if ((err = mp_mul(R->y, t1, R->y)) != CRYPT_OK)                                { goto done; }
+   if ((err = mp_montgomery_reduce(R->y, modulus, mp)) != CRYPT_OK)               { goto done; }
+   /* Y = Y - T2 */
+   if ((err = mp_sub(R->y, t2, R->y)) != CRYPT_OK)                                { goto done; }
+   if (mp_cmp_d(R->y, 0) == LTC_MP_LT) {
+      if ((err = mp_add(R->y, modulus, R->y)) != CRYPT_OK)                        { goto done; }
+   }
+
+   err = CRYPT_OK;
+done:
+   mp_clear_multi(t1, t2, NULL);
+   return err;
+}
+#endif
+/* $Source: /cvs/libtom/libtomcrypt/src/pk/ecc/ltc_ecc_projective_dbl_point.c,v $ */
+/* $Revision: 1.11 $ */
+/* $Date: 2007/05/12 14:32:35 $ */
diff --git a/core/lib/libtomcrypt/src/pk/ecc/sub.mk b/core/lib/libtomcrypt/src/pk/ecc/sub.mk
new file mode 100644
index 0000000..6d38dbc
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/ecc/sub.mk
@@ -0,0 +1,14 @@
+srcs-y += ecc.c
+srcs-y += ecc_free.c
+srcs-y += ecc_make_key.c
+srcs-y += ecc_shared_secret.c
+srcs-y += ecc_sign_hash.c
+srcs-y += ecc_verify_hash.c
+srcs-y += ltc_ecc_is_valid_idx.c
+srcs-y += ltc_ecc_map.c
+srcs-y += ltc_ecc_mulmod.c
+srcs-y += ltc_ecc_mulmod_timing.c
+srcs-y += ltc_ecc_mul2add.c
+srcs-y += ltc_ecc_points.c
+srcs-y += ltc_ecc_projective_add_point.c
+srcs-y += ltc_ecc_projective_dbl_point.c
diff --git a/core/lib/libtomcrypt/src/pk/pkcs1/pkcs_1_i2osp.c b/core/lib/libtomcrypt/src/pk/pkcs1/pkcs_1_i2osp.c
new file mode 100644
index 0000000..5e40bcb
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/pkcs1/pkcs_1_i2osp.c
@@ -0,0 +1,78 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/** 
+  @file pkcs_1_i2osp.c
+  Integer to Octet I2OSP, Tom St Denis 
+*/
+
+#ifdef LTC_PKCS_1
+
+/* always stores the same # of bytes, pads with leading zero bytes
+   as required
+ */
+
+/**
+   LTC_PKCS #1 Integer to binary
+   @param n             The integer to store
+   @param modulus_len   The length of the RSA modulus
+   @param out           [out] The destination for the integer
+   @return CRYPT_OK if successful
+*/
+int pkcs_1_i2osp(void *n, unsigned long modulus_len, unsigned char *out)
+{
+   unsigned long size;
+
+   size = mp_unsigned_bin_size(n);
+
+   if (size > modulus_len) {
+      return CRYPT_BUFFER_OVERFLOW;
+   }
+
+   /* store it */
+   zeromem(out, modulus_len);
+   return mp_to_unsigned_bin(n, out+(modulus_len-size));
+}
+
+#endif /* LTC_PKCS_1 */
+
+
+/* $Source: /cvs/libtom/libtomcrypt/src/pk/pkcs1/pkcs_1_i2osp.c,v $ */
+/* $Revision: 1.7 $ */
+/* $Date: 2007/05/12 14:32:35 $ */
diff --git a/core/lib/libtomcrypt/src/pk/pkcs1/pkcs_1_mgf1.c b/core/lib/libtomcrypt/src/pk/pkcs1/pkcs_1_mgf1.c
new file mode 100644
index 0000000..393e146
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/pkcs1/pkcs_1_mgf1.c
@@ -0,0 +1,135 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/** 
+  @file pkcs_1_mgf1.c
+  The Mask Generation Function (MGF1) for PKCS #1, Tom St Denis
+*/
+
+#ifdef LTC_PKCS_1
+
+/**
+   Perform PKCS #1 MGF1 (internal)
+   @param seed        The seed for MGF1
+   @param seedlen     The length of the seed
+   @param hash_idx    The index of the hash desired
+   @param mask        [out] The destination
+   @param masklen     The length of the mask desired
+   @return CRYPT_OK if successful
+*/
+int pkcs_1_mgf1(int                  hash_idx,
+                const unsigned char *seed, unsigned long seedlen,
+                      unsigned char *mask, unsigned long masklen)
+{
+   unsigned long hLen, x;
+   ulong32       counter;
+   int           err;
+   hash_state    *md;
+   unsigned char *buf;
+ 
+   LTC_ARGCHK(seed != NULL);
+   LTC_ARGCHK(mask != NULL);
+
+   /* ensure valid hash */
+   if ((err = hash_is_valid(hash_idx)) != CRYPT_OK) { 
+      return err;
+   }
+
+   /* get hash output size */
+   hLen = hash_descriptor[hash_idx]->hashsize;
+
+   /* allocate memory */
+   md  = XMALLOC(sizeof(hash_state));
+   buf = XMALLOC(hLen);
+   if (md == NULL || buf == NULL) {
+      if (md != NULL) {
+         XFREE(md);
+      }
+      if (buf != NULL) {
+         XFREE(buf);
+      }
+      return CRYPT_MEM;
+   }
+
+   /* start counter */
+   counter = 0;
+
+   while (masklen > 0) {
+       /* handle counter */
+       STORE32H(counter, buf);
+       ++counter;
+
+       /* get hash of seed || counter */
+       if ((err = hash_descriptor[hash_idx]->init(md)) != CRYPT_OK) {
+          goto LBL_ERR;
+       }
+       if ((err = hash_descriptor[hash_idx]->process(md, seed, seedlen)) != CRYPT_OK) {
+          goto LBL_ERR;
+       }
+       if ((err = hash_descriptor[hash_idx]->process(md, buf, 4)) != CRYPT_OK) {
+          goto LBL_ERR;
+       }
+       if ((err = hash_descriptor[hash_idx]->done(md, buf)) != CRYPT_OK) {
+          goto LBL_ERR;
+       }
+
+       /* store it */
+       for (x = 0; x < hLen && masklen > 0; x++, masklen--) {
+          *mask++ = buf[x];
+       }
+   }
+
+   err = CRYPT_OK;
+LBL_ERR:
+#ifdef LTC_CLEAN_STACK
+   zeromem(buf, hLen);
+   zeromem(md,  sizeof(hash_state));
+#endif
+
+   XFREE(buf);
+   XFREE(md);
+
+   return err;
+}
+
+#endif /* LTC_PKCS_1 */
+
+/* $Source: /cvs/libtom/libtomcrypt/src/pk/pkcs1/pkcs_1_mgf1.c,v $ */
+/* $Revision: 1.8 $ */
+/* $Date: 2007/05/12 14:32:35 $ */
diff --git a/core/lib/libtomcrypt/src/pk/pkcs1/pkcs_1_oaep_decode.c b/core/lib/libtomcrypt/src/pk/pkcs1/pkcs_1_oaep_decode.c
new file mode 100644
index 0000000..f5e1d98
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/pkcs1/pkcs_1_oaep_decode.c
@@ -0,0 +1,215 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/** 
+  @file pkcs_1_oaep_decode.c
+  OAEP Padding for PKCS #1, Tom St Denis
+*/
+
+#ifdef LTC_PKCS_1
+
+/**
+   PKCS #1 v2.00 OAEP decode
+   @param msg              The encoded data to decode
+   @param msglen           The length of the encoded data (octets)
+   @param lparam           The session or system data (can be NULL)
+   @param lparamlen        The length of the lparam
+   @param modulus_bitlen   The bit length of the RSA modulus
+   @param hash_idx         The index of the hash desired
+   @param out              [out] Destination of decoding
+   @param outlen           [in/out] The max size and resulting size of the decoding
+   @param res              [out] Result of decoding, 1==valid, 0==invalid
+   @return CRYPT_OK if successful
+*/
+int pkcs_1_oaep_decode(const unsigned char *msg,    unsigned long msglen,
+                       const unsigned char *lparam, unsigned long lparamlen,
+                             unsigned long modulus_bitlen, int hash_idx,
+                             unsigned char *out,    unsigned long *outlen,
+                             int           *res)
+{
+   unsigned char *DB, *seed, *mask;
+   unsigned long hLen, x, y, modulus_len;
+   int           err, ret;
+
+   LTC_ARGCHK(msg    != NULL);
+   LTC_ARGCHK(out    != NULL);
+   LTC_ARGCHK(outlen != NULL);
+   LTC_ARGCHK(res    != NULL);
+
+   /* default to invalid packet */
+   *res = 0;
+   
+   /* test valid hash */
+   if ((err = hash_is_valid(hash_idx)) != CRYPT_OK) { 
+      return err;
+   }
+   hLen        = hash_descriptor[hash_idx]->hashsize;
+   modulus_len = (modulus_bitlen >> 3) + (modulus_bitlen & 7 ? 1 : 0);
+
+   /* test hash/message size */
+   if ((2*hLen >= (modulus_len - 2)) || (msglen != modulus_len)) {
+      return CRYPT_PK_INVALID_SIZE;
+   }
+
+   /* allocate ram for DB/mask/salt of size modulus_len */
+   DB   = XMALLOC(modulus_len);
+   mask = XMALLOC(modulus_len);
+   seed = XMALLOC(hLen);
+   if (DB == NULL || mask == NULL || seed == NULL) {
+      if (DB != NULL) {
+         XFREE(DB);
+      }
+      if (mask != NULL) {
+         XFREE(mask);
+      }
+      if (seed != NULL) {
+         XFREE(seed);
+      }
+      return CRYPT_MEM;
+   }
+
+   /* ok so it's now in the form
+  
+      0x00  || maskedseed || maskedDB 
+  
+       1    ||   hLen     ||  modulus_len - hLen - 1
+   
+    */
+
+   ret = CRYPT_OK;
+
+   /* must have leading 0x00 byte */
+   if (msg[0] != 0x00) {
+      ret = CRYPT_INVALID_PACKET;
+   }
+
+   /* now read the masked seed */
+   x = 1;
+   XMEMCPY(seed, msg + x, hLen);
+   x += hLen;
+
+   /* now read the masked DB */
+   XMEMCPY(DB, msg + x, modulus_len - hLen - 1);
+   x += modulus_len - hLen - 1;
+
+   /* compute MGF1 of maskedDB (hLen) */ 
+   if ((err = pkcs_1_mgf1(hash_idx, DB, modulus_len - hLen - 1, mask, hLen)) != CRYPT_OK) {
+      goto LBL_ERR;
+   }
+
+   /* XOR against seed */
+   for (y = 0; y < hLen; y++) {
+      seed[y] ^= mask[y];
+   }
+
+   /* compute MGF1 of seed (k - hlen - 1) */
+   if ((err = pkcs_1_mgf1(hash_idx, seed, hLen, mask, modulus_len - hLen - 1)) != CRYPT_OK) {
+      goto LBL_ERR;
+   }
+
+   /* xor against DB */
+   for (y = 0; y < (modulus_len - hLen - 1); y++) {
+       DB[y] ^= mask[y]; 
+   }
+
+   /* now DB == lhash || PS || 0x01 || M, PS == k - mlen - 2hlen - 2 zeroes */
+
+   /* compute lhash and store it in seed [reuse temps!] */
+   x = modulus_len;
+   if (lparam != NULL) {
+      if ((err = hash_memory(hash_idx, lparam, lparamlen, seed, &x)) != CRYPT_OK) {
+         goto LBL_ERR;
+      }
+   } else {
+      /* can't pass hash_memory a NULL so use DB with zero length */
+      if ((err = hash_memory(hash_idx, DB, 0, seed, &x)) != CRYPT_OK) {
+         goto LBL_ERR;
+      }
+   }
+
+   /* compare the lhash'es */
+   if (XMEM_NEQ(seed, DB, hLen) != 0) {
+      ret = CRYPT_INVALID_PACKET;
+   }
+
+   /* now zeroes before a 0x01 */
+   for (x = hLen; x < (modulus_len - hLen - 1) && DB[x] == 0x00; x++) {
+      /* step... */
+   }
+
+   /* error if wasn't 0x01 */
+   if (x == (modulus_len - hLen - 1) || DB[x] != 0x01) {
+      ret = CRYPT_INVALID_PACKET;
+   }
+
+   /* rest is the message (and skip 0x01) */
+   if ((modulus_len - hLen - 1 - ++x) > *outlen) {
+      ret = CRYPT_INVALID_PACKET;
+   }
+
+   if (ret == CRYPT_OK) {
+      /* copy message */
+      *outlen = modulus_len - hLen - 1 - x;
+      XMEMCPY(out, DB + x, modulus_len - hLen - 1 - x);
+      x += modulus_len - hLen - 1;
+
+      /* valid packet */
+      *res = 1;
+   }
+   err = ret;
+
+LBL_ERR:
+#ifdef LTC_CLEAN_STACK
+   zeromem(DB,   modulus_len);
+   zeromem(seed, hLen);
+   zeromem(mask, modulus_len);
+#endif
+
+   XFREE(seed);
+   XFREE(mask);
+   XFREE(DB);
+
+   return err;
+}
+
+#endif /* LTC_PKCS_1 */
+
+/* $Source: /cvs/libtom/libtomcrypt/src/pk/pkcs1/pkcs_1_oaep_decode.c,v $ */
+/* $Revision: 1.13 $ */
+/* $Date: 2007/05/12 14:32:35 $ */
diff --git a/core/lib/libtomcrypt/src/pk/pkcs1/pkcs_1_oaep_encode.c b/core/lib/libtomcrypt/src/pk/pkcs1/pkcs_1_oaep_encode.c
new file mode 100644
index 0000000..6bd3e1f
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/pkcs1/pkcs_1_oaep_encode.c
@@ -0,0 +1,200 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file pkcs_1_oaep_encode.c
+  OAEP Padding for PKCS #1, Tom St Denis
+*/
+
+#ifdef LTC_PKCS_1
+
+/**
+  PKCS #1 v2.00 OAEP encode
+  @param msg             The data to encode
+  @param msglen          The length of the data to encode (octets)
+  @param lparam          A session or system parameter (can be NULL)
+  @param lparamlen       The length of the lparam data
+  @param modulus_bitlen  The bit length of the RSA modulus
+  @param prng            An active PRNG state
+  @param prng_idx        The index of the PRNG desired
+  @param hash_idx        The index of the hash desired
+  @param out             [out] The destination for the encoded data
+  @param outlen          [in/out] The max size and resulting size of the encoded data
+  @return CRYPT_OK if successful
+*/
+int pkcs_1_oaep_encode(const unsigned char *msg,    unsigned long msglen,
+                       const unsigned char *lparam, unsigned long lparamlen,
+                             unsigned long modulus_bitlen, prng_state *prng,
+                             int           prng_idx,         int  hash_idx,
+                             unsigned char *out,    unsigned long *outlen)
+{
+   unsigned char *DB, *seed, *mask;
+   unsigned long hLen, x, y, modulus_len;
+   int           err;
+
+   LTC_ARGCHK(msg    != NULL);
+   LTC_ARGCHK(out    != NULL);
+   LTC_ARGCHK(outlen != NULL);
+
+   /* test valid hash */
+   if ((err = hash_is_valid(hash_idx)) != CRYPT_OK) { 
+      return err;
+   }
+
+   /* valid prng */
+   if ((err = prng_is_valid(prng_idx)) != CRYPT_OK) {
+      return err;
+   }
+
+   hLen        = hash_descriptor[hash_idx]->hashsize;
+   modulus_len = (modulus_bitlen >> 3) + (modulus_bitlen & 7 ? 1 : 0);
+
+   /* test message size */
+   if ((2*hLen >= (modulus_len - 2)) || (msglen > (modulus_len - 2*hLen - 2))) {
+      return CRYPT_PK_INVALID_SIZE;
+   }
+
+   /* allocate ram for DB/mask/salt of size modulus_len */
+   DB   = XMALLOC(modulus_len);
+   mask = XMALLOC(modulus_len);
+   seed = XMALLOC(hLen);
+   if (DB == NULL || mask == NULL || seed == NULL) {
+      if (DB != NULL) {
+         XFREE(DB);
+      }
+      if (mask != NULL) {
+         XFREE(mask);
+      }
+      if (seed != NULL) {
+         XFREE(seed);
+      }
+      return CRYPT_MEM;
+   }
+
+   /* get lhash */
+   /* DB == lhash || PS || 0x01 || M, PS == k - mlen - 2hlen - 2 zeroes */
+   x = modulus_len;
+   if (lparam != NULL) {
+      if ((err = hash_memory(hash_idx, lparam, lparamlen, DB, &x)) != CRYPT_OK) {
+         goto LBL_ERR;
+      }
+   } else {
+      /* can't pass hash_memory a NULL so use DB with zero length */
+      if ((err = hash_memory(hash_idx, DB, 0, DB, &x)) != CRYPT_OK) {
+         goto LBL_ERR;
+      }
+   }
+
+   /* append PS then 0x01 (to lhash)  */
+   x = hLen;
+   y = modulus_len - msglen - 2*hLen - 2;
+   XMEMSET(DB+x, 0, y);
+   x += y;
+
+   /* 0x01 byte */
+   DB[x++] = 0x01;
+
+   /* message (length = msglen) */
+   XMEMCPY(DB+x, msg, msglen);
+   x += msglen;
+
+   /* now choose a random seed */
+   if (prng_descriptor[prng_idx]->read(seed, hLen, prng) != hLen) {
+      err = CRYPT_ERROR_READPRNG;
+      goto LBL_ERR;
+   }
+
+   /* compute MGF1 of seed (k - hlen - 1) */
+   if ((err = pkcs_1_mgf1(hash_idx, seed, hLen, mask, modulus_len - hLen - 1)) != CRYPT_OK) {
+      goto LBL_ERR;
+   }
+
+   /* xor against DB */
+   for (y = 0; y < (modulus_len - hLen - 1); y++) {
+       DB[y] ^= mask[y]; 
+   }
+
+   /* compute MGF1 of maskedDB (hLen) */ 
+   if ((err = pkcs_1_mgf1(hash_idx, DB, modulus_len - hLen - 1, mask, hLen)) != CRYPT_OK) {
+      goto LBL_ERR;
+   }
+
+   /* XOR against seed */
+   for (y = 0; y < hLen; y++) {
+      seed[y] ^= mask[y];
+   }
+
+   /* create string of length modulus_len */
+   if (*outlen < modulus_len) {
+      *outlen = modulus_len;
+      err = CRYPT_BUFFER_OVERFLOW;
+      goto LBL_ERR;
+   }
+
+   /* start output which is 0x00 || maskedSeed || maskedDB */
+   x = 0;
+   out[x++] = 0x00;
+   XMEMCPY(out+x, seed, hLen);
+   x += hLen;
+   XMEMCPY(out+x, DB, modulus_len - hLen - 1);
+   x += modulus_len - hLen - 1;
+
+   *outlen = x;
+    
+   err = CRYPT_OK;
+LBL_ERR:
+#ifdef LTC_CLEAN_STACK
+   zeromem(DB,   modulus_len);
+   zeromem(seed, hLen);
+   zeromem(mask, modulus_len);
+#endif
+
+   XFREE(seed);
+   XFREE(mask);
+   XFREE(DB);
+
+   return err;
+}
+
+#endif /* LTC_PKCS_1 */
+
+
+/* $Source: /cvs/libtom/libtomcrypt/src/pk/pkcs1/pkcs_1_oaep_encode.c,v $ */
+/* $Revision: 1.9 $ */
+/* $Date: 2007/05/12 14:32:35 $ */
diff --git a/core/lib/libtomcrypt/src/pk/pkcs1/pkcs_1_os2ip.c b/core/lib/libtomcrypt/src/pk/pkcs1/pkcs_1_os2ip.c
new file mode 100644
index 0000000..ee4d947
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/pkcs1/pkcs_1_os2ip.c
@@ -0,0 +1,63 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/** 
+  @file pkcs_1_os2ip.c
+  Octet to Integer OS2IP, Tom St Denis 
+*/
+#ifdef LTC_PKCS_1
+
+/**
+  Read a binary string into an mp_int
+  @param n          [out] The mp_int destination
+  @param in         The binary string to read
+  @param inlen      The length of the binary string
+  @return CRYPT_OK if successful
+*/
+int pkcs_1_os2ip(void *n, unsigned char *in, unsigned long inlen)
+{
+   return mp_read_unsigned_bin(n, in, inlen);
+}
+
+#endif /* LTC_PKCS_1 */
+
+
+/* $Source: /cvs/libtom/libtomcrypt/src/pk/pkcs1/pkcs_1_os2ip.c,v $ */
+/* $Revision: 1.7 $ */
+/* $Date: 2007/05/12 14:32:35 $ */
diff --git a/core/lib/libtomcrypt/src/pk/pkcs1/pkcs_1_pss_decode.c b/core/lib/libtomcrypt/src/pk/pkcs1/pkcs_1_pss_decode.c
new file mode 100644
index 0000000..f79f79e
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/pkcs1/pkcs_1_pss_decode.c
@@ -0,0 +1,205 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/** 
+  @file pkcs_1_pss_decode.c
+  PKCS #1 PSS Signature Padding, Tom St Denis
+*/
+
+#ifdef LTC_PKCS_1
+
+/**
+   PKCS #1 v2.00 PSS decode
+   @param  msghash         The hash to verify
+   @param  msghashlen      The length of the hash (octets)
+   @param  sig             The signature data (encoded data)
+   @param  siglen          The length of the signature data (octets)
+   @param  saltlen         The length of the salt used (octets)
+   @param  hash_idx        The index of the hash desired
+   @param  modulus_bitlen  The bit length of the RSA modulus
+   @param  res             [out] The result of the comparison, 1==valid, 0==invalid
+   @return CRYPT_OK if successful (even if the comparison failed)
+*/
+int pkcs_1_pss_decode(const unsigned char *msghash, unsigned long msghashlen,
+                      const unsigned char *sig,     unsigned long siglen,
+                            unsigned long saltlen,  int           hash_idx,
+                            unsigned long modulus_bitlen, int    *res)
+{
+   unsigned char *DB, *mask, *salt, *hash;
+   unsigned long x, y, hLen, modulus_len;
+   int           err;
+   hash_state    md;
+
+   LTC_ARGCHK(msghash != NULL);
+   LTC_ARGCHK(res     != NULL);
+
+   /* default to invalid */
+   *res = 0;
+
+   /* ensure hash is valid */
+   if ((err = hash_is_valid(hash_idx)) != CRYPT_OK) {
+      return err;
+   }
+
+   hLen        = hash_descriptor[hash_idx]->hashsize;
+   modulus_bitlen--;
+   modulus_len = (modulus_bitlen>>3) + (modulus_bitlen & 7 ? 1 : 0);
+
+   /* check sizes */
+   if ((saltlen > modulus_len) || 
+       (modulus_len < hLen + saltlen + 2)) {
+      return CRYPT_PK_INVALID_SIZE;
+   }
+
+   /* allocate ram for DB/mask/salt/hash of size modulus_len */
+   DB   = XMALLOC(modulus_len);
+   mask = XMALLOC(modulus_len);
+   salt = XMALLOC(modulus_len);
+   hash = XMALLOC(modulus_len);
+   if (DB == NULL || mask == NULL || salt == NULL || hash == NULL) {
+      if (DB != NULL) {
+         XFREE(DB);
+      }
+      if (mask != NULL) {
+         XFREE(mask);
+      }
+      if (salt != NULL) {
+         XFREE(salt);
+      }
+      if (hash != NULL) {
+         XFREE(hash);
+      }
+      return CRYPT_MEM;
+   }
+
+   /* ensure the 0xBC byte */
+   if (sig[siglen-1] != 0xBC) {
+      err = CRYPT_INVALID_PACKET;
+      goto LBL_ERR;
+   }
+
+   /* copy out the DB */
+   x = 0;
+   XMEMCPY(DB, sig + x, modulus_len - hLen - 1);
+   x += modulus_len - hLen - 1;
+
+   /* copy out the hash */
+   XMEMCPY(hash, sig + x, hLen);
+   /* x += hLen; */
+
+   /* check the MSB */
+   if ((sig[0] & ~(0xFF >> ((modulus_len<<3) - (modulus_bitlen)))) != 0) {
+      err = CRYPT_INVALID_PACKET;
+      goto LBL_ERR;
+   }
+
+   /* generate mask of length modulus_len - hLen - 1 from hash */
+   if ((err = pkcs_1_mgf1(hash_idx, hash, hLen, mask, modulus_len - hLen - 1)) != CRYPT_OK) {
+      goto LBL_ERR;
+   }
+
+   /* xor against DB */
+   for (y = 0; y < (modulus_len - hLen - 1); y++) {
+      DB[y] ^= mask[y];
+   }
+   
+   /* now clear the first byte [make sure smaller than modulus] */
+   DB[0] &= 0xFF >> ((modulus_len<<3) - (modulus_bitlen));
+
+   /* DB = PS || 0x01 || salt, PS == modulus_len - saltlen - hLen - 2 zero bytes */
+
+   /* check for zeroes and 0x01 */
+   for (x = 0; x < modulus_len - saltlen - hLen - 2; x++) {
+       if (DB[x] != 0x00) {
+          err = CRYPT_INVALID_PACKET;
+          goto LBL_ERR;
+       }
+   }
+
+   /* check for the 0x01 */
+   if (DB[x++] != 0x01) {
+      err = CRYPT_INVALID_PACKET;
+      goto LBL_ERR;
+   }
+
+   /* M = (eight) 0x00 || msghash || salt, mask = H(M) */
+   if ((err = hash_descriptor[hash_idx]->init(&md)) != CRYPT_OK) {
+      goto LBL_ERR;
+   }
+   zeromem(mask, 8);
+   if ((err = hash_descriptor[hash_idx]->process(&md, mask, 8)) != CRYPT_OK) {
+      goto LBL_ERR;
+   }
+   if ((err = hash_descriptor[hash_idx]->process(&md, msghash, msghashlen)) != CRYPT_OK) {
+      goto LBL_ERR;
+   }
+   if ((err = hash_descriptor[hash_idx]->process(&md, DB+x, saltlen)) != CRYPT_OK) {
+      goto LBL_ERR;
+   }
+   if ((err = hash_descriptor[hash_idx]->done(&md, mask)) != CRYPT_OK) {
+      goto LBL_ERR;
+   }
+
+   /* mask == hash means valid signature */
+   if (XMEM_NEQ(mask, hash, hLen) == 0) {
+      *res = 1;
+   }
+
+   err = CRYPT_OK;
+LBL_ERR:
+#ifdef LTC_CLEAN_STACK
+   zeromem(DB,   modulus_len);   
+   zeromem(mask, modulus_len);   
+   zeromem(salt, modulus_len);   
+   zeromem(hash, modulus_len);   
+#endif
+
+   XFREE(hash);
+   XFREE(salt);
+   XFREE(mask);
+   XFREE(DB);
+
+   return err;
+}
+
+#endif /* LTC_PKCS_1 */
+
+/* $Source: /cvs/libtom/libtomcrypt/src/pk/pkcs1/pkcs_1_pss_decode.c,v $ */
+/* $Revision: 1.11 $ */
+/* $Date: 2007/05/12 14:32:35 $ */
diff --git a/core/lib/libtomcrypt/src/pk/pkcs1/pkcs_1_pss_encode.c b/core/lib/libtomcrypt/src/pk/pkcs1/pkcs_1_pss_encode.c
new file mode 100644
index 0000000..b975213
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/pkcs1/pkcs_1_pss_encode.c
@@ -0,0 +1,203 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/** 
+  @file pkcs_1_pss_encode.c
+  PKCS #1 PSS Signature Padding, Tom St Denis
+*/
+
+#ifdef LTC_PKCS_1
+
+/**
+   PKCS #1 v2.00 Signature Encoding
+   @param msghash          The hash to encode
+   @param msghashlen       The length of the hash (octets)
+   @param saltlen          The length of the salt desired (octets)
+   @param prng             An active PRNG context
+   @param prng_idx         The index of the PRNG desired
+   @param hash_idx         The index of the hash desired
+   @param modulus_bitlen   The bit length of the RSA modulus
+   @param out              [out] The destination of the encoding
+   @param outlen           [in/out] The max size and resulting size of the encoded data
+   @return CRYPT_OK if successful
+*/
+int pkcs_1_pss_encode(const unsigned char *msghash, unsigned long msghashlen,
+                            unsigned long saltlen,  prng_state   *prng,     
+                            int           prng_idx, int           hash_idx,
+                            unsigned long modulus_bitlen,
+                            unsigned char *out,     unsigned long *outlen)
+{
+   unsigned char *DB, *mask, *salt, *hash;
+   unsigned long x, y, hLen, modulus_len;
+   int           err;
+   hash_state    md;
+
+   LTC_ARGCHK(msghash != NULL);
+   LTC_ARGCHK(out     != NULL);
+   LTC_ARGCHK(outlen  != NULL);
+
+   /* ensure hash and PRNG are valid */
+   if ((err = hash_is_valid(hash_idx)) != CRYPT_OK) {
+      return err;
+   }
+   if ((err = prng_is_valid(prng_idx)) != CRYPT_OK) {
+      return err;
+   }
+
+   hLen        = hash_descriptor[hash_idx]->hashsize;
+   modulus_bitlen--;
+   modulus_len = (modulus_bitlen>>3) + (modulus_bitlen & 7 ? 1 : 0);
+
+   /* check sizes */
+   if ((saltlen > modulus_len) || (modulus_len < hLen + saltlen + 2)) {
+      return CRYPT_PK_INVALID_SIZE;
+   }
+
+   /* allocate ram for DB/mask/salt/hash of size modulus_len */
+   DB   = XMALLOC(modulus_len);
+   mask = XMALLOC(modulus_len);
+   salt = XMALLOC(modulus_len);
+   hash = XMALLOC(modulus_len);
+   if (DB == NULL || mask == NULL || salt == NULL || hash == NULL) {
+      if (DB != NULL) {
+         XFREE(DB);
+      }
+      if (mask != NULL) {
+         XFREE(mask);
+      }
+      if (salt != NULL) {
+         XFREE(salt);
+      }
+      if (hash != NULL) {
+         XFREE(hash);
+      }
+      return CRYPT_MEM;
+   }
+
+
+   /* generate random salt */
+   if (saltlen > 0) {
+      if (prng_descriptor[prng_idx]->read(salt, saltlen, prng) != saltlen) {
+         err = CRYPT_ERROR_READPRNG;
+         goto LBL_ERR;
+      }
+   }
+
+   /* M = (eight) 0x00 || msghash || salt, hash = H(M) */
+   if ((err = hash_descriptor[hash_idx]->init(&md)) != CRYPT_OK) {
+      goto LBL_ERR;
+   }
+   zeromem(DB, 8);
+   if ((err = hash_descriptor[hash_idx]->process(&md, DB, 8)) != CRYPT_OK) {
+      goto LBL_ERR;
+   }
+   if ((err = hash_descriptor[hash_idx]->process(&md, msghash, msghashlen)) != CRYPT_OK) {
+      goto LBL_ERR;
+   }
+   if ((err = hash_descriptor[hash_idx]->process(&md, salt, saltlen)) != CRYPT_OK) {
+      goto LBL_ERR;
+   }
+   if ((err = hash_descriptor[hash_idx]->done(&md, hash)) != CRYPT_OK) {
+      goto LBL_ERR;
+   }
+
+   /* generate DB = PS || 0x01 || salt, PS == modulus_len - saltlen - hLen - 2 zero bytes */
+   x = 0;
+   XMEMSET(DB + x, 0, modulus_len - saltlen - hLen - 2);
+   x += modulus_len - saltlen - hLen - 2;
+   DB[x++] = 0x01;
+   XMEMCPY(DB + x, salt, saltlen);
+   /* x += saltlen; */
+
+   /* generate mask of length modulus_len - hLen - 1 from hash */
+   if ((err = pkcs_1_mgf1(hash_idx, hash, hLen, mask, modulus_len - hLen - 1)) != CRYPT_OK) {
+      goto LBL_ERR;
+   }
+
+   /* xor against DB */
+   for (y = 0; y < (modulus_len - hLen - 1); y++) {
+      DB[y] ^= mask[y];
+   }
+
+   /* output is DB || hash || 0xBC */
+   if (*outlen < modulus_len) {
+      *outlen = modulus_len;
+      err = CRYPT_BUFFER_OVERFLOW;
+      goto LBL_ERR;
+   }
+
+   /* DB len = modulus_len - hLen - 1 */
+   y = 0;
+   XMEMCPY(out + y, DB, modulus_len - hLen - 1);
+   y += modulus_len - hLen - 1;
+
+   /* hash */
+   XMEMCPY(out + y, hash, hLen);
+   y += hLen;
+
+   /* 0xBC */
+   out[y] = 0xBC;
+
+   /* now clear the 8*modulus_len - modulus_bitlen most significant bits */
+   out[0] &= 0xFF >> ((modulus_len<<3) - modulus_bitlen);
+
+   /* store output size */
+   *outlen = modulus_len;
+   err = CRYPT_OK;
+LBL_ERR:
+#ifdef LTC_CLEAN_STACK
+   zeromem(DB,   modulus_len);   
+   zeromem(mask, modulus_len);   
+   zeromem(salt, modulus_len);   
+   zeromem(hash, modulus_len);   
+#endif
+
+   XFREE(hash);
+   XFREE(salt);
+   XFREE(mask);
+   XFREE(DB);
+
+   return err;
+}
+
+#endif /* LTC_PKCS_1 */
+
+/* $Source: /cvs/libtom/libtomcrypt/src/pk/pkcs1/pkcs_1_pss_encode.c,v $ */
+/* $Revision: 1.9 $ */
+/* $Date: 2007/05/12 14:32:35 $ */
diff --git a/core/lib/libtomcrypt/src/pk/pkcs1/pkcs_1_v1_5_decode.c b/core/lib/libtomcrypt/src/pk/pkcs1/pkcs_1_v1_5_decode.c
new file mode 100644
index 0000000..3f5eff7
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/pkcs1/pkcs_1_v1_5_decode.c
@@ -0,0 +1,141 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/** @file pkcs_1_v1_5_decode.c
+ *
+ *  PKCS #1 v1.5 Padding. (Andreas Lange)
+ */
+
+#ifdef LTC_PKCS_1
+
+/** @brief PKCS #1 v1.5 decode.
+ *
+ *  @param msg              The encoded data to decode
+ *  @param msglen           The length of the encoded data (octets)
+ *  @param block_type       Block type to use in padding (\sa ltc_pkcs_1_v1_5_blocks)
+ *  @param modulus_bitlen   The bit length of the RSA modulus
+ *  @param out              [out] Destination of decoding
+ *  @param outlen           [in/out] The max size and resulting size of the decoding
+ *  @param is_valid         [out] Boolean whether the padding was valid
+ *
+ *  @return CRYPT_OK if successful
+ */
+int pkcs_1_v1_5_decode(const unsigned char *msg, 
+                             unsigned long  msglen,
+                                       int  block_type,
+                             unsigned long  modulus_bitlen,
+                             unsigned char *out, 
+                             unsigned long *outlen,
+                                       int *is_valid)
+{
+  unsigned long modulus_len, ps_len, i;
+  int result;
+
+  /* default to invalid packet */
+  *is_valid = 0;
+
+  modulus_len = (modulus_bitlen >> 3) + (modulus_bitlen & 7 ? 1 : 0);
+
+  /* test message size */
+
+  if ((msglen > modulus_len) || (modulus_len < 11)) {
+    return CRYPT_PK_INVALID_SIZE;
+  }
+
+  result = CRYPT_OK;
+
+  /* separate encoded message */
+
+  if ((msg[0] != 0x00) || (msg[1] != (unsigned char)block_type)) {
+    result = CRYPT_INVALID_PACKET;
+  }
+
+  if (block_type == LTC_PKCS_1_EME) {
+    for (i = 2; i < modulus_len; i++) {
+      /* separator */
+      if (msg[i] == 0x00) { break; }
+    }
+    ps_len = i++ - 2;
+
+    if (i >= modulus_len) {
+      /* There was no octet with hexadecimal value 0x00 to separate ps from m.
+       */
+      result = CRYPT_INVALID_PACKET;
+    }
+  } else {
+    for (i = 2; i < modulus_len - 1; i++) {
+       if (msg[i] != 0xFF) { break; }
+    }
+
+    /* separator check */
+    if (msg[i] != 0) {
+      /* There was no octet with hexadecimal value 0x00 to separate ps from m. */
+      result = CRYPT_INVALID_PACKET;
+    }
+
+    ps_len = i - 2;
+  }
+
+  if (ps_len < 8)
+  {
+    /* The length of ps is less than 8 octets.
+     */
+    result = CRYPT_INVALID_PACKET;
+  }
+
+  if (*outlen < (msglen - (2 + ps_len + 1))) {
+    result = CRYPT_INVALID_PACKET;
+  }
+
+  if (result == CRYPT_OK) {
+     *outlen = (msglen - (2 + ps_len + 1));
+     XMEMCPY(out, &msg[2 + ps_len + 1], *outlen);
+
+     /* valid packet */
+     *is_valid = 1;
+  }
+
+  return result;
+} /* pkcs_1_v1_5_decode */
+
+#endif /* #ifdef LTC_PKCS_1 */
+
+/* $Source: /cvs/libtom/libtomcrypt/src/pk/pkcs1/pkcs_1_v1_5_decode.c,v $ */
+/* $Revision: 1.7 $ */
+/* $Date: 2007/05/12 14:32:35 $ */
diff --git a/core/lib/libtomcrypt/src/pk/pkcs1/pkcs_1_v1_5_encode.c b/core/lib/libtomcrypt/src/pk/pkcs1/pkcs_1_v1_5_encode.c
new file mode 100644
index 0000000..0b1dde3
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/pkcs1/pkcs_1_v1_5_encode.c
@@ -0,0 +1,138 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/*! \file pkcs_1_v1_5_encode.c
+ *
+ *  PKCS #1 v1.5 Padding (Andreas Lange)
+ */
+
+#ifdef LTC_PKCS_1
+
+/*! \brief PKCS #1 v1.5 encode.
+ *
+ *  \param msg              The data to encode
+ *  \param msglen           The length of the data to encode (octets)
+ *  \param block_type       Block type to use in padding (\sa ltc_pkcs_1_v1_5_blocks)
+ *  \param modulus_bitlen   The bit length of the RSA modulus
+ *  \param prng             An active PRNG state (only for LTC_PKCS_1_EME)
+ *  \param prng_idx         The index of the PRNG desired (only for LTC_PKCS_1_EME)
+ *  \param out              [out] The destination for the encoded data
+ *  \param outlen           [in/out] The max size and resulting size of the encoded data
+ *
+ *  \return CRYPT_OK if successful
+ */
+int pkcs_1_v1_5_encode(const unsigned char *msg, 
+                             unsigned long  msglen,
+                                       int  block_type,
+                             unsigned long  modulus_bitlen,
+                                prng_state *prng, 
+                                       int  prng_idx,
+                             unsigned char *out, 
+                             unsigned long *outlen)
+{
+  unsigned long modulus_len, ps_len, i;
+  unsigned char *ps;
+  int result;
+
+  /* valid block_type? */
+  if ((block_type != LTC_PKCS_1_EMSA) &&
+      (block_type != LTC_PKCS_1_EME)) {
+     return CRYPT_PK_INVALID_PADDING;
+  }
+
+  if (block_type == LTC_PKCS_1_EME) {    /* encryption padding, we need a valid PRNG */
+    if ((result = prng_is_valid(prng_idx)) != CRYPT_OK) {
+       return result;
+    }
+  }
+
+  modulus_len = (modulus_bitlen >> 3) + (modulus_bitlen & 7 ? 1 : 0);
+
+  /* test message size */
+  if ((msglen + 11) > modulus_len) {
+    return CRYPT_PK_INVALID_SIZE;
+  }
+
+  if (*outlen < modulus_len) {
+    *outlen = modulus_len;
+    result = CRYPT_BUFFER_OVERFLOW;
+    goto bail;
+  }
+
+  /* generate an octets string PS */
+  ps = &out[2];
+  ps_len = modulus_len - msglen - 3;
+
+  if (block_type == LTC_PKCS_1_EME) {
+    /* now choose a random ps */
+    if (prng_descriptor[prng_idx]->read(ps, ps_len, prng) != ps_len) {
+      result = CRYPT_ERROR_READPRNG;
+      goto bail;
+    }
+
+    /* transform zero bytes (if any) to non-zero random bytes */
+    for (i = 0; i < ps_len; i++) {
+      while (ps[i] == 0) {
+        if (prng_descriptor[prng_idx]->read(&ps[i], 1, prng) != 1) {
+          result = CRYPT_ERROR_READPRNG;
+          goto bail;
+        }
+      }
+    }
+  } else {
+    XMEMSET(ps, 0xFF, ps_len);
+  }
+
+  /* create string of length modulus_len */
+  out[0]          = 0x00;
+  out[1]          = (unsigned char)block_type;  /* block_type 1 or 2 */
+  out[2 + ps_len] = 0x00;
+  XMEMCPY(&out[2 + ps_len + 1], msg, msglen);
+  *outlen = modulus_len;
+
+  result  = CRYPT_OK;
+bail:
+  return result;
+} /* pkcs_1_v1_5_encode */
+
+#endif /* #ifdef LTC_PKCS_1 */
+
+/* $Source: /cvs/libtom/libtomcrypt/src/pk/pkcs1/pkcs_1_v1_5_encode.c,v $ */
+/* $Revision: 1.4 $ */
+/* $Date: 2007/05/12 14:32:35 $ */
diff --git a/core/lib/libtomcrypt/src/pk/pkcs1/sub.mk b/core/lib/libtomcrypt/src/pk/pkcs1/sub.mk
new file mode 100644
index 0000000..43f96b9
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/pkcs1/sub.mk
@@ -0,0 +1,9 @@
+srcs-y += pkcs_1_i2osp.c
+srcs-y += pkcs_1_mgf1.c
+srcs-y += pkcs_1_oaep_decode.c
+srcs-y += pkcs_1_oaep_encode.c
+srcs-y += pkcs_1_os2ip.c
+srcs-y += pkcs_1_pss_decode.c
+srcs-y += pkcs_1_pss_encode.c
+srcs-y += pkcs_1_v1_5_decode.c
+srcs-y += pkcs_1_v1_5_encode.c
diff --git a/core/lib/libtomcrypt/src/pk/rsa/rsa_decrypt_key.c b/core/lib/libtomcrypt/src/pk/rsa/rsa_decrypt_key.c
new file mode 100644
index 0000000..f83e047
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/rsa/rsa_decrypt_key.c
@@ -0,0 +1,132 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file rsa_decrypt_key.c
+  RSA PKCS #1 Decryption, Tom St Denis and Andreas Lange
+*/
+
+#ifdef LTC_MRSA
+
+/**
+   PKCS #1 decrypt then v1.5 or OAEP depad
+   @param in          The ciphertext
+   @param inlen       The length of the ciphertext (octets)
+   @param out         [out] The plaintext
+   @param outlen      [in/out] The max size and resulting size of the plaintext (octets)
+   @param lparam      The system "lparam" value
+   @param lparamlen   The length of the lparam value (octets)
+   @param hash_idx    The index of the hash desired
+   @param padding     Type of padding (LTC_PKCS_1_OAEP or LTC_PKCS_1_V1_5)
+   @param stat        [out] Result of the decryption, 1==valid, 0==invalid
+   @param key         The corresponding private RSA key
+   @return CRYPT_OK if succcessul (even if invalid)
+*/
+int rsa_decrypt_key_ex(const unsigned char *in,       unsigned long  inlen,
+                             unsigned char *out,      unsigned long *outlen,
+                       const unsigned char *lparam,   unsigned long  lparamlen,
+                             int            hash_idx, int            padding,
+                             int           *stat,     rsa_key       *key)
+{
+  unsigned long modulus_bitlen, modulus_bytelen, x;
+  int           err;
+  unsigned char *tmp;
+
+  LTC_ARGCHK(out    != NULL);
+  LTC_ARGCHK(outlen != NULL);
+  LTC_ARGCHK(key    != NULL);
+  LTC_ARGCHK(stat   != NULL);
+
+  /* default to invalid */
+  *stat = 0;
+
+  /* valid padding? */
+
+  if ((padding != LTC_PKCS_1_V1_5) &&
+      (padding != LTC_PKCS_1_OAEP)) {
+    return CRYPT_PK_INVALID_PADDING;
+  }
+
+  if (padding == LTC_PKCS_1_OAEP) {
+    /* valid hash ? */
+    if ((err = hash_is_valid(hash_idx)) != CRYPT_OK) {
+       return err;
+    }
+  }
+
+  /* get modulus len in bits */
+  modulus_bitlen = mp_count_bits( (key->N));
+
+  /* outlen must be at least the size of the modulus */
+  modulus_bytelen = mp_unsigned_bin_size( (key->N));
+  if (modulus_bytelen != inlen) {
+     return CRYPT_INVALID_PACKET;
+  }
+
+  /* allocate ram */
+  tmp = XMALLOC(inlen);
+  if (tmp == NULL) {
+     return CRYPT_MEM;
+  }
+
+  /* rsa decode the packet */
+  x = inlen;
+  if ((err = ltc_mp.rsa_me(in, inlen, tmp, &x, PK_PRIVATE, key)) != CRYPT_OK) {
+     XFREE(tmp);
+     return err;
+  }
+
+  if (padding == LTC_PKCS_1_OAEP) {
+    /* now OAEP decode the packet */
+    err = pkcs_1_oaep_decode(tmp, x, lparam, lparamlen, modulus_bitlen, hash_idx,
+                             out, outlen, stat);
+  } else {
+    /* now PKCS #1 v1.5 depad the packet */
+    err = pkcs_1_v1_5_decode(tmp, x, LTC_PKCS_1_EME, modulus_bitlen, out, outlen, stat);
+  }
+
+  XFREE(tmp);
+  return err;
+}
+
+#endif /* LTC_MRSA */
+
+/* $Source: /cvs/libtom/libtomcrypt/src/pk/rsa/rsa_decrypt_key.c,v $ */
+/* $Revision: 1.10 $ */
+/* $Date: 2007/05/12 14:32:35 $ */
diff --git a/core/lib/libtomcrypt/src/pk/rsa/rsa_encrypt_key.c b/core/lib/libtomcrypt/src/pk/rsa/rsa_encrypt_key.c
new file mode 100644
index 0000000..d01e5b6
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/rsa/rsa_encrypt_key.c
@@ -0,0 +1,129 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file rsa_encrypt_key.c
+  RSA PKCS #1 encryption, Tom St Denis and Andreas Lange
+*/
+
+#ifdef LTC_MRSA
+
+/**
+    (PKCS #1 v2.0) OAEP pad then encrypt
+    @param in          The plaintext
+    @param inlen       The length of the plaintext (octets)
+    @param out         [out] The ciphertext
+    @param outlen      [in/out] The max size and resulting size of the ciphertext
+    @param lparam      The system "lparam" for the encryption
+    @param lparamlen   The length of lparam (octets)
+    @param prng        An active PRNG
+    @param prng_idx    The index of the desired prng
+    @param hash_idx    The index of the desired hash
+    @param padding     Type of padding (LTC_PKCS_1_OAEP or LTC_PKCS_1_V1_5)
+    @param key         The RSA key to encrypt to
+    @return CRYPT_OK if successful
+*/
+int rsa_encrypt_key_ex(const unsigned char *in,     unsigned long inlen,
+                             unsigned char *out,    unsigned long *outlen,
+                       const unsigned char *lparam, unsigned long lparamlen,
+                       prng_state *prng, int prng_idx, int hash_idx, int padding, rsa_key *key)
+{
+  unsigned long modulus_bitlen, modulus_bytelen, x;
+  int           err;
+
+  LTC_ARGCHK(in     != NULL);
+  LTC_ARGCHK(out    != NULL);
+  LTC_ARGCHK(outlen != NULL);
+  LTC_ARGCHK(key    != NULL);
+
+  /* valid padding? */
+  if ((padding != LTC_PKCS_1_V1_5) &&
+      (padding != LTC_PKCS_1_OAEP)) {
+    return CRYPT_PK_INVALID_PADDING;
+  }
+
+  /* valid prng? */
+  if ((err = prng_is_valid(prng_idx)) != CRYPT_OK) {
+     return err;
+  }
+
+  if (padding == LTC_PKCS_1_OAEP) {
+    /* valid hash? */
+    if ((err = hash_is_valid(hash_idx)) != CRYPT_OK) {
+       return err;
+    }
+  }
+
+  /* get modulus len in bits */
+  modulus_bitlen = mp_count_bits( (key->N));
+
+  /* outlen must be at least the size of the modulus */
+  modulus_bytelen = mp_unsigned_bin_size( (key->N));
+  if (modulus_bytelen > *outlen) {
+     *outlen = modulus_bytelen;
+     return CRYPT_BUFFER_OVERFLOW;
+  }
+
+  if (padding == LTC_PKCS_1_OAEP) {
+    /* OAEP pad the key */
+    x = *outlen;
+    if ((err = pkcs_1_oaep_encode(in, inlen, lparam,
+                                  lparamlen, modulus_bitlen, prng, prng_idx, hash_idx,
+                                  out, &x)) != CRYPT_OK) {
+       return err;
+    }
+  } else {
+    /* PKCS #1 v1.5 pad the key */
+    x = *outlen;
+    if ((err = pkcs_1_v1_5_encode(in, inlen, LTC_PKCS_1_EME,
+                                  modulus_bitlen, prng, prng_idx,
+                                  out, &x)) != CRYPT_OK) {
+      return err;
+    }
+  }
+
+  /* rsa exptmod the OAEP or PKCS #1 v1.5 pad */
+  return ltc_mp.rsa_me(out, x, out, outlen, PK_PUBLIC, key);
+}
+
+#endif /* LTC_MRSA */
+
+/* $Source: /cvs/libtom/libtomcrypt/src/pk/rsa/rsa_encrypt_key.c,v $ */
+/* $Revision: 1.10 $ */
+/* $Date: 2007/05/12 14:32:35 $ */
diff --git a/core/lib/libtomcrypt/src/pk/rsa/rsa_export.c b/core/lib/libtomcrypt/src/pk/rsa/rsa_export.c
new file mode 100644
index 0000000..6ef370a
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/rsa/rsa_export.c
@@ -0,0 +1,126 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file rsa_export.c
+  Export RSA PKCS keys, Tom St Denis
+*/
+
+#ifdef LTC_MRSA
+
+/**
+    This will export either an RSAPublicKey or RSAPrivateKey [defined in PKCS #1 v2.1]
+    @param out       [out] Destination of the packet
+    @param outlen    [in/out] The max size and resulting size of the packet
+    @param type      The type of exported key (PK_PRIVATE or PK_PUBLIC)
+    @param key       The RSA key to export
+    @return CRYPT_OK if successful
+*/    
+int rsa_export(unsigned char *out, unsigned long *outlen, int type, rsa_key *key)
+{
+   unsigned long zero=0;
+   int err;
+   LTC_ARGCHK(out    != NULL);
+   LTC_ARGCHK(outlen != NULL);
+   LTC_ARGCHK(key    != NULL);
+
+   /* type valid? */
+   if (!(key->type == PK_PRIVATE) && (type == PK_PRIVATE)) {
+      return CRYPT_PK_INVALID_TYPE;
+   }
+
+   if (type == PK_PRIVATE) {
+      /* private key */
+      /* output is 
+            Version, n, e, d, p, q, d mod (p-1), d mod (q - 1), 1/q mod p
+       */
+      return der_encode_sequence_multi(out, outlen, 
+                          LTC_ASN1_SHORT_INTEGER, 1UL, &zero, 
+                          LTC_ASN1_INTEGER, 1UL,  key->N, 
+                          LTC_ASN1_INTEGER, 1UL,  key->e,
+                          LTC_ASN1_INTEGER, 1UL,  key->d, 
+                          LTC_ASN1_INTEGER, 1UL,  key->p, 
+                          LTC_ASN1_INTEGER, 1UL,  key->q, 
+                          LTC_ASN1_INTEGER, 1UL,  key->dP,
+                          LTC_ASN1_INTEGER, 1UL,  key->dQ, 
+                          LTC_ASN1_INTEGER, 1UL,  key->qP, 
+                          LTC_ASN1_EOL,     0UL, NULL);
+   } else {
+      /* public key */
+      unsigned long tmplen, *ptmplen;
+      unsigned char* tmp = NULL;
+
+      if (type & PK_STD) {
+          tmplen = (mp_count_bits(key->N)/8)*2+8;
+          tmp = XMALLOC(tmplen);
+          ptmplen = &tmplen;
+          if (tmp == NULL) {
+              return CRYPT_MEM;
+          }
+      }
+      else {
+          tmp = out;
+          ptmplen = outlen;
+      }
+
+      err = der_encode_sequence_multi(tmp, ptmplen,
+                                 LTC_ASN1_INTEGER, 1UL,  key->N,
+                                 LTC_ASN1_INTEGER, 1UL,  key->e,
+                                 LTC_ASN1_EOL,     0UL, NULL);
+
+      if ((err != CRYPT_OK) || !(type & PK_STD)) {
+          goto finish;
+      }
+
+      err = der_encode_subject_public_key_info(out, outlen,
+        PKA_RSA, tmp, tmplen, LTC_ASN1_NULL, NULL, 0);
+
+finish:
+      if (tmp != out)
+        XFREE(tmp);
+      return err;
+
+   }
+}
+
+#endif /* LTC_MRSA */
+
+/* $Source: /cvs/libtom/libtomcrypt/src/pk/rsa/rsa_export.c,v $ */
+/* $Revision: 1.17 $ */
+/* $Date: 2007/05/12 14:32:35 $ */
diff --git a/core/lib/libtomcrypt/src/pk/rsa/rsa_exptmod.c b/core/lib/libtomcrypt/src/pk/rsa/rsa_exptmod.c
new file mode 100644
index 0000000..1f533fa
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/rsa/rsa_exptmod.c
@@ -0,0 +1,221 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ *
+ * Added RSA blinding --nmav
+ */
+#include "tomcrypt.h"
+
+/**
+  @file rsa_exptmod.c
+  RSA PKCS exptmod, Tom St Denis
+*/
+
+#ifdef LTC_MRSA
+
+/** 
+   Compute an RSA modular exponentiation 
+   @param in         The input data to send into RSA
+   @param inlen      The length of the input (octets)
+   @param out        [out] The destination 
+   @param outlen     [in/out] The max size and resulting size of the output
+   @param which      Which exponent to use, e.g. PK_PRIVATE or PK_PUBLIC
+   @param key        The RSA key to use 
+   @return CRYPT_OK if successful
+*/   
+int rsa_exptmod(const unsigned char *in,   unsigned long inlen,
+                      unsigned char *out,  unsigned long *outlen, int which,
+                      rsa_key *key)
+{
+   void         *tmp, *tmpa, *tmpb;
+#ifdef LTC_RSA_BLINDING
+   void        *rnd, *rndi /* inverse of rnd */;
+#endif
+   unsigned long x;
+   int           err, no_crt;
+
+   LTC_ARGCHK(in     != NULL);
+   LTC_ARGCHK(out    != NULL);
+   LTC_ARGCHK(outlen != NULL);
+   LTC_ARGCHK(key    != NULL);
+  
+   /* is the key of the right type for the operation? */
+   if (which == PK_PRIVATE && (key->type != PK_PRIVATE)) {
+      return CRYPT_PK_NOT_PRIVATE;
+   }
+
+   /* must be a private or public operation */
+   if (which != PK_PRIVATE && which != PK_PUBLIC) {
+      return CRYPT_PK_INVALID_TYPE;
+   }
+
+   /* init and copy into tmp */
+   if ((err = mp_init_multi(&tmp, &tmpa, &tmpb,
+#ifdef LTC_RSA_BLINDING
+                                               &rnd, &rndi,
+#endif /* LTC_RSA_BLINDING */
+                                                           NULL)) != CRYPT_OK)
+        { return err; }
+   if ((err = mp_read_unsigned_bin(tmp, (unsigned char *)in, (int)inlen)) != CRYPT_OK)
+        { goto error; }
+
+   /* sanity check on the input */
+   if (mp_cmp(key->N, tmp) == LTC_MP_LT) {
+      err = CRYPT_PK_INVALID_SIZE;
+      goto error;
+   }
+
+   /* are we using the private exponent and is the key optimized? */
+#ifdef LTC_LINARO_FIX_RSAWITHOUTCRT
+   if ((which == PK_PRIVATE) && (key->dP == NULL)) {
+      /*
+       * Fix when CRT optimization parameters are not there
+       * In such a case, we directly use the private key
+       */
+      LTC_ARGCHK(key->dQ == NULL);
+      LTC_ARGCHK(key->qP == NULL);
+      LTC_ARGCHK(key->p  == NULL);
+      LTC_ARGCHK(key->q  == NULL);
+      /* exptmod it */
+      if ((err = mp_exptmod(tmp, key->d, key->N, tmp)) != CRYPT_OK)                                { goto error; }
+   } else
+#endif
+   if (which == PK_PRIVATE) {
+#ifdef LTC_RSA_BLINDING
+      /* do blinding */
+      err = mp_rand(rnd, mp_get_digit_count(key->N));
+      if (err != CRYPT_OK) {
+             goto error;
+      }
+
+      /* rndi = 1/rnd mod N */
+      err = mp_invmod(rnd, key->N, rndi);
+      if (err != CRYPT_OK) {
+             goto error;
+      }
+
+      /* rnd = rnd^e */
+      err = mp_exptmod( rnd, key->e, key->N, rnd);
+      if (err != CRYPT_OK) {
+             goto error;
+      }
+
+      /* tmp = tmp*rnd mod N */
+      err = mp_mulmod( tmp, rnd, key->N, tmp);
+      if (err != CRYPT_OK) {
+             goto error;
+      }
+#endif /* LTC_RSA_BLINDING */
+
+      no_crt = (key->dP == NULL) || (mp_get_digit_count(key->dP) == 0);
+
+      if (no_crt) {
+         /*
+          * In case CRT optimization parameters are not provided,
+          * the private key is directly used to exptmod it
+          */
+         if ((err = mp_exptmod(tmp, key->d, key->N, tmp)) != CRYPT_OK)                              { goto error; }
+      } else {
+         /* tmpa = tmp^dP mod p */
+         if ((err = mp_exptmod(tmp, key->dP, key->p, tmpa)) != CRYPT_OK)                            { goto error; }
+
+         /* tmpb = tmp^dQ mod q */
+         if ((err = mp_exptmod(tmp, key->dQ, key->q, tmpb)) != CRYPT_OK)                            { goto error; }
+
+         /* tmp = (tmpa - tmpb) * qInv (mod p) */
+         if ((err = mp_sub(tmpa, tmpb, tmp)) != CRYPT_OK)                                           { goto error; }
+         if ((err = mp_mulmod(tmp, key->qP, key->p, tmp)) != CRYPT_OK)                              { goto error; }
+
+         /* tmp = tmpb + q * tmp */
+         if ((err = mp_mul(tmp, key->q, tmp)) != CRYPT_OK)                                          { goto error; }
+         if ((err = mp_add(tmp, tmpb, tmp)) != CRYPT_OK)                                            { goto error; }
+      }
+
+      #ifdef LTC_RSA_BLINDING
+      /* unblind */
+      err = mp_mulmod( tmp, rndi, key->N, tmp);
+      if (err != CRYPT_OK) {
+             goto error;
+      }
+      #endif
+
+      #ifdef LTC_RSA_CRT_HARDENING
+      if (!no_crt) {
+         if ((err = mp_exptmod(tmp, key->e, key->N, tmpa)) != CRYPT_OK)                              { goto error; }
+         if ((err = mp_read_unsigned_bin(tmpb, (unsigned char *)in, (int)inlen)) != CRYPT_OK)        { goto error; }
+         if (mp_cmp(tmpa, tmpb) != LTC_MP_EQ)                                     { err = CRYPT_ERROR; goto error; }
+      }
+      #endif
+   } else {
+      /* exptmod it */
+      if ((err = mp_exptmod(tmp, key->e, key->N, tmp)) != CRYPT_OK)                                { goto error; }
+   }
+
+   /* read it back */
+   x = (unsigned long)mp_unsigned_bin_size(key->N);
+   if (x > *outlen) {
+      *outlen = x;
+      err = CRYPT_BUFFER_OVERFLOW;
+      goto error;
+   }
+
+   /* this should never happen ... */
+   if (mp_unsigned_bin_size(tmp) > mp_unsigned_bin_size(key->N)) {
+      err = CRYPT_ERROR;
+      goto error;
+   }
+   *outlen = x;
+
+   /* convert it */
+   zeromem(out, x);
+   if ((err = mp_to_unsigned_bin(tmp, out+(x-mp_unsigned_bin_size(tmp)))) != CRYPT_OK)               { goto error; }
+
+   /* clean up and return */
+   err = CRYPT_OK;
+error:
+   mp_clear_multi(
+#ifdef LTC_RSA_BLINDING
+                  rndi, rnd,
+#endif /* LTC_RSA_BLINDING */
+                             tmpb, tmpa, tmp, NULL);
+   return err;
+}
+
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/pk/rsa/rsa_exptmod.c,v $ */
+/* $Revision: 1.18 $ */
+/* $Date: 2007/05/12 14:32:35 $ */
diff --git a/core/lib/libtomcrypt/src/pk/rsa/rsa_free.c b/core/lib/libtomcrypt/src/pk/rsa/rsa_free.c
new file mode 100644
index 0000000..cfc617c
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/rsa/rsa_free.c
@@ -0,0 +1,61 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file rsa_free.c
+  Free an RSA key, Tom St Denis
+*/  
+
+#ifdef LTC_MRSA
+
+/**
+  Free an RSA key from memory
+  @param key   The RSA key to free
+*/
+void rsa_free(rsa_key *key)
+{
+   LTC_ARGCHKVD(key != NULL);
+   mp_clear_multi(key->e, key->d, key->N, key->dQ, key->dP, key->qP, key->p, key->q, NULL);
+}
+
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/pk/rsa/rsa_free.c,v $ */
+/* $Revision: 1.10 $ */
+/* $Date: 2007/05/12 14:32:35 $ */
diff --git a/core/lib/libtomcrypt/src/pk/rsa/rsa_import.c b/core/lib/libtomcrypt/src/pk/rsa/rsa_import.c
new file mode 100644
index 0000000..7975d2a
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/rsa/rsa_import.c
@@ -0,0 +1,157 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file rsa_import.c
+  Import a PKCS RSA key, Tom St Denis
+*/
+
+#ifdef LTC_MRSA
+
+/**
+  Import an RSAPublicKey or RSAPrivateKey [two-prime only, only support >= 1024-bit keys, defined in PKCS #1 v2.1]
+  @param in      The packet to import from
+  @param inlen   It's length (octets)
+  @param key     [out] Destination for newly imported key
+  @return CRYPT_OK if successful, upon error allocated memory is freed
+*/
+int rsa_import(const unsigned char *in, unsigned long inlen, rsa_key *key)
+{
+   int           err;
+   void         *zero;
+   unsigned char *tmpbuf=NULL;
+   unsigned long tmpbuf_len;
+
+   LTC_ARGCHK(in          != NULL);
+   LTC_ARGCHK(key         != NULL);
+   LTC_ARGCHK(ltc_mp.name != NULL);
+
+   /* init key */
+   if ((err = mp_init_multi(&key->e, &key->d, &key->N, &key->dQ, 
+                            &key->dP, &key->qP, &key->p, &key->q, NULL)) != CRYPT_OK) {
+      return err;
+   }
+
+   /* see if the OpenSSL DER format RSA public key will work */
+   tmpbuf_len = MAX_RSA_SIZE * 8;
+   tmpbuf = XCALLOC(1, tmpbuf_len);
+   if (tmpbuf == NULL) {
+       err = CRYPT_MEM;
+       goto LBL_ERR;
+   }
+
+   err = der_decode_subject_public_key_info(in, inlen,
+        PKA_RSA, tmpbuf, &tmpbuf_len,
+        LTC_ASN1_NULL, NULL, 0);
+
+   if (err == CRYPT_OK) { /* SubjectPublicKeyInfo format */
+
+      /* now it should be SEQUENCE { INTEGER, INTEGER } */
+      if ((err = der_decode_sequence_multi(tmpbuf, tmpbuf_len,
+                                           LTC_ASN1_INTEGER, 1UL, key->N,
+                                           LTC_ASN1_INTEGER, 1UL, key->e,
+                                           LTC_ASN1_EOL,     0UL, NULL)) != CRYPT_OK) {
+         goto LBL_ERR;
+      }
+      key->type = PK_PUBLIC;
+      err = CRYPT_OK;
+      goto LBL_FREE;
+   }
+
+   /* not SSL public key, try to match against PKCS #1 standards */
+   if ((err = der_decode_sequence_multi(in, inlen, 
+                                  LTC_ASN1_INTEGER, 1UL, key->N, 
+                                  LTC_ASN1_EOL,     0UL, NULL)) != CRYPT_OK) {
+      goto LBL_ERR;
+   }
+
+   if (mp_cmp_d(key->N, 0) == LTC_MP_EQ) {
+      if ((err = mp_init(&zero)) != CRYPT_OK) { 
+         goto LBL_ERR;
+      }
+      /* it's a private key */
+      if ((err = der_decode_sequence_multi(in, inlen, 
+                          LTC_ASN1_INTEGER, 1UL, zero, 
+                          LTC_ASN1_INTEGER, 1UL, key->N, 
+                          LTC_ASN1_INTEGER, 1UL, key->e,
+                          LTC_ASN1_INTEGER, 1UL, key->d, 
+                          LTC_ASN1_INTEGER, 1UL, key->p, 
+                          LTC_ASN1_INTEGER, 1UL, key->q, 
+                          LTC_ASN1_INTEGER, 1UL, key->dP,
+                          LTC_ASN1_INTEGER, 1UL, key->dQ, 
+                          LTC_ASN1_INTEGER, 1UL, key->qP, 
+                          LTC_ASN1_EOL,     0UL, NULL)) != CRYPT_OK) {
+         mp_clear(zero);
+         goto LBL_ERR;
+      }
+      mp_clear(zero);
+      key->type = PK_PRIVATE;
+   } else if (mp_cmp_d(key->N, 1) == LTC_MP_EQ) {
+      /* we don't support multi-prime RSA */
+      err = CRYPT_PK_INVALID_TYPE;
+      goto LBL_ERR;
+   } else {
+      /* it's a public key and we lack e */
+      if ((err = der_decode_sequence_multi(in, inlen, 
+                                     LTC_ASN1_INTEGER, 1UL, key->N, 
+                                     LTC_ASN1_INTEGER, 1UL, key->e, 
+                                     LTC_ASN1_EOL,     0UL, NULL)) != CRYPT_OK) {
+         goto LBL_ERR;
+      }
+      key->type = PK_PUBLIC;
+   }
+   err = CRYPT_OK;
+   goto LBL_FREE;
+
+LBL_ERR:
+   mp_clear_multi(key->d,  key->e, key->N, key->dQ, key->dP, key->qP, key->p, key->q, NULL);
+
+LBL_FREE:
+   if (tmpbuf != NULL)
+     XFREE(tmpbuf);
+
+   return err;
+}
+
+#endif /* LTC_MRSA */
+
+
+/* $Source: /cvs/libtom/libtomcrypt/src/pk/rsa/rsa_import.c,v $ */
+/* $Revision: 1.23 $ */
+/* $Date: 2007/05/12 14:32:35 $ */
diff --git a/core/lib/libtomcrypt/src/pk/rsa/rsa_make_key.c b/core/lib/libtomcrypt/src/pk/rsa/rsa_make_key.c
new file mode 100644
index 0000000..163f643
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/rsa/rsa_make_key.c
@@ -0,0 +1,139 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file rsa_make_key.c
+  RSA key generation, Tom St Denis
+*/  
+
+#ifdef LTC_MRSA
+
+/** 
+   Create an RSA key
+   @param prng     An active PRNG state
+   @param wprng    The index of the PRNG desired
+   @param size     The size of the modulus (key size) desired (octets)
+   @param e        The "e" value (public key).  e==65537 is a good choice
+   @param key      [out] Destination of a newly created private key pair
+   @return CRYPT_OK if successful, upon error all allocated ram is freed
+*/
+int rsa_make_key(prng_state *prng, int wprng, int size, long e, rsa_key *key)
+{
+   void *p, *q, *tmp1, *tmp2, *tmp3;
+   int    err;
+
+   LTC_ARGCHK(ltc_mp.name != NULL);
+   LTC_ARGCHK(key         != NULL);
+
+   if ((size < (MIN_RSA_SIZE/8)) || (size > (MAX_RSA_SIZE/8))) {
+      return CRYPT_INVALID_KEYSIZE;
+   }
+
+   if ((e < 3) || ((e & 1) == 0)) {
+      return CRYPT_INVALID_ARG;
+   }
+
+   if ((err = prng_is_valid(wprng)) != CRYPT_OK) {
+      return err;
+   }
+
+   if ((err = mp_init_multi(&p, &q, &tmp1, &tmp2, &tmp3, NULL)) != CRYPT_OK) {
+      return err;
+   }
+
+   /* make primes p and q (optimization provided by Wayne Scott) */
+   if ((err = mp_set_int(tmp3, e)) != CRYPT_OK)                      { goto cleanup; }  /* tmp3 = e */
+
+   /* make prime "p" */
+   do {
+       if ((err = rand_prime( p, size/2, prng, wprng)) != CRYPT_OK)  { goto cleanup; }
+       if ((err = mp_sub_d( p, 1,  tmp1)) != CRYPT_OK)               { goto cleanup; }  /* tmp1 = p-1 */
+       if ((err = mp_gcd( tmp1,  tmp3,  tmp2)) != CRYPT_OK)          { goto cleanup; }  /* tmp2 = gcd(p-1, e) */
+   } while (mp_cmp_d( tmp2, 1) != 0);                                                  /* while e divides p-1 */
+
+   /* make prime "q" */
+   do {
+       if ((err = rand_prime( q, size/2, prng, wprng)) != CRYPT_OK)  { goto cleanup; }
+       if ((err = mp_sub_d( q, 1,  tmp1)) != CRYPT_OK)               { goto cleanup; } /* tmp1 = q-1 */
+       if ((err = mp_gcd( tmp1,  tmp3,  tmp2)) != CRYPT_OK)          { goto cleanup; } /* tmp2 = gcd(q-1, e) */
+   } while (mp_cmp_d( tmp2, 1) != 0);                                                 /* while e divides q-1 */
+
+   /* tmp1 = lcm(p-1, q-1) */
+   if ((err = mp_sub_d( p, 1,  tmp2)) != CRYPT_OK)                   { goto cleanup; } /* tmp2 = p-1 */
+                                                                                      /* tmp1 = q-1 (previous do/while loop) */
+   if ((err = mp_lcm( tmp1,  tmp2,  tmp1)) != CRYPT_OK)              { goto cleanup; } /* tmp1 = lcm(p-1, q-1) */
+
+   /* make key */
+   if ((err = mp_init_multi(&key->e, &key->d, &key->N, &key->dQ, &key->dP, &key->qP, &key->p, &key->q, NULL)) != CRYPT_OK) {
+      goto errkey;
+   }
+
+   if ((err = mp_set_int( key->e, e)) != CRYPT_OK)                     { goto errkey; } /* key->e =  e */
+   if ((err = mp_invmod( key->e,  tmp1,  key->d)) != CRYPT_OK)         { goto errkey; } /* key->d = 1/e mod lcm(p-1,q-1) */
+   if ((err = mp_mul( p,  q,  key->N)) != CRYPT_OK)                    { goto errkey; } /* key->N = pq */
+
+   /* optimize for CRT now */
+   /* find d mod q-1 and d mod p-1 */
+   if ((err = mp_sub_d( p, 1,  tmp1)) != CRYPT_OK)                     { goto errkey; } /* tmp1 = q-1 */
+   if ((err = mp_sub_d( q, 1,  tmp2)) != CRYPT_OK)                     { goto errkey; } /* tmp2 = p-1 */
+   if ((err = mp_mod( key->d,  tmp1,  key->dP)) != CRYPT_OK)           { goto errkey; } /* dP = d mod p-1 */
+   if ((err = mp_mod( key->d,  tmp2,  key->dQ)) != CRYPT_OK)           { goto errkey; } /* dQ = d mod q-1 */
+   if ((err = mp_invmod( q,  p,  key->qP)) != CRYPT_OK)                { goto errkey; } /* qP = 1/q mod p */
+
+   if ((err = mp_copy( p,  key->p)) != CRYPT_OK)                       { goto errkey; }
+   if ((err = mp_copy( q,  key->q)) != CRYPT_OK)                       { goto errkey; }
+
+   /* set key type (in this case it's CRT optimized) */
+   key->type = PK_PRIVATE;
+
+   /* return ok and free temps */
+   err       = CRYPT_OK;
+   goto cleanup;
+errkey:
+   mp_clear_multi(key->q, key->p, key->qP, key->dP, key->dQ, key->N, key->d, key->e, NULL);
+cleanup:
+   mp_clear_multi(tmp3, tmp2, tmp1, q, p, NULL);
+   return err;
+}
+
+#endif
+
+/* $Source: /cvs/libtom/libtomcrypt/src/pk/rsa/rsa_make_key.c,v $ */
+/* $Revision: 1.16 $ */
+/* $Date: 2007/05/12 14:32:35 $ */
diff --git a/core/lib/libtomcrypt/src/pk/rsa/rsa_sign_hash.c b/core/lib/libtomcrypt/src/pk/rsa/rsa_sign_hash.c
new file mode 100644
index 0000000..257ddec
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/rsa/rsa_sign_hash.c
@@ -0,0 +1,161 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file rsa_sign_hash.c
+  RSA PKCS #1 v1.5 and v2 PSS sign hash, Tom St Denis and Andreas Lange
+*/
+
+#ifdef LTC_MRSA
+
+/**
+  PKCS #1 pad then sign
+  @param in        The hash to sign
+  @param inlen     The length of the hash to sign (octets)
+  @param out       [out] The signature
+  @param outlen    [in/out] The max size and resulting size of the signature
+  @param padding   Type of padding (LTC_PKCS_1_PSS or LTC_PKCS_1_V1_5)
+  @param prng      An active PRNG state
+  @param prng_idx  The index of the PRNG desired
+  @param hash_idx  The index of the hash desired
+  @param saltlen   The length of the salt desired (octets)
+  @param key       The private RSA key to use
+  @return CRYPT_OK if successful
+*/
+int rsa_sign_hash_ex(const unsigned char *in,       unsigned long  inlen,
+                           unsigned char *out,      unsigned long *outlen,
+                           int            padding,
+                           prng_state    *prng,     int            prng_idx,
+                           int            hash_idx, unsigned long  saltlen,
+                           rsa_key *key)
+{
+   unsigned long modulus_bitlen, modulus_bytelen, x, y;
+   int           err;
+
+   LTC_ARGCHK(in       != NULL);
+   LTC_ARGCHK(out      != NULL);
+   LTC_ARGCHK(outlen   != NULL);
+   LTC_ARGCHK(key      != NULL);
+
+   /* valid padding? */
+   if ((padding != LTC_PKCS_1_V1_5) && (padding != LTC_PKCS_1_PSS)) {
+     return CRYPT_PK_INVALID_PADDING;
+   }
+
+   if (padding == LTC_PKCS_1_PSS) {
+     /* valid prng and hash ? */
+     if ((err = prng_is_valid(prng_idx)) != CRYPT_OK) {
+        return err;
+     }
+     if ((err = hash_is_valid(hash_idx)) != CRYPT_OK) {
+        return err;
+     }
+   }
+
+   /* get modulus len in bits */
+   modulus_bitlen = mp_count_bits((key->N));
+
+  /* outlen must be at least the size of the modulus */
+  modulus_bytelen = mp_unsigned_bin_size((key->N));
+  if (modulus_bytelen > *outlen) {
+     *outlen = modulus_bytelen;
+     return CRYPT_BUFFER_OVERFLOW;
+  }
+
+  if (padding == LTC_PKCS_1_PSS) {
+    /* PSS pad the key */
+    x = *outlen;
+    if ((err = pkcs_1_pss_encode(in, inlen, saltlen, prng, prng_idx,
+                                 hash_idx, modulus_bitlen, out, &x)) != CRYPT_OK) {
+       return err;
+    }
+  } else {
+    /* PKCS #1 v1.5 pad the hash */
+    unsigned char *tmpin;
+    ltc_asn1_list digestinfo[2], siginfo[2];
+
+    /* not all hashes have OIDs... so sad */
+    if (hash_descriptor[hash_idx]->OIDlen == 0) {
+       return CRYPT_INVALID_ARG;
+    }
+
+    /* construct the SEQUENCE 
+      SEQUENCE {
+         SEQUENCE {hashoid OID
+                   blah    NULL
+         }
+         hash    OCTET STRING 
+      }
+   */
+    LTC_SET_ASN1(digestinfo, 0, LTC_ASN1_OBJECT_IDENTIFIER, hash_descriptor[hash_idx]->OID, hash_descriptor[hash_idx]->OIDlen);
+    LTC_SET_ASN1(digestinfo, 1, LTC_ASN1_NULL,              NULL,                          0);
+    LTC_SET_ASN1(siginfo,    0, LTC_ASN1_SEQUENCE,          digestinfo,                    2);
+    LTC_SET_ASN1(siginfo,    1, LTC_ASN1_OCTET_STRING,      in,                            inlen);
+
+    /* allocate memory for the encoding */
+    y = mp_unsigned_bin_size(key->N);
+    tmpin = XMALLOC(y);
+    if (tmpin == NULL) {
+       return CRYPT_MEM;
+    }
+
+    if ((err = der_encode_sequence(siginfo, 2, tmpin, &y)) != CRYPT_OK) {
+       XFREE(tmpin);
+       return err;
+    }
+
+    x = *outlen;
+    if ((err = pkcs_1_v1_5_encode(tmpin, y, LTC_PKCS_1_EMSA,
+                                  modulus_bitlen, NULL, 0,
+                                  out, &x)) != CRYPT_OK) {
+      XFREE(tmpin);
+      return err;
+    }
+    XFREE(tmpin);
+  }
+
+  /* RSA encode it */
+  return ltc_mp.rsa_me(out, x, out, outlen, PK_PRIVATE, key);
+}
+
+#endif /* LTC_MRSA */
+
+/* $Source: /cvs/libtom/libtomcrypt/src/pk/rsa/rsa_sign_hash.c,v $ */
+/* $Revision: 1.11 $ */
+/* $Date: 2007/05/12 14:32:35 $ */
diff --git a/core/lib/libtomcrypt/src/pk/rsa/rsa_verify_hash.c b/core/lib/libtomcrypt/src/pk/rsa/rsa_verify_hash.c
new file mode 100644
index 0000000..a3c86d3
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/rsa/rsa_verify_hash.c
@@ -0,0 +1,207 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file rsa_verify_hash.c
+  RSA PKCS #1 v1.5 or v2 PSS signature verification, Tom St Denis and Andreas Lange
+*/
+
+#ifdef LTC_MRSA
+
+/**
+  PKCS #1 de-sign then v1.5 or PSS depad
+  @param sig              The signature data
+  @param siglen           The length of the signature data (octets)
+  @param hash             The hash of the message that was signed
+  @param hashlen          The length of the hash of the message that was signed (octets)
+  @param padding          Type of padding (LTC_PKCS_1_PSS or LTC_PKCS_1_V1_5)
+  @param hash_idx         The index of the desired hash
+  @param saltlen          The length of the salt used during signature
+  @param stat             [out] The result of the signature comparison, 1==valid, 0==invalid
+  @param key              The public RSA key corresponding to the key that performed the signature
+  @return CRYPT_OK on success (even if the signature is invalid)
+*/
+int rsa_verify_hash_ex(const unsigned char *sig,      unsigned long siglen,
+                       const unsigned char *hash,     unsigned long hashlen,
+                             int            padding,
+                             int            hash_idx, unsigned long saltlen,
+                             int           *stat,     rsa_key      *key)
+{
+  unsigned long modulus_bitlen, modulus_bytelen, x;
+  int           err;
+  unsigned char *tmpbuf;
+
+  LTC_ARGCHK(hash  != NULL);
+  LTC_ARGCHK(sig   != NULL);
+  LTC_ARGCHK(stat  != NULL);
+  LTC_ARGCHK(key   != NULL);
+
+  /* default to invalid */
+  *stat = 0;
+
+  /* valid padding? */
+
+  if ((padding != LTC_PKCS_1_V1_5) &&
+      (padding != LTC_PKCS_1_PSS)) {
+    return CRYPT_PK_INVALID_PADDING;
+  }
+
+  if (padding == LTC_PKCS_1_PSS) {
+    /* valid hash ? */
+    if ((err = hash_is_valid(hash_idx)) != CRYPT_OK) {
+       return err;
+    }
+  }
+
+  /* get modulus len in bits */
+  modulus_bitlen = mp_count_bits( (key->N));
+
+  /* outlen must be at least the size of the modulus */
+  modulus_bytelen = mp_unsigned_bin_size( (key->N));
+  if (modulus_bytelen != siglen) {
+     return CRYPT_INVALID_PACKET;
+  }
+
+  /* allocate temp buffer for decoded sig */
+  tmpbuf = XMALLOC(siglen);
+  if (tmpbuf == NULL) {
+     return CRYPT_MEM;
+  }
+
+  /* RSA decode it  */
+  x = siglen;
+  if ((err = ltc_mp.rsa_me(sig, siglen, tmpbuf, &x, PK_PUBLIC, key)) != CRYPT_OK) {
+     XFREE(tmpbuf);
+     return err;
+  }
+
+  /* make sure the output is the right size */
+  if (x != siglen) {
+     XFREE(tmpbuf);
+     return CRYPT_INVALID_PACKET;
+  }
+
+  if (padding == LTC_PKCS_1_PSS) {
+    /* PSS decode and verify it */
+
+    if(modulus_bitlen%8 == 1){
+      err = pkcs_1_pss_decode(hash, hashlen, tmpbuf+1, x-1, saltlen, hash_idx, modulus_bitlen, stat);
+    }
+    else{
+      err = pkcs_1_pss_decode(hash, hashlen, tmpbuf, x, saltlen, hash_idx, modulus_bitlen, stat);
+    }
+
+  } else {
+    /* PKCS #1 v1.5 decode it */
+    unsigned char *out;
+    unsigned long outlen, loid[16], reallen;
+    int           decoded;
+    ltc_asn1_list digestinfo[2], siginfo[2];
+
+    /* not all hashes have OIDs... so sad */
+    if (hash_descriptor[hash_idx]->OIDlen == 0) {
+       err = CRYPT_INVALID_ARG;
+       goto bail_2;
+    }
+
+    /* allocate temp buffer for decoded hash */
+    outlen = ((modulus_bitlen >> 3) + (modulus_bitlen & 7 ? 1 : 0)) - 3;
+    out    = XMALLOC(outlen);
+    if (out == NULL) {
+      err = CRYPT_MEM;
+      goto bail_2;
+    }
+
+    if ((err = pkcs_1_v1_5_decode(tmpbuf, x, LTC_PKCS_1_EMSA, modulus_bitlen, out, &outlen, &decoded)) != CRYPT_OK) {
+      XFREE(out);       
+      goto bail_2;
+    }
+
+    /* now we must decode out[0...outlen-1] using ASN.1, test the OID and then test the hash */
+    /* construct the SEQUENCE 
+      SEQUENCE {
+         SEQUENCE {hashoid OID
+                   blah    NULL
+         }
+         hash    OCTET STRING 
+      }
+   */
+    LTC_SET_ASN1(digestinfo, 0, LTC_ASN1_OBJECT_IDENTIFIER, loid, sizeof(loid)/sizeof(loid[0]));
+    LTC_SET_ASN1(digestinfo, 1, LTC_ASN1_NULL,              NULL,                          0);
+    LTC_SET_ASN1(siginfo,    0, LTC_ASN1_SEQUENCE,          digestinfo,                    2);
+    LTC_SET_ASN1(siginfo,    1, LTC_ASN1_OCTET_STRING,      tmpbuf,                        siglen);
+   
+    if ((err = der_decode_sequence(out, outlen, siginfo, 2)) != CRYPT_OK) {
+       XFREE(out);
+       goto bail_2;
+    }
+
+    if ((err = der_length_sequence(siginfo, 2, &reallen)) != CRYPT_OK) {
+       XFREE(out);
+       goto bail_2;
+    }
+
+    /* test OID */
+    if ((reallen == outlen) &&
+        (digestinfo[0].size == hash_descriptor[hash_idx]->OIDlen) &&
+        (XMEM_NEQ(digestinfo[0].data, hash_descriptor[hash_idx]->OID, sizeof(unsigned long) * hash_descriptor[hash_idx]->OIDlen) == 0) &&
+        (siginfo[1].size == hashlen) &&
+        (XMEM_NEQ(siginfo[1].data, hash, hashlen) == 0)) {
+       *stat = 1;
+    }
+
+#ifdef LTC_CLEAN_STACK
+    zeromem(out, outlen);
+#endif
+    XFREE(out);
+  }
+
+bail_2:
+#ifdef LTC_CLEAN_STACK
+  zeromem(tmpbuf, siglen);
+#endif
+  XFREE(tmpbuf);
+  return err;
+}
+
+#endif /* LTC_MRSA */
+
+/* $Source: /cvs/libtom/libtomcrypt/src/pk/rsa/rsa_verify_hash.c,v $ */
+/* $Revision: 1.13 $ */
+/* $Date: 2007/05/12 14:32:35 $ */
diff --git a/core/lib/libtomcrypt/src/pk/rsa/sub.mk b/core/lib/libtomcrypt/src/pk/rsa/sub.mk
new file mode 100644
index 0000000..b72a7c0
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/rsa/sub.mk
@@ -0,0 +1,9 @@
+srcs-y += rsa_decrypt_key.c
+srcs-y += rsa_encrypt_key.c
+srcs-y += rsa_export.c
+srcs-y += rsa_exptmod.c
+srcs-y += rsa_free.c
+srcs-y += rsa_import.c
+srcs-y += rsa_make_key.c
+srcs-y += rsa_sign_hash.c
+srcs-y += rsa_verify_hash.c
diff --git a/core/lib/libtomcrypt/src/pk/sub.mk b/core/lib/libtomcrypt/src/pk/sub.mk
new file mode 100644
index 0000000..562642c
--- /dev/null
+++ b/core/lib/libtomcrypt/src/pk/sub.mk
@@ -0,0 +1,7 @@
+subdirs-$(_CFG_CRYPTO_WITH_ASN1) += asn1
+subdirs-$(CFG_CRYPTO_DSA) += dsa
+# PKCS1 paddings are used with RSA only
+subdirs-$(CFG_CRYPTO_RSA) += pkcs1
+subdirs-$(CFG_CRYPTO_RSA) += rsa
+subdirs-$(CFG_CRYPTO_DH) += dh
+subdirs-$(CFG_CRYPTO_ECC) += ecc
diff --git a/core/lib/libtomcrypt/src/prngs/fortuna.c b/core/lib/libtomcrypt/src/prngs/fortuna.c
new file mode 100644
index 0000000..173deea
--- /dev/null
+++ b/core/lib/libtomcrypt/src/prngs/fortuna.c
@@ -0,0 +1,430 @@
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file fortuna.c
+  Fortuna PRNG, Tom St Denis
+*/
+
+/* Implementation of Fortuna by Tom St Denis
+
+We deviate slightly here for reasons of simplicity [and to fit in the API].  First all "sources"
+in the AddEntropy function are fixed to 0.  Second since no reliable timer is provided
+we reseed automatically when len(pool0) >= 64 or every LTC_FORTUNA_WD calls to the read function */
+
+#ifdef LTC_FORTUNA
+
+/* requries LTC_SHA256 and AES  */
+#if !(defined(LTC_RIJNDAEL) && defined(LTC_SHA256))
+   #error LTC_FORTUNA requires LTC_SHA256 and LTC_RIJNDAEL (AES)
+#endif
+
+#ifndef LTC_FORTUNA_POOLS
+   #warning LTC_FORTUNA_POOLS was not previously defined (old headers?)
+   #define LTC_FORTUNA_POOLS 32
+#endif
+
+#if LTC_FORTUNA_POOLS < 4 || LTC_FORTUNA_POOLS > 32
+   #error LTC_FORTUNA_POOLS must be in [4..32]
+#endif
+
+const struct ltc_prng_descriptor fortuna_desc = {
+    "fortuna", 1024,
+    &fortuna_start,
+    &fortuna_add_entropy,
+    &fortuna_ready,
+    &fortuna_read,
+    &fortuna_done,
+    &fortuna_export,
+    &fortuna_import,
+    &fortuna_test
+};
+
+/* update the IV */
+static void fortuna_update_iv(prng_state *prng)
+{
+   int            x;
+   unsigned char *IV;
+   /* update IV */
+   IV = prng->fortuna.IV;
+   for (x = 0; x < 16; x++) {
+      IV[x] = (IV[x] + 1) & 255;
+      if (IV[x] != 0) break;
+   }
+}
+
+/* reseed the PRNG */
+static int fortuna_reseed(prng_state *prng)
+{
+   unsigned char tmp[MAXBLOCKSIZE];
+   hash_state    md;
+   int           err, x;
+
+   ++prng->fortuna.reset_cnt;
+
+   /* new K == LTC_SHA256(K || s) where s == LTC_SHA256(P0) || LTC_SHA256(P1) ... */
+   sha256_init(&md);
+   if ((err = sha256_process(&md, prng->fortuna.K, 32)) != CRYPT_OK) {
+      sha256_done(&md, tmp);
+      return err;
+   }
+
+   for (x = 0; x < LTC_FORTUNA_POOLS; x++) {
+       if (x == 0 || ((prng->fortuna.reset_cnt >> (x-1)) & 1) == 0) {
+          /* terminate this hash */
+          if ((err = sha256_done(&prng->fortuna.pool[x], tmp)) != CRYPT_OK) {
+             sha256_done(&md, tmp);
+             return err;
+          }
+          /* add it to the string */
+          if ((err = sha256_process(&md, tmp, 32)) != CRYPT_OK) {
+             sha256_done(&md, tmp);
+             return err;
+          }
+          /* reset this pool */
+          if ((err = sha256_init(&prng->fortuna.pool[x])) != CRYPT_OK) {
+             sha256_done(&md, tmp);
+             return err;
+          }
+       } else {
+          break;
+       }
+   }
+
+   /* finish key */
+   if ((err = sha256_done(&md, prng->fortuna.K)) != CRYPT_OK) {
+      return err;
+   }
+   if ((err = rijndael_setup(prng->fortuna.K, 32, 0, &prng->fortuna.skey)) != CRYPT_OK) {
+      return err;
+   }
+   fortuna_update_iv(prng);
+
+   /* reset pool len */
+   prng->fortuna.pool0_len = 0;
+   prng->fortuna.wd        = 0;
+
+
+#ifdef LTC_CLEAN_STACK
+   zeromem(&md, sizeof(md));
+   zeromem(tmp, sizeof(tmp));
+#endif
+
+   return CRYPT_OK;
+}
+
+/**
+  Start the PRNG
+  @param prng     [out] The PRNG state to initialize
+  @return CRYPT_OK if successful
+*/
+int fortuna_start(prng_state *prng)
+{
+   int err, x, y;
+   unsigned char tmp[MAXBLOCKSIZE];
+
+   LTC_ARGCHK(prng != NULL);
+
+   /* initialize the pools */
+   for (x = 0; x < LTC_FORTUNA_POOLS; x++) {
+       if ((err = sha256_init(&prng->fortuna.pool[x])) != CRYPT_OK) {
+          for (y = 0; y < x; y++) {
+              sha256_done(&prng->fortuna.pool[y], tmp);
+          }
+          return err;
+       }
+   }
+   prng->fortuna.pool_idx = prng->fortuna.pool0_len = prng->fortuna.wd = 0;
+   prng->fortuna.reset_cnt = 0;
+
+   /* reset bufs */
+   zeromem(prng->fortuna.K, 32);
+   if ((err = rijndael_setup(prng->fortuna.K, 32, 0, &prng->fortuna.skey)) != CRYPT_OK) {
+      for (x = 0; x < LTC_FORTUNA_POOLS; x++) {
+          sha256_done(&prng->fortuna.pool[x], tmp);
+      }
+      return err;
+   }
+   zeromem(prng->fortuna.IV, 16);
+
+   LTC_MUTEX_INIT(&prng->fortuna.prng_lock)
+
+   return CRYPT_OK;
+}
+
+/**
+  Add entropy to the PRNG state
+  @param in       The data to add
+  @param inlen    Length of the data to add
+  @param prng     PRNG state to update
+  @return CRYPT_OK if successful
+*/
+int fortuna_add_entropy(const unsigned char *in, unsigned long inlen, prng_state *prng)
+{
+   unsigned char tmp[2];
+   int           err;
+
+   LTC_ARGCHK(in  != NULL);
+   LTC_ARGCHK(prng != NULL);
+
+   LTC_MUTEX_LOCK(&prng->fortuna.prng_lock);
+
+   /* ensure inlen <= 32 */
+   if (inlen > 32) {
+      LTC_MUTEX_UNLOCK(&prng->fortuna.prng_lock);
+      return CRYPT_INVALID_ARG;
+   }
+
+   /* add s || length(in) || in to pool[pool_idx] */
+   tmp[0] = 0;
+   tmp[1] = (unsigned char)inlen;
+   if ((err = sha256_process(&prng->fortuna.pool[prng->fortuna.pool_idx], tmp, 2)) != CRYPT_OK) {
+      LTC_MUTEX_UNLOCK(&prng->fortuna.prng_lock);
+      return err;
+   }
+   if ((err = sha256_process(&prng->fortuna.pool[prng->fortuna.pool_idx], in, inlen)) != CRYPT_OK) {
+      LTC_MUTEX_UNLOCK(&prng->fortuna.prng_lock);
+      return err;
+   }
+   if (prng->fortuna.pool_idx == 0) {
+      prng->fortuna.pool0_len += inlen;
+   }
+   if (++(prng->fortuna.pool_idx) == LTC_FORTUNA_POOLS) {
+      prng->fortuna.pool_idx = 0;
+   }
+
+   LTC_MUTEX_UNLOCK(&prng->fortuna.prng_lock);
+   return CRYPT_OK;
+}
+
+/**
+  Make the PRNG ready to read from
+  @param prng   The PRNG to make active
+  @return CRYPT_OK if successful
+*/
+int fortuna_ready(prng_state *prng)
+{
+   return fortuna_reseed(prng);
+}
+
+/**
+  Read from the PRNG
+  @param out      Destination
+  @param outlen   Length of output
+  @param prng     The active PRNG to read from
+  @return Number of octets read
+*/
+unsigned long fortuna_read(unsigned char *out, unsigned long outlen, prng_state *prng)
+{
+   unsigned char tmp[16];
+   unsigned long tlen;
+
+   LTC_ARGCHK(out  != NULL);
+   LTC_ARGCHK(prng != NULL);
+
+   LTC_MUTEX_LOCK(&prng->fortuna.prng_lock);
+
+   /* do we have to reseed? */
+   if (++prng->fortuna.wd == LTC_FORTUNA_WD || prng->fortuna.pool0_len >= 64) {
+      if (fortuna_reseed(prng) != CRYPT_OK) {
+         LTC_MUTEX_UNLOCK(&prng->fortuna.prng_lock);
+         return 0;
+      }
+   }
+
+   /* now generate the blocks required */
+   tlen = outlen;
+
+   /* handle whole blocks without the extra XMEMCPY */
+   while (outlen >= 16) {
+      /* encrypt the IV and store it */
+      rijndael_ecb_encrypt(prng->fortuna.IV, out, &prng->fortuna.skey);
+      out += 16;
+      outlen -= 16;
+      fortuna_update_iv(prng);
+   }
+
+   /* left over bytes? */
+   if (outlen > 0) {
+      rijndael_ecb_encrypt(prng->fortuna.IV, tmp, &prng->fortuna.skey);
+      XMEMCPY(out, tmp, outlen);
+      fortuna_update_iv(prng);
+   }
+
+   /* generate new key */
+   rijndael_ecb_encrypt(prng->fortuna.IV, prng->fortuna.K   , &prng->fortuna.skey);
+   fortuna_update_iv(prng);
+
+   rijndael_ecb_encrypt(prng->fortuna.IV, prng->fortuna.K+16, &prng->fortuna.skey);
+   fortuna_update_iv(prng);
+
+   if (rijndael_setup(prng->fortuna.K, 32, 0, &prng->fortuna.skey) != CRYPT_OK) {
+      LTC_MUTEX_UNLOCK(&prng->fortuna.prng_lock);
+      return 0;
+   }
+
+#ifdef LTC_CLEAN_STACK
+   zeromem(tmp, sizeof(tmp));
+#endif
+   LTC_MUTEX_UNLOCK(&prng->fortuna.prng_lock);
+   return tlen;
+}
+
+/**
+  Terminate the PRNG
+  @param prng   The PRNG to terminate
+  @return CRYPT_OK if successful
+*/
+int fortuna_done(prng_state *prng)
+{
+   int           err, x;
+   unsigned char tmp[32];
+
+   LTC_ARGCHK(prng != NULL);
+   LTC_MUTEX_LOCK(&prng->fortuna.prng_lock);
+
+   /* terminate all the hashes */
+   for (x = 0; x < LTC_FORTUNA_POOLS; x++) {
+       if ((err = sha256_done(&(prng->fortuna.pool[x]), tmp)) != CRYPT_OK) {
+          LTC_MUTEX_UNLOCK(&prng->fortuna.prng_lock);
+          return err;
+       }
+   }
+   /* call cipher done when we invent one ;-) */
+
+#ifdef LTC_CLEAN_STACK
+   zeromem(tmp, sizeof(tmp));
+#endif
+
+   LTC_MUTEX_UNLOCK(&prng->fortuna.prng_lock);
+   return CRYPT_OK;
+}
+
+/**
+  Export the PRNG state
+  @param out       [out] Destination
+  @param outlen    [in/out] Max size and resulting size of the state
+  @param prng      The PRNG to export
+  @return CRYPT_OK if successful
+*/
+int fortuna_export(unsigned char *out, unsigned long *outlen, prng_state *prng)
+{
+   int         x, err;
+   hash_state *md;
+
+   LTC_ARGCHK(out    != NULL);
+   LTC_ARGCHK(outlen != NULL);
+   LTC_ARGCHK(prng   != NULL);
+
+   LTC_MUTEX_LOCK(&prng->fortuna.prng_lock);
+
+   /* we'll write bytes for s&g's */
+   if (*outlen < 32*LTC_FORTUNA_POOLS) {
+      LTC_MUTEX_UNLOCK(&prng->fortuna.prng_lock);
+      *outlen = 32*LTC_FORTUNA_POOLS;
+      return CRYPT_BUFFER_OVERFLOW;
+   }
+
+   md = XMALLOC(sizeof(hash_state));
+   if (md == NULL) {
+      LTC_MUTEX_UNLOCK(&prng->fortuna.prng_lock);
+      return CRYPT_MEM;
+   }
+
+   /* to emit the state we copy each pool, terminate it then hash it again so
+    * an attacker who sees the state can't determine the current state of the PRNG
+    */
+   for (x = 0; x < LTC_FORTUNA_POOLS; x++) {
+      /* copy the PRNG */
+      XMEMCPY(md, &(prng->fortuna.pool[x]), sizeof(*md));
+
+      /* terminate it */
+      if ((err = sha256_done(md, out+x*32)) != CRYPT_OK) {
+         goto LBL_ERR;
+      }
+
+      /* now hash it */
+      if ((err = sha256_init(md)) != CRYPT_OK) {
+         goto LBL_ERR;
+      }
+      if ((err = sha256_process(md, out+x*32, 32)) != CRYPT_OK) {
+         goto LBL_ERR;
+      }
+      if ((err = sha256_done(md, out+x*32)) != CRYPT_OK) {
+         goto LBL_ERR;
+      }
+   }
+   *outlen = 32*LTC_FORTUNA_POOLS;
+   err = CRYPT_OK;
+
+LBL_ERR:
+#ifdef LTC_CLEAN_STACK
+   zeromem(md, sizeof(*md));
+#endif
+   XFREE(md);
+   LTC_MUTEX_UNLOCK(&prng->fortuna.prng_lock);
+   return err;
+}
+
+/**
+  Import a PRNG state
+  @param in       The PRNG state
+  @param inlen    Size of the state
+  @param prng     The PRNG to import
+  @return CRYPT_OK if successful
+*/
+int fortuna_import(const unsigned char *in, unsigned long inlen, prng_state *prng)
+{
+   int err, x;
+
+   LTC_ARGCHK(in   != NULL);
+   LTC_ARGCHK(prng != NULL);
+
+   if (inlen != 32*LTC_FORTUNA_POOLS) {
+      return CRYPT_INVALID_ARG;
+   }
+
+   if ((err = fortuna_start(prng)) != CRYPT_OK) {
+      return err;
+   }
+   for (x = 0; x < LTC_FORTUNA_POOLS; x++) {
+      if ((err = fortuna_add_entropy(in+x*32, 32, prng)) != CRYPT_OK) {
+         return err;
+      }
+   }
+   return err;
+}
+
+/**
+  PRNG self-test
+  @return CRYPT_OK if successful, CRYPT_NOP if self-testing has been disabled
+*/
+int fortuna_test(void)
+{
+#ifndef LTC_TEST
+   return CRYPT_NOP;
+#else
+   int err;
+
+   if ((err = sha256_test()) != CRYPT_OK) {
+      return err;
+   }
+   return rijndael_test();
+#endif
+}
+
+#endif
+
+
+/* $Source$ */
+/* $Revision$ */
+/* $Date$ */
diff --git a/core/lib/libtomcrypt/src/prngs/rc4.c b/core/lib/libtomcrypt/src/prngs/rc4.c
new file mode 100644
index 0000000..2583451
--- /dev/null
+++ b/core/lib/libtomcrypt/src/prngs/rc4.c
@@ -0,0 +1,269 @@
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file rc4.c
+  LTC_RC4 PRNG, Tom St Denis
+*/
+
+#ifdef LTC_RC4
+
+const struct ltc_prng_descriptor rc4_desc =
+{
+   "rc4", 32,
+    &rc4_start,
+    &rc4_add_entropy,
+    &rc4_ready,
+    &rc4_read,
+    &rc4_done,
+    &rc4_export,
+    &rc4_import,
+    &rc4_test
+};
+
+/**
+  Start the PRNG
+  @param prng     [out] The PRNG state to initialize
+  @return CRYPT_OK if successful
+*/
+int rc4_start(prng_state *prng)
+{
+    LTC_ARGCHK(prng != NULL);
+
+    /* set keysize to zero */
+    prng->rc4.x = 0;
+
+    return CRYPT_OK;
+}
+
+/**
+  Add entropy to the PRNG state
+  @param in       The data to add
+  @param inlen    Length of the data to add
+  @param prng     PRNG state to update
+  @return CRYPT_OK if successful
+*/
+int rc4_add_entropy(const unsigned char *in, unsigned long inlen, prng_state *prng)
+{
+    LTC_ARGCHK(in  != NULL);
+    LTC_ARGCHK(prng != NULL);
+
+    /* trim as required */
+    if (prng->rc4.x + inlen > 256) {
+       if (prng->rc4.x == 256) {
+          /* I can't possibly accept another byte, ok maybe a mint wafer... */
+          return CRYPT_OK;
+       } else {
+          /* only accept part of it */
+          inlen = 256 - prng->rc4.x;
+       }
+    }
+
+    while (inlen--) {
+       prng->rc4.buf[prng->rc4.x++] = *in++;
+    }
+
+    return CRYPT_OK;
+
+}
+
+/**
+  Make the PRNG ready to read from
+  @param prng   The PRNG to make active
+  @return CRYPT_OK if successful
+*/
+int rc4_ready(prng_state *prng)
+{
+    unsigned char key[256], tmp, *s;
+    int keylen, x, y, j;
+
+    LTC_ARGCHK(prng != NULL);
+
+    /* extract the key */
+    s = prng->rc4.buf;
+    XMEMCPY(key, s, 256);
+    keylen = prng->rc4.x;
+
+    /* make LTC_RC4 perm and shuffle */
+    for (x = 0; x < 256; x++) {
+        s[x] = x;
+    }
+
+    for (j = x = y = 0; x < 256; x++) {
+        y = (y + prng->rc4.buf[x] + key[j++]) & 255;
+        if (j == keylen) {
+           j = 0;
+        }
+        tmp = s[x]; s[x] = s[y]; s[y] = tmp;
+    }
+    prng->rc4.x = 0;
+    prng->rc4.y = 0;
+
+#ifdef LTC_CLEAN_STACK
+    zeromem(key, sizeof(key));
+#endif
+
+    return CRYPT_OK;
+}
+
+/**
+  Read from the PRNG
+  @param out      Destination
+  @param outlen   Length of output
+  @param prng     The active PRNG to read from
+  @return Number of octets read
+*/
+unsigned long rc4_read(unsigned char *out, unsigned long outlen, prng_state *prng)
+{
+   unsigned char x, y, *s, tmp;
+   unsigned long n;
+
+   LTC_ARGCHK(out != NULL);
+   LTC_ARGCHK(prng != NULL);
+
+#ifdef LTC_VALGRIND
+   zeromem(out, outlen);
+#endif
+
+   n = outlen;
+   x = prng->rc4.x;
+   y = prng->rc4.y;
+   s = prng->rc4.buf;
+   while (outlen--) {
+      x = (x + 1) & 255;
+      y = (y + s[x]) & 255;
+      tmp = s[x]; s[x] = s[y]; s[y] = tmp;
+      tmp = (s[x] + s[y]) & 255;
+      *out++ ^= s[tmp];
+   }
+   prng->rc4.x = x;
+   prng->rc4.y = y;
+   return n;
+}
+
+/**
+  Terminate the PRNG
+  @param prng   The PRNG to terminate
+  @return CRYPT_OK if successful
+*/
+int rc4_done(prng_state *prng)
+{
+   LTC_ARGCHK(prng != NULL);
+   return CRYPT_OK;
+}
+
+/**
+  Export the PRNG state
+  @param out       [out] Destination
+  @param outlen    [in/out] Max size and resulting size of the state
+  @param prng      The PRNG to export
+  @return CRYPT_OK if successful
+*/
+int rc4_export(unsigned char *out, unsigned long *outlen, prng_state *prng)
+{
+   LTC_ARGCHK(outlen != NULL);
+   LTC_ARGCHK(out    != NULL);
+   LTC_ARGCHK(prng   != NULL);
+
+   if (*outlen < 32) {
+      *outlen = 32;
+      return CRYPT_BUFFER_OVERFLOW;
+   }
+
+   if (rc4_read(out, 32, prng) != 32) {
+      return CRYPT_ERROR_READPRNG;
+   }
+   *outlen = 32;
+
+   return CRYPT_OK;
+}
+
+/**
+  Import a PRNG state
+  @param in       The PRNG state
+  @param inlen    Size of the state
+  @param prng     The PRNG to import
+  @return CRYPT_OK if successful
+*/
+int rc4_import(const unsigned char *in, unsigned long inlen, prng_state *prng)
+{
+   int err;
+   LTC_ARGCHK(in   != NULL);
+   LTC_ARGCHK(prng != NULL);
+
+   if (inlen != 32) {
+      return CRYPT_INVALID_ARG;
+   }
+
+   if ((err = rc4_start(prng)) != CRYPT_OK) {
+      return err;
+   }
+   return rc4_add_entropy(in, 32, prng);
+}
+
+/**
+  PRNG self-test
+  @return CRYPT_OK if successful, CRYPT_NOP if self-testing has been disabled
+*/
+int rc4_test(void)
+{
+#if !defined(LTC_TEST) || defined(LTC_VALGRIND)
+   return CRYPT_NOP;
+#else
+   static const struct {
+      unsigned char key[8], pt[8], ct[8];
+   } tests[] = {
+{
+   { 0x01, 0x23, 0x45, 0x67, 0x89, 0xab, 0xcd, 0xef },
+   { 0x01, 0x23, 0x45, 0x67, 0x89, 0xab, 0xcd, 0xef },
+   { 0x75, 0xb7, 0x87, 0x80, 0x99, 0xe0, 0xc5, 0x96 }
+}
+};
+   prng_state prng;
+   unsigned char dst[8];
+   int err, x;
+
+   for (x = 0; x < (int)(sizeof(tests)/sizeof(tests[0])); x++) {
+       if ((err = rc4_start(&prng)) != CRYPT_OK) {
+          return err;
+       }
+       if ((err = rc4_add_entropy(tests[x].key, 8, &prng)) != CRYPT_OK) {
+          return err;
+       }
+       if ((err = rc4_ready(&prng)) != CRYPT_OK) {
+          return err;
+       }
+       XMEMCPY(dst, tests[x].pt, 8);
+       if (rc4_read(dst, 8, &prng) != 8) {
+          return CRYPT_ERROR_READPRNG;
+       }
+       rc4_done(&prng);
+       if (XMEMCMP(dst, tests[x].ct, 8)) {
+#if 0
+          int y;
+          printf("\n\nLTC_RC4 failed, I got:\n");
+          for (y = 0; y < 8; y++) printf("%02x ", dst[y]);
+          printf("\n");
+#endif
+          return CRYPT_FAIL_TESTVECTOR;
+       }
+   }
+   return CRYPT_OK;
+#endif
+}
+
+#endif
+
+
+/* $Source$ */
+/* $Revision$ */
+/* $Date$ */
diff --git a/core/lib/libtomcrypt/src/prngs/rng_get_bytes.c b/core/lib/libtomcrypt/src/prngs/rng_get_bytes.c
new file mode 100644
index 0000000..f0e591d
--- /dev/null
+++ b/core/lib/libtomcrypt/src/prngs/rng_get_bytes.c
@@ -0,0 +1,184 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/** 
+   @file rng_get_bytes.c
+   portable way to get secure random bits to feed a PRNG (Tom St Denis)
+*/
+
+#ifdef LTC_DEVRANDOM
+/* on *NIX read /dev/random */
+static unsigned long rng_nix(unsigned char *buf, unsigned long len, 
+                             void (*callback)(void))
+{
+#ifdef LTC_NO_FILE
+    LTC_UNUSED_PARAM(callback);
+    LTC_UNUSED_PARAM(buf);
+    LTC_UNUSED_PARAM(len);
+    return 0;
+#else
+    FILE *f;
+    unsigned long x;
+    LTC_UNUSED_PARAM(callback);
+#ifdef LTC_TRY_URANDOM_FIRST
+    f = fopen("/dev/urandom", "rb");
+    if (f == NULL)
+#endif /* LTC_TRY_URANDOM_FIRST */
+       f = fopen("/dev/random", "rb");
+
+    if (f == NULL) {
+       return 0;
+    }
+    
+    /* disable buffering */
+    if (setvbuf(f, NULL, _IONBF, 0) != 0) {
+       fclose(f);
+       return 0;
+    }   
+ 
+    x = (unsigned long)fread(buf, 1, (size_t)len, f);
+    fclose(f);
+    return x;
+#endif /* LTC_NO_FILE */
+}
+
+#endif /* LTC_DEVRANDOM */
+
+/* on ANSI C platforms with 100 < CLOCKS_PER_SEC < 10000 */
+#if defined(CLOCKS_PER_SEC) && !defined(WINCE)
+
+#define ANSI_RNG
+
+static unsigned long rng_ansic(unsigned char *buf, unsigned long len, 
+                               void (*callback)(void))
+{
+   clock_t t1;
+   int l, acc, bits, a, b;
+
+   if (XCLOCKS_PER_SEC < 100 || XCLOCKS_PER_SEC > 10000) {
+      return 0;
+   }
+
+   l = len;
+   bits = 8;
+   acc  = a = b = 0;
+   while (len--) {
+       if (callback != NULL) callback();
+       while (bits--) {
+          do {
+             t1 = XCLOCK(); while (t1 == XCLOCK()) a ^= 1;
+             t1 = XCLOCK(); while (t1 == XCLOCK()) b ^= 1;
+          } while (a == b);
+          acc = (acc << 1) | a;
+       }
+       *buf++ = acc; 
+       acc  = 0;
+       bits = 8;
+   }
+   acc = bits = a = b = 0;
+   return l;
+}
+
+#endif 
+
+/* Try the Microsoft CSP */
+#if defined(WIN32) || defined(_WIN32) || defined(WINCE)
+#ifndef _WIN32_WINNT
+  #define _WIN32_WINNT 0x0400
+#endif
+#ifdef WINCE
+   #define UNDER_CE
+   #define ARM
+#endif
+
+#define WIN32_LEAN_AND_MEAN
+#include <windows.h>
+#include <wincrypt.h>
+
+static unsigned long rng_win32(unsigned char *buf, unsigned long len, 
+                               void (*callback)(void))
+{
+   HCRYPTPROV hProv = 0;
+   LTC_UNUSED_PARAM(callback);
+   if (!CryptAcquireContext(&hProv, NULL, MS_DEF_PROV, PROV_RSA_FULL, 
+                            (CRYPT_VERIFYCONTEXT | CRYPT_MACHINE_KEYSET)) && 
+       !CryptAcquireContext (&hProv, NULL, MS_DEF_PROV, PROV_RSA_FULL, 
+                            CRYPT_VERIFYCONTEXT | CRYPT_MACHINE_KEYSET | CRYPT_NEWKEYSET))
+      return 0;
+
+   if (CryptGenRandom(hProv, len, buf) == TRUE) {
+      CryptReleaseContext(hProv, 0);
+      return len;
+   } else {
+      CryptReleaseContext(hProv, 0);
+      return 0;
+   }
+}
+
+#endif /* WIN32 */
+
+/**
+  Read the system RNG
+  @param out       Destination
+  @param outlen    Length desired (octets)
+  @param callback  Pointer to void function to act as "callback" when RNG is slow.  This can be NULL
+  @return Number of octets read
+*/     
+unsigned long rng_get_bytes(unsigned char *out, unsigned long outlen, 
+                            void (*callback)(void))
+{
+   unsigned long x;
+
+   LTC_ARGCHK(out != NULL);
+
+#if defined(LTC_DEVRANDOM)
+   x = rng_nix(out, outlen, callback);   if (x != 0) { return x; }
+#endif
+#if defined(WIN32) || defined(_WIN32) || defined(WINCE)
+   x = rng_win32(out, outlen, callback); if (x != 0) { return x; }
+#endif
+#ifdef ANSI_RNG
+   x = rng_ansic(out, outlen, callback); if (x != 0) { return x; }
+#endif
+   return 0;
+}
+
+/* $Source: /cvs/libtom/libtomcrypt/src/prngs/rng_get_bytes.c,v $ */
+/* $Revision: 1.7 $ */
+/* $Date: 2007/05/12 14:32:35 $ */
diff --git a/core/lib/libtomcrypt/src/prngs/rng_make_prng.c b/core/lib/libtomcrypt/src/prngs/rng_make_prng.c
new file mode 100644
index 0000000..3d67ba9
--- /dev/null
+++ b/core/lib/libtomcrypt/src/prngs/rng_make_prng.c
@@ -0,0 +1,96 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/** 
+  @file rng_make_prng.c
+  portable way to get secure random bits to feed a PRNG  (Tom St Denis)
+*/
+
+/**
+  Create a PRNG from a RNG
+  @param bits     Number of bits of entropy desired (64 ... 1024)
+  @param wprng    Index of which PRNG to setup
+  @param prng     [out] PRNG state to initialize
+  @param callback A pointer to a void function for when the RNG is slow, this can be NULL
+  @return CRYPT_OK if successful
+*/  
+int rng_make_prng(int bits, int wprng, prng_state *prng, 
+                  void (*callback)(void))
+{
+   unsigned char buf[256];
+   int err;
+   
+   LTC_ARGCHK(prng != NULL);
+
+   /* check parameter */
+   if ((err = prng_is_valid(wprng)) != CRYPT_OK) {
+      return err;
+   }
+
+   if (bits < 64 || bits > 1024) {
+      return CRYPT_INVALID_PRNGSIZE;
+   }
+
+   if ((err = prng_descriptor[wprng]->start(prng)) != CRYPT_OK) {
+      return err;
+   }
+
+   bits = ((bits/8)+((bits&7)!=0?1:0)) * 2;
+   if (rng_get_bytes(buf, (unsigned long)bits, callback) != (unsigned long)bits) {
+      return CRYPT_ERROR_READPRNG;
+   }
+
+   if ((err = prng_descriptor[wprng]->add_entropy(buf, (unsigned long)bits, prng)) != CRYPT_OK) {
+      return err;
+   }
+
+   if ((err = prng_descriptor[wprng]->ready(prng)) != CRYPT_OK) {
+      return err;
+   }
+
+   #ifdef LTC_CLEAN_STACK
+      zeromem(buf, sizeof(buf));
+   #endif
+   return CRYPT_OK;
+}
+
+
+/* $Source: /cvs/libtom/libtomcrypt/src/prngs/rng_make_prng.c,v $ */
+/* $Revision: 1.5 $ */
+/* $Date: 2006/12/28 01:27:24 $ */
diff --git a/core/lib/libtomcrypt/src/prngs/sprng.c b/core/lib/libtomcrypt/src/prngs/sprng.c
new file mode 100644
index 0000000..7244407
--- /dev/null
+++ b/core/lib/libtomcrypt/src/prngs/sprng.c
@@ -0,0 +1,175 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+   @file sprng.c
+   Secure PRNG, Tom St Denis
+*/
+   
+/* A secure PRNG using the RNG functions.  Basically this is a
+ * wrapper that allows you to use a secure RNG as a PRNG
+ * in the various other functions.
+ */
+
+#ifdef LTC_SPRNG
+
+const struct ltc_prng_descriptor sprng_desc =
+{
+    "sprng", 0,
+    &sprng_start,
+    &sprng_add_entropy,
+    &sprng_ready,
+    &sprng_read,
+    &sprng_done,
+    &sprng_export,
+    &sprng_import,
+    &sprng_test
+};
+
+/**
+  Start the PRNG
+  @param prng     [out] The PRNG state to initialize
+  @return CRYPT_OK if successful
+*/  
+int sprng_start(prng_state *prng)
+{
+   LTC_UNUSED_PARAM(prng);
+   return CRYPT_OK;
+}
+
+/**
+  Add entropy to the PRNG state
+  @param in       The data to add
+  @param inlen    Length of the data to add
+  @param prng     PRNG state to update
+  @return CRYPT_OK if successful
+*/  
+int sprng_add_entropy(const unsigned char *in, unsigned long inlen, prng_state *prng)
+{
+   LTC_UNUSED_PARAM(in);
+   LTC_UNUSED_PARAM(inlen);
+   LTC_UNUSED_PARAM(prng);
+   return CRYPT_OK;
+}
+
+/**
+  Make the PRNG ready to read from
+  @param prng   The PRNG to make active
+  @return CRYPT_OK if successful
+*/  
+int sprng_ready(prng_state *prng)
+{
+   LTC_UNUSED_PARAM(prng);
+   return CRYPT_OK;
+}
+
+/**
+  Read from the PRNG
+  @param out      Destination
+  @param outlen   Length of output
+  @param prng     The active PRNG to read from
+  @return Number of octets read
+*/  
+unsigned long sprng_read(unsigned char *out, unsigned long outlen, prng_state *prng)
+{
+   LTC_ARGCHK(out != NULL);
+   LTC_UNUSED_PARAM(prng);
+   return rng_get_bytes(out, outlen, NULL);
+}
+
+/**
+  Terminate the PRNG
+  @param prng   The PRNG to terminate
+  @return CRYPT_OK if successful
+*/  
+int sprng_done(prng_state *prng)
+{
+   LTC_UNUSED_PARAM(prng);
+   return CRYPT_OK;
+}
+
+/**
+  Export the PRNG state
+  @param out       [out] Destination
+  @param outlen    [in/out] Max size and resulting size of the state
+  @param prng      The PRNG to export
+  @return CRYPT_OK if successful
+*/  
+int sprng_export(unsigned char *out, unsigned long *outlen, prng_state *prng)
+{
+   LTC_ARGCHK(outlen != NULL);
+   LTC_UNUSED_PARAM(out);
+   LTC_UNUSED_PARAM(prng);
+
+   *outlen = 0;
+   return CRYPT_OK;
+}
+ 
+/**
+  Import a PRNG state
+  @param in       The PRNG state
+  @param inlen    Size of the state
+  @param prng     The PRNG to import
+  @return CRYPT_OK if successful
+*/  
+int sprng_import(const unsigned char *in, unsigned long inlen, prng_state *prng)
+{
+  LTC_UNUSED_PARAM(in);
+  LTC_UNUSED_PARAM(inlen);
+  LTC_UNUSED_PARAM(prng);
+   return CRYPT_OK;
+}
+
+/**
+  PRNG self-test
+  @return CRYPT_OK if successful, CRYPT_NOP if self-testing has been disabled
+*/  
+int sprng_test(void)
+{
+   return CRYPT_OK;
+}
+
+#endif
+
+
+ 
+
+/* $Source: /cvs/libtom/libtomcrypt/src/prngs/sprng.c,v $ */
+/* $Revision: 1.6 $ */
+/* $Date: 2007/05/12 14:32:35 $ */
diff --git a/core/lib/libtomcrypt/src/prngs/sub.mk b/core/lib/libtomcrypt/src/prngs/sub.mk
new file mode 100644
index 0000000..6aeaa77
--- /dev/null
+++ b/core/lib/libtomcrypt/src/prngs/sub.mk
@@ -0,0 +1,7 @@
+cflags-y += -Wno-unused-parameter -Wno-unused-variable
+
+srcs-y += rng_get_bytes.c
+srcs-y += rng_make_prng.c
+srcs-y += sprng.c
+srcs-y += rc4.c
+srcs-$(_CFG_CRYPTO_WITH_FORTUNA_PRNG) += fortuna.c
diff --git a/core/lib/libtomcrypt/src/prngs/yarrow.c b/core/lib/libtomcrypt/src/prngs/yarrow.c
new file mode 100644
index 0000000..f245cde
--- /dev/null
+++ b/core/lib/libtomcrypt/src/prngs/yarrow.c
@@ -0,0 +1,391 @@
+/*
+ * Copyright (c) 2001-2007, Tom St Denis
+ * 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.
+ */
+
+/* LibTomCrypt, modular cryptographic library -- Tom St Denis
+ *
+ * LibTomCrypt is a library that provides various cryptographic
+ * algorithms in a highly modular and flexible manner.
+ *
+ * The library is free for all purposes without any express
+ * guarantee it works.
+ *
+ * Tom St Denis, tomstdenis@gmail.com, http://libtom.org
+ */
+#include "tomcrypt.h"
+
+/**
+  @file yarrow.c
+  Yarrow PRNG, Tom St Denis
+*/  
+
+#ifdef LTC_YARROW
+
+const struct ltc_prng_descriptor yarrow_desc =
+{
+    "yarrow", 64,
+    &yarrow_start,
+    &yarrow_add_entropy,
+    &yarrow_ready,
+    &yarrow_read,
+    &yarrow_done,
+    &yarrow_export,
+    &yarrow_import,
+    &yarrow_test
+};
+
+/**
+  Start the PRNG
+  @param prng     [out] The PRNG state to initialize
+  @return CRYPT_OK if successful
+*/  
+int yarrow_start(prng_state *prng)
+{
+   int err;
+   
+   LTC_ARGCHK(prng != NULL);
+
+   /* these are the default hash/cipher combo used */
+#ifdef LTC_RIJNDAEL
+#if    LTC_YARROW_AES==0
+   prng->yarrow.cipher = register_cipher(&rijndael_enc_desc);
+#elif  LTC_YARROW_AES==1
+   prng->yarrow.cipher = register_cipher(&aes_enc_desc);
+#elif  LTC_YARROW_AES==2
+   prng->yarrow.cipher = register_cipher(&rijndael_desc);
+#elif  LTC_YARROW_AES==3
+   prng->yarrow.cipher = register_cipher(&aes_desc);
+#endif
+#elif defined(LTC_BLOWFISH)
+   prng->yarrow.cipher = register_cipher(&blowfish_desc);
+#elif defined(LTC_TWOFISH)
+   prng->yarrow.cipher = register_cipher(&twofish_desc);
+#elif defined(LTC_RC6)
+   prng->yarrow.cipher = register_cipher(&rc6_desc);
+#elif defined(LTC_RC5)
+   prng->yarrow.cipher = register_cipher(&rc5_desc);
+#elif defined(LTC_SAFERP)
+   prng->yarrow.cipher = register_cipher(&saferp_desc);
+#elif defined(LTC_RC2)
+   prng->yarrow.cipher = register_cipher(&rc2_desc);
+#elif defined(LTC_NOEKEON)   
+   prng->yarrow.cipher = register_cipher(&noekeon_desc);
+#elif defined(LTC_ANUBIS)   
+   prng->yarrow.cipher = register_cipher(&anubis_desc);
+#elif defined(LTC_KSEED)   
+   prng->yarrow.cipher = register_cipher(&kseed_desc);
+#elif defined(LTC_KHAZAD)   
+   prng->yarrow.cipher = register_cipher(&khazad_desc);
+#elif defined(LTC_CAST5)
+   prng->yarrow.cipher = register_cipher(&cast5_desc);
+#elif defined(LTC_XTEA)
+   prng->yarrow.cipher = register_cipher(&xtea_desc);
+#elif defined(LTC_SAFER)
+   prng->yarrow.cipher = register_cipher(&safer_sk128_desc);
+#elif defined(LTC_DES)
+   prng->yarrow.cipher = register_cipher(&des3_desc);
+#else
+   #error LTC_YARROW needs at least one CIPHER
+#endif
+   if ((err = cipher_is_valid(prng->yarrow.cipher)) != CRYPT_OK) {
+      return err;
+   }
+
+#ifdef LTC_SHA256
+   prng->yarrow.hash   = register_hash(&sha256_desc);
+#elif defined(LTC_SHA512)
+   prng->yarrow.hash   = register_hash(&sha512_desc);
+#elif defined(LTC_TIGER)
+   prng->yarrow.hash   = register_hash(&tiger_desc);
+#elif defined(LTC_SHA1)
+   prng->yarrow.hash   = register_hash(&sha1_desc);
+#elif defined(LTC_RIPEMD320)
+   prng->yarrow.hash   = register_hash(&rmd320_desc);
+#elif defined(LTC_RIPEMD256)
+   prng->yarrow.hash   = register_hash(&rmd256_desc);
+#elif defined(LTC_RIPEMD160)
+   prng->yarrow.hash   = register_hash(&rmd160_desc);
+#elif defined(LTC_RIPEMD128)
+   prng->yarrow.hash   = register_hash(&rmd128_desc);
+#elif defined(LTC_MD5)
+   prng->yarrow.hash   = register_hash(&md5_desc);
+#elif defined(LTC_MD4)
+   prng->yarrow.hash   = register_hash(&md4_desc);
+#elif defined(LTC_MD2)
+   prng->yarrow.hash   = register_hash(&md2_desc);
+#elif defined(LTC_WHIRLPOOL)
+   prng->yarrow.hash   = register_hash(&whirlpool_desc);
+#else
+   #error LTC_YARROW needs at least one HASH
+#endif
+   if ((err = hash_is_valid(prng->yarrow.hash)) != CRYPT_OK) {
+      return err;
+   }
+
+   /* zero the memory used */
+   zeromem(prng->yarrow.pool, sizeof(prng->yarrow.pool));
+   LTC_MUTEX_INIT(&prng->yarrow.prng_lock)
+
+   return CRYPT_OK;
+}
+
+/**
+  Add entropy to the PRNG state
+  @param in       The data to add
+  @param inlen    Length of the data to add
+  @param prng     PRNG state to update
+  @return CRYPT_OK if successful
+*/  
+int yarrow_add_entropy(const unsigned char *in, unsigned long inlen, prng_state *prng)
+{
+   hash_state md;
+   int err;
+
+   LTC_ARGCHK(in  != NULL);
+   LTC_ARGCHK(prng != NULL);
+   
+   LTC_MUTEX_LOCK(&prng->yarrow.prng_lock);
+   
+   if ((err = hash_is_valid(prng->yarrow.hash)) != CRYPT_OK) {
+      LTC_MUTEX_UNLOCK(&prng->yarrow.prng_lock);
+      return err;
+   }
+
+   /* start the hash */
+   if ((err = hash_descriptor[prng->yarrow.hash].init(&md)) != CRYPT_OK) {
+      LTC_MUTEX_UNLOCK(&prng->yarrow.prng_lock);
+      return err; 
+   }
+
+   /* hash the current pool */
+   if ((err = hash_descriptor[prng->yarrow.hash].process(&md, prng->yarrow.pool, 
+                                                        hash_descriptor[prng->yarrow.hash].hashsize)) != CRYPT_OK) {
+      LTC_MUTEX_UNLOCK(&prng->yarrow.prng_lock);
+      return err;
+   }
+
+   /* add the new entropy */
+   if ((err = hash_descriptor[prng->yarrow.hash].process(&md, in, inlen)) != CRYPT_OK) {
+      LTC_MUTEX_UNLOCK(&prng->yarrow.prng_lock);
+      return err;
+   }
+
+   /* store result */
+   if ((err = hash_descriptor[prng->yarrow.hash].done(&md, prng->yarrow.pool)) != CRYPT_OK) {
+      LTC_MUTEX_UNLOCK(&prng->yarrow.prng_lock);
+      return err;
+   }
+
+   LTC_MUTEX_UNLOCK(&prng->yarrow.prng_lock);
+   return CRYPT_OK;
+}
+
+/**
+  Make the PRNG ready to read from
+  @param prng   The PRNG to make active
+  @return CRYPT_OK if successful
+*/  
+int yarrow_ready(prng_state *prng)
+{
+   int ks, err;
+
+   LTC_ARGCHK(prng != NULL);
+   LTC_MUTEX_LOCK(&prng->yarrow.prng_lock);
+
+   if ((err = hash_is_valid(prng->yarrow.hash)) != CRYPT_OK) {
+      LTC_MUTEX_UNLOCK(&prng->yarrow.prng_lock);
+      return err;
+   }
+   
+   if ((err = cipher_is_valid(prng->yarrow.cipher)) != CRYPT_OK) {
+      LTC_MUTEX_UNLOCK(&prng->yarrow.prng_lock);
+      return err;
+   }
+
+   /* setup CTR mode using the "pool" as the key */
+   ks = (int)hash_descriptor[prng->yarrow.hash].hashsize;
+   if ((err = cipher_descriptor[prng->yarrow.cipher].keysize(&ks)) != CRYPT_OK) {
+      LTC_MUTEX_UNLOCK(&prng->yarrow.prng_lock);
+      return err;
+   }
+
+   if ((err = ctr_start(prng->yarrow.cipher,     /* what cipher to use */
+                        prng->yarrow.pool,       /* IV */
+                        prng->yarrow.pool, ks,   /* KEY and key size */
+                        0,                       /* number of rounds */
+                        CTR_COUNTER_LITTLE_ENDIAN, /* little endian counter */
+                        &prng->yarrow.ctr)) != CRYPT_OK) {
+      LTC_MUTEX_UNLOCK(&prng->yarrow.prng_lock);
+      return err;
+   }
+   LTC_MUTEX_UNLOCK(&prng->yarrow.prng_lock);
+   return CRYPT_OK;
+}
+
+/**
+  Read from the PRNG
+  @param out      Destination
+  @param outlen   Length of output
+  @param prng     The active PRNG to read from
+  @return Number of octets read
+*/  
+unsigned long yarrow_read(unsigned char *out, unsigned long outlen, prng_state *prng)
+{
+   LTC_ARGCHK(out  != NULL);
+   LTC_ARGCHK(prng != NULL);
+
+   LTC_MUTEX_LOCK(&prng->yarrow.prng_lock);
+
+   /* put out in predictable state first */
+   zeromem(out, outlen);
+   
+   /* now randomize it */
+   if (ctr_encrypt(out, out, outlen, &prng->yarrow.ctr) != CRYPT_OK) {
+      LTC_MUTEX_UNLOCK(&prng->yarrow.prng_lock);
+      return 0;
+   }
+   LTC_MUTEX_UNLOCK(&prng->yarrow.prng_lock);
+   return outlen;
+}
+
+/**
+  Terminate the PRNG
+  @param prng   The PRNG to terminate
+  @return CRYPT_OK if successful
+*/  
+int yarrow_done(prng_state *prng)
+{
+   int err;
+   LTC_ARGCHK(prng != NULL);
+
+   LTC_MUTEX_LOCK(&prng->yarrow.prng_lock);
+
+   /* call cipher done when we invent one ;-) */
+
+   /* we invented one */
+   err = ctr_done(&prng->yarrow.ctr);
+   
+   LTC_MUTEX_UNLOCK(&prng->yarrow.prng_lock);
+   return err;
+}
+
+/**
+  Export the PRNG state
+  @param out       [out] Destination
+  @param outlen    [in/out] Max size and resulting size of the state
+  @param prng      The PRNG to export
+  @return CRYPT_OK if successful
+*/  
+int yarrow_export(unsigned char *out, unsigned long *outlen, prng_state *prng)
+{
+   LTC_ARGCHK(out    != NULL);
+   LTC_ARGCHK(outlen != NULL);
+   LTC_ARGCHK(prng   != NULL);
+
+   LTC_MUTEX_LOCK(&prng->yarrow.prng_lock);
+
+   /* we'll write 64 bytes for s&g's */
+   if (*outlen < 64) {
+      LTC_MUTEX_UNLOCK(&prng->yarrow.prng_lock);
+      *outlen = 64;
+      return CRYPT_BUFFER_OVERFLOW;
+   }
+
+   if (yarrow_read(out, 64, prng) != 64) {
+      LTC_MUTEX_UNLOCK(&prng->yarrow.prng_lock);
+      return CRYPT_ERROR_READPRNG;
+   }
+   *outlen = 64;
+
+   return CRYPT_OK;
+}
+ 
+/**
+  Import a PRNG state
+  @param in       The PRNG state
+  @param inlen    Size of the state
+  @param prng     The PRNG to import
+  @return CRYPT_OK if successful
+*/  
+int yarrow_import(const unsigned char *in, unsigned long inlen, prng_state *prng)
+{
+   int err;
+
+   LTC_ARGCHK(in   != NULL);
+   LTC_ARGCHK(prng != NULL);
+   
+   LTC_MUTEX_LOCK(&prng->yarrow.prng_lock);
+
+   if (inlen != 64) {
+      LTC_MUTEX_UNLOCK(&prng->yarrow.prng_lock);
+      return CRYPT_INVALID_ARG;
+   }
+
+   if ((err = yarrow_start(prng)) != CRYPT_OK) {
+      LTC_MUTEX_UNLOCK(&prng->yarrow.prng_lock);
+      return err;
+   }
+   err = yarrow_add_entropy(in, 64, prng);
+   LTC_MUTEX_UNLOCK(&prng->yarrow.prng_lock);
+   return err;
+}
+
+/**
+  PRNG self-test
+  @return CRYPT_OK if successful, CRYPT_NOP if self-testing has been disabled
+*/  
+int yarrow_test(void)
+{
+#ifndef LTC_TEST
+   return CRYPT_NOP;
+#else
+   int err;
+   prng_state prng;
+
+   if ((err = yarrow_start(&prng)) != CRYPT_OK) {
+      return err;
+   }
+   
+   /* now let's test the hash/cipher that was chosen */
+   if (cipher_descriptor[prng.yarrow.cipher].test &&
+       ((err = cipher_descriptor[prng.yarrow.cipher].test()) != CRYPT_OK)) {
+      return err;
+   }
+   if (hash_descriptor[prng.yarrow.hash].test &&
+       ((err = hash_descriptor[prng.yarrow.hash].test()) != CRYPT_OK)) {
+      return err;
+   }
+
+   return CRYPT_OK;
+#endif
+}
+
+#endif
+
+
+/* $Source: /cvs/libtom/libtomcrypt/src/prngs/yarrow.c,v $ */
+/* $Revision: 1.16 $ */
+/* $Date: 2007/05/12 14:32:35 $ */
diff --git a/core/lib/libtomcrypt/src/sub.mk b/core/lib/libtomcrypt/src/sub.mk
new file mode 100644
index 0000000..89e74bd
--- /dev/null
+++ b/core/lib/libtomcrypt/src/sub.mk
@@ -0,0 +1,19 @@
+ifdef _CFG_CRYPTO_WITH_ACIPHER
+srcs-y += mpa_desc.c
+# Get mpa.h which normally is an internal .h file
+cppflags-mpa_desc.c-y += -Ilib/libmpa
+cflags-mpa_desc.c-y += -Wno-declaration-after-statement
+cflags-mpa_desc.c-y += -Wno-unused-parameter
+endif
+
+srcs-y += tee_ltc_provider.c
+
+subdirs-$(_CFG_CRYPTO_WITH_CIPHER) += ciphers
+subdirs-$(_CFG_CRYPTO_WITH_AUTHENC) += encauth
+subdirs-y += hashes
+subdirs-$(_CFG_CRYPTO_WITH_MAC) += mac
+subdirs-$(_CFG_CRYPTO_WITH_ACIPHER) += math
+subdirs-y += misc
+subdirs-y += modes
+subdirs-$(_CFG_CRYPTO_WITH_ACIPHER) += pk
+subdirs-$(CFG_WITH_SOFTWARE_PRNG) += prngs
diff --git a/core/lib/libtomcrypt/src/tee_ltc_provider.c b/core/lib/libtomcrypt/src/tee_ltc_provider.c
new file mode 100644
index 0000000..06dc983
--- /dev/null
+++ b/core/lib/libtomcrypt/src/tee_ltc_provider.c
@@ -0,0 +1,3116 @@
+/*
+ * Copyright (c) 2014, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <assert.h>
+#include <tee/tee_cryp_provider.h>
+#include <tee/tee_cryp_utl.h>
+
+#include <tomcrypt.h>
+#include <mpalib.h>
+#include <stdlib.h>
+#include <string.h>
+#include <utee_defines.h>
+#include <trace.h>
+#include <tee_api_types.h>
+#include <string_ext.h>
+#include <util.h>
+#include <kernel/panic.h>
+#include "tomcrypt_mpa.h"
+
+#if defined(CFG_WITH_VFP)
+#include <tomcrypt_arm_neon.h>
+#include <kernel/thread.h>
+#endif
+
+#if !defined(CFG_WITH_SOFTWARE_PRNG)
+
+/* Random generator */
+static int prng_mpa_start(union Prng_state *prng __unused)
+{
+	return CRYPT_OK;
+}
+
+static int prng_mpa_add_entropy(const unsigned char *in __unused,
+				unsigned long inlen __unused,
+				union Prng_state *prng __unused)
+{
+	/* No entropy is required */
+	return CRYPT_OK;
+}
+
+static int prng_mpa_ready(union Prng_state *prng __unused)
+{
+	return CRYPT_OK;
+}
+
+static unsigned long prng_mpa_read(unsigned char *out, unsigned long outlen,
+				   union Prng_state *prng __unused)
+{
+	if (TEE_SUCCESS == get_rng_array(out, outlen))
+		return outlen;
+	else
+		return 0;
+}
+
+static int prng_mpa_done(union Prng_state *prng __unused)
+{
+	return CRYPT_OK;
+}
+
+static int prng_mpa_export(unsigned char *out __unused,
+			   unsigned long *outlen __unused,
+			   union Prng_state *prng __unused)
+{
+	return CRYPT_OK;
+}
+
+static int prng_mpa_import(const unsigned char *in  __unused,
+			   unsigned long inlen __unused,
+			   union Prng_state *prng __unused)
+{
+	return CRYPT_OK;
+}
+
+static int prng_mpa_test(void)
+{
+	return CRYPT_OK;
+}
+
+static const struct ltc_prng_descriptor prng_mpa_desc = {
+	.name = "prng_mpa",
+	.export_size = 64,
+	.start = &prng_mpa_start,
+	.add_entropy = &prng_mpa_add_entropy,
+	.ready = &prng_mpa_ready,
+	.read = &prng_mpa_read,
+	.done = &prng_mpa_done,
+	.pexport = &prng_mpa_export,
+	.pimport = &prng_mpa_import,
+	.test = &prng_mpa_test,
+};
+
+#endif /* !CFG_WITH_SOFTWARE_PRNG */
+
+struct tee_ltc_prng {
+	int index;
+	const char *name;
+	prng_state state;
+	bool inited;
+};
+
+static struct tee_ltc_prng _tee_ltc_prng =
+#if defined(CFG_WITH_SOFTWARE_PRNG)
+	{
+#if defined(_CFG_CRYPTO_WITH_FORTUNA_PRNG)
+		.name = "fortuna",
+#else
+		/*
+		 * we need AES and SHA256 for fortuna PRNG,
+		 * if the system configuration can't provide those,
+		 * fallback to RC4
+		 */
+		.name = "rc4",
+#endif
+	};
+#else
+	{
+		.name = "prng_mpa",
+	};
+#endif
+
+static struct tee_ltc_prng *tee_ltc_get_prng(void)
+{
+	return &_tee_ltc_prng;
+}
+
+static TEE_Result tee_ltc_prng_init(struct tee_ltc_prng *prng)
+{
+	int res;
+	int prng_index;
+
+	assert(prng);
+
+	prng_index = find_prng(prng->name);
+	if (prng_index == -1)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	if (!prng->inited) {
+		res = prng_descriptor[prng_index]->start(&prng->state);
+		if (res != CRYPT_OK)
+			return TEE_ERROR_BAD_STATE;
+
+		res = prng_descriptor[prng_index]->ready(&prng->state);
+		if (res != CRYPT_OK)
+			return TEE_ERROR_BAD_STATE;
+		prng->inited = true;
+	}
+
+	prng->index = prng_index;
+
+	plat_prng_add_jitter_entropy();
+
+	return  TEE_SUCCESS;
+}
+
+/*
+ * tee_ltc_reg_algs(): Registers
+ *	- algorithms
+ *	- hash
+ *	- prng (pseudo random generator)
+ */
+
+static void tee_ltc_reg_algs(void)
+{
+#if defined(CFG_CRYPTO_AES)
+	register_cipher(&aes_desc);
+#endif
+#if defined(CFG_CRYPTO_DES)
+	register_cipher(&des_desc);
+	register_cipher(&des3_desc);
+#endif
+#if defined(CFG_CRYPTO_MD5)
+	register_hash(&md5_desc);
+#endif
+#if defined(CFG_CRYPTO_SHA1)
+	register_hash(&sha1_desc);
+#endif
+#if defined(CFG_CRYPTO_SHA224)
+	register_hash(&sha224_desc);
+#endif
+#if defined(CFG_CRYPTO_SHA256)
+	register_hash(&sha256_desc);
+#endif
+#if defined(CFG_CRYPTO_SHA384)
+	register_hash(&sha384_desc);
+#endif
+#if defined(CFG_CRYPTO_SHA512)
+	register_hash(&sha512_desc);
+#endif
+
+#if defined(CFG_WITH_SOFTWARE_PRNG)
+#if defined(_CFG_CRYPTO_WITH_FORTUNA_PRNG)
+	register_prng(&fortuna_desc);
+#else
+	register_prng(&rc4_desc);
+#endif
+#else
+	register_prng(&prng_mpa_desc);
+#endif
+}
+
+
+#if defined(_CFG_CRYPTO_WITH_HASH) || defined(CFG_CRYPTO_RSA) || \
+	defined(CFG_CRYPTO_HMAC)
+
+/*
+ * Compute the LibTomCrypt "hashindex" given a TEE Algorithm "algo"
+ * Return
+ * - TEE_SUCCESS in case of success,
+ * - TEE_ERROR_BAD_PARAMETERS in case algo is not a valid algo
+ * - TEE_ERROR_NOT_SUPPORTED in case algo is not supported by LTC
+ * Return -1 in case of error
+ */
+static TEE_Result tee_algo_to_ltc_hashindex(uint32_t algo, int *ltc_hashindex)
+{
+	switch (algo) {
+#if defined(CFG_CRYPTO_SHA1)
+	case TEE_ALG_RSASSA_PKCS1_V1_5_SHA1:
+	case TEE_ALG_RSASSA_PKCS1_PSS_MGF1_SHA1:
+	case TEE_ALG_RSAES_PKCS1_OAEP_MGF1_SHA1:
+	case TEE_ALG_SHA1:
+	case TEE_ALG_DSA_SHA1:
+	case TEE_ALG_HMAC_SHA1:
+		*ltc_hashindex = find_hash("sha1");
+		break;
+#endif
+#if defined(CFG_CRYPTO_MD5)
+	case TEE_ALG_RSASSA_PKCS1_V1_5_MD5:
+	case TEE_ALG_MD5:
+	case TEE_ALG_HMAC_MD5:
+		*ltc_hashindex = find_hash("md5");
+		break;
+#endif
+#if defined(CFG_CRYPTO_SHA224)
+	case TEE_ALG_RSASSA_PKCS1_V1_5_SHA224:
+	case TEE_ALG_RSASSA_PKCS1_PSS_MGF1_SHA224:
+	case TEE_ALG_RSAES_PKCS1_OAEP_MGF1_SHA224:
+	case TEE_ALG_SHA224:
+	case TEE_ALG_DSA_SHA224:
+	case TEE_ALG_HMAC_SHA224:
+		*ltc_hashindex = find_hash("sha224");
+		break;
+#endif
+#if defined(CFG_CRYPTO_SHA256)
+	case TEE_ALG_RSASSA_PKCS1_V1_5_SHA256:
+	case TEE_ALG_RSASSA_PKCS1_PSS_MGF1_SHA256:
+	case TEE_ALG_RSAES_PKCS1_OAEP_MGF1_SHA256:
+	case TEE_ALG_SHA256:
+	case TEE_ALG_DSA_SHA256:
+	case TEE_ALG_HMAC_SHA256:
+		*ltc_hashindex = find_hash("sha256");
+		break;
+#endif
+#if defined(CFG_CRYPTO_SHA384)
+	case TEE_ALG_RSASSA_PKCS1_V1_5_SHA384:
+	case TEE_ALG_RSASSA_PKCS1_PSS_MGF1_SHA384:
+	case TEE_ALG_RSAES_PKCS1_OAEP_MGF1_SHA384:
+	case TEE_ALG_SHA384:
+	case TEE_ALG_HMAC_SHA384:
+		*ltc_hashindex = find_hash("sha384");
+		break;
+#endif
+#if defined(CFG_CRYPTO_SHA512)
+	case TEE_ALG_RSASSA_PKCS1_V1_5_SHA512:
+	case TEE_ALG_RSASSA_PKCS1_PSS_MGF1_SHA512:
+	case TEE_ALG_RSAES_PKCS1_OAEP_MGF1_SHA512:
+	case TEE_ALG_SHA512:
+	case TEE_ALG_HMAC_SHA512:
+		*ltc_hashindex = find_hash("sha512");
+		break;
+#endif
+	case TEE_ALG_RSAES_PKCS1_V1_5:
+		/* invalid one. but it should not be used anyway */
+		*ltc_hashindex = -1;
+		return TEE_SUCCESS;
+
+	default:
+		return TEE_ERROR_BAD_PARAMETERS;
+	}
+
+	if (*ltc_hashindex < 0)
+		return TEE_ERROR_NOT_SUPPORTED;
+	else
+		return TEE_SUCCESS;
+}
+#endif /* defined(_CFG_CRYPTO_WITH_HASH) ||
+	  defined(_CFG_CRYPTO_WITH_ACIPHER) || defined(_CFG_CRYPTO_WITH_MAC) */
+
+#if defined(_CFG_CRYPTO_WITH_CIPHER) || defined(_CFG_CRYPTO_WITH_MAC) || \
+	defined(_CFG_CRYPTO_WITH_AUTHENC)
+/*
+ * Compute the LibTomCrypt "cipherindex" given a TEE Algorithm "algo"
+ * Return
+ * - TEE_SUCCESS in case of success,
+ * - TEE_ERROR_BAD_PARAMETERS in case algo is not a valid algo
+ * - TEE_ERROR_NOT_SUPPORTED in case algo is not supported by LTC
+ * Return -1 in case of error
+ */
+static TEE_Result tee_algo_to_ltc_cipherindex(uint32_t algo,
+					      int *ltc_cipherindex)
+{
+	switch (algo) {
+#if defined(CFG_CRYPTO_AES)
+	case TEE_ALG_AES_CBC_MAC_NOPAD:
+	case TEE_ALG_AES_CBC_MAC_PKCS5:
+	case TEE_ALG_AES_CMAC:
+	case TEE_ALG_AES_ECB_NOPAD:
+	case TEE_ALG_AES_CBC_NOPAD:
+	case TEE_ALG_AES_CTR:
+	case TEE_ALG_AES_CTS:
+	case TEE_ALG_AES_XTS:
+	case TEE_ALG_AES_CCM:
+	case TEE_ALG_AES_GCM:
+		*ltc_cipherindex = find_cipher("aes");
+		break;
+#endif
+#if defined(CFG_CRYPTO_DES)
+	case TEE_ALG_DES_CBC_MAC_NOPAD:
+	case TEE_ALG_DES_CBC_MAC_PKCS5:
+	case TEE_ALG_DES_ECB_NOPAD:
+	case TEE_ALG_DES_CBC_NOPAD:
+		*ltc_cipherindex = find_cipher("des");
+		break;
+
+	case TEE_ALG_DES3_CBC_MAC_NOPAD:
+	case TEE_ALG_DES3_CBC_MAC_PKCS5:
+	case TEE_ALG_DES3_ECB_NOPAD:
+	case TEE_ALG_DES3_CBC_NOPAD:
+		*ltc_cipherindex = find_cipher("3des");
+		break;
+#endif
+	default:
+		return TEE_ERROR_BAD_PARAMETERS;
+	}
+
+	if (*ltc_cipherindex < 0)
+		return TEE_ERROR_NOT_SUPPORTED;
+	else
+		return TEE_SUCCESS;
+}
+#endif /* defined(_CFG_CRYPTO_WITH_CIPHER) ||
+	  defined(_CFG_CRYPTO_WITH_HASH) || defined(_CFG_CRYPTO_WITH_AUTHENC) */
+
+/******************************************************************************
+ * Message digest functions
+ ******************************************************************************/
+
+#if defined(_CFG_CRYPTO_WITH_HASH)
+
+static TEE_Result hash_get_ctx_size(uint32_t algo, size_t *size)
+{
+	switch (algo) {
+#if defined(CFG_CRYPTO_MD5)
+	case TEE_ALG_MD5:
+#endif
+#if defined(CFG_CRYPTO_SHA1)
+	case TEE_ALG_SHA1:
+#endif
+#if defined(CFG_CRYPTO_SHA224)
+	case TEE_ALG_SHA224:
+#endif
+#if defined(CFG_CRYPTO_SHA256)
+	case TEE_ALG_SHA256:
+#endif
+#if defined(CFG_CRYPTO_SHA384)
+	case TEE_ALG_SHA384:
+#endif
+#if defined(CFG_CRYPTO_SHA512)
+	case TEE_ALG_SHA512:
+#endif
+		*size = sizeof(hash_state);
+		break;
+	default:
+		return TEE_ERROR_NOT_SUPPORTED;
+	}
+
+	return TEE_SUCCESS;
+}
+
+static TEE_Result hash_init(void *ctx, uint32_t algo)
+{
+	int ltc_res;
+	int ltc_hashindex;
+
+	ltc_res = tee_algo_to_ltc_hashindex(algo, &ltc_hashindex);
+	if (ltc_res != TEE_SUCCESS)
+		return TEE_ERROR_NOT_SUPPORTED;
+
+	if (hash_descriptor[ltc_hashindex]->init(ctx) == CRYPT_OK)
+		return TEE_SUCCESS;
+	else
+		return TEE_ERROR_BAD_STATE;
+}
+
+static TEE_Result hash_update(void *ctx, uint32_t algo,
+				      const uint8_t *data, size_t len)
+{
+	int ltc_res;
+	int ltc_hashindex;
+
+	ltc_res = tee_algo_to_ltc_hashindex(algo, &ltc_hashindex);
+	if (ltc_res != TEE_SUCCESS)
+		return TEE_ERROR_NOT_SUPPORTED;
+
+	if (hash_descriptor[ltc_hashindex]->process(ctx, data, len) == CRYPT_OK)
+		return TEE_SUCCESS;
+	else
+		return TEE_ERROR_BAD_STATE;
+}
+
+static TEE_Result hash_final(void *ctx, uint32_t algo, uint8_t *digest,
+				     size_t len)
+{
+	int ltc_res;
+	int ltc_hashindex;
+	size_t hash_size;
+	uint8_t block_digest[TEE_MAX_HASH_SIZE];
+	uint8_t *tmp_digest;
+
+	ltc_res = tee_algo_to_ltc_hashindex(algo, &ltc_hashindex);
+	if (ltc_res != TEE_SUCCESS)
+		return TEE_ERROR_NOT_SUPPORTED;
+
+	if (len == 0)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	hash_size = hash_descriptor[ltc_hashindex]->hashsize;
+
+	if (hash_size > len) {
+		if (hash_size > sizeof(block_digest))
+			return TEE_ERROR_BAD_STATE;
+		tmp_digest = block_digest; /* use a tempory buffer */
+	} else {
+		tmp_digest = digest;
+	}
+	if (hash_descriptor[ltc_hashindex]->done(ctx, tmp_digest) == CRYPT_OK) {
+		if (hash_size > len)
+			memcpy(digest, tmp_digest, len);
+	} else {
+		return TEE_ERROR_BAD_STATE;
+	}
+
+	return TEE_SUCCESS;
+}
+
+#endif /* _CFG_CRYPTO_WITH_HASH */
+
+/******************************************************************************
+ * Asymmetric algorithms
+ ******************************************************************************/
+
+#if defined(_CFG_CRYPTO_WITH_ACIPHER)
+
+#define LTC_MAX_BITS_PER_VARIABLE   (4096)
+#define LTC_VARIABLE_NUMBER         (50)
+
+#define LTC_MEMPOOL_U32_SIZE \
+	mpa_scratch_mem_size_in_U32(LTC_VARIABLE_NUMBER, \
+				    LTC_MAX_BITS_PER_VARIABLE)
+
+#if defined(CFG_WITH_PAGER)
+#include <mm/tee_pager.h>
+#include <util.h>
+#include <mm/core_mmu.h>
+
+static uint32_t *_ltc_mempool_u32;
+
+/* allocate pageable_zi vmem for mpa scratch memory pool */
+static mpa_scratch_mem get_mpa_scratch_memory_pool(size_t *size_pool)
+{
+	void *pool;
+
+	*size_pool = ROUNDUP((LTC_MEMPOOL_U32_SIZE * sizeof(uint32_t)),
+			     SMALL_PAGE_SIZE);
+	_ltc_mempool_u32 = tee_pager_alloc(*size_pool, 0);
+	if (!_ltc_mempool_u32)
+		panic();
+	pool = (void *)_ltc_mempool_u32;
+	return (mpa_scratch_mem)pool;
+}
+
+/* release unused pageable_zi vmem */
+static void release_unused_mpa_scratch_memory(void)
+{
+	mpa_scratch_mem pool = (mpa_scratch_mem)_ltc_mempool_u32;
+	struct mpa_scratch_item *item;
+	vaddr_t start;
+	vaddr_t end;
+
+	/* we never free the header */
+	if (pool->last_offset) {
+		item = (struct mpa_scratch_item *)
+				((vaddr_t)pool + pool->last_offset);
+		start = (vaddr_t)item + item->size;
+	} else {
+		start = (vaddr_t)pool + sizeof(struct mpa_scratch_mem_struct);
+	}
+	end = (vaddr_t)pool + pool->size;
+	start = ROUNDUP(start, SMALL_PAGE_SIZE);
+	end = ROUNDDOWN(end, SMALL_PAGE_SIZE);
+
+	if (start < end)
+		tee_pager_release_phys((void *)start, end - start);
+}
+#else /* CFG_WITH_PAGER */
+
+static uint32_t _ltc_mempool_u32[LTC_MEMPOOL_U32_SIZE]
+	__aligned(__alignof__(mpa_scratch_mem_base));
+
+static mpa_scratch_mem get_mpa_scratch_memory_pool(size_t *size_pool)
+{
+	void *pool = (void *)_ltc_mempool_u32;
+
+	*size_pool = sizeof(_ltc_mempool_u32);
+	return (mpa_scratch_mem)pool;
+}
+
+static void release_unused_mpa_scratch_memory(void)
+{
+	/* nothing to do in non-pager mode */
+}
+
+#endif
+
+static void pool_postactions(void)
+{
+	mpa_scratch_mem pool = (void *)_ltc_mempool_u32;
+
+	if (pool->last_offset)
+		panic("release issue in mpa scratch memory");
+	release_unused_mpa_scratch_memory();
+}
+
+#if defined(CFG_LTC_OPTEE_THREAD)
+#include <kernel/thread.h>
+static struct mpa_scratch_mem_sync {
+	struct mutex mu;
+	struct condvar cv;
+	size_t count;
+	int owner;
+} pool_sync = {
+	.mu = MUTEX_INITIALIZER,
+	.cv = CONDVAR_INITIALIZER,
+	.owner = THREAD_ID_INVALID,
+};
+#elif defined(LTC_PTHREAD)
+#error NOT SUPPORTED
+#else
+static struct mpa_scratch_mem_sync {
+	size_t count;
+} pool_sync;
+#endif
+
+/* Get exclusive access to scratch memory pool */
+#if defined(CFG_LTC_OPTEE_THREAD)
+static void get_pool(struct mpa_scratch_mem_sync *sync)
+{
+	mutex_lock(&sync->mu);
+
+	if (sync->owner != thread_get_id()) {
+		/* Wait until the pool is available */
+		while (sync->owner != THREAD_ID_INVALID)
+			condvar_wait(&sync->cv, &sync->mu);
+
+		sync->owner = thread_get_id();
+		assert(sync->count == 0);
+	}
+
+	sync->count++;
+
+	mutex_unlock(&sync->mu);
+}
+
+/* Put (release) exclusive access to scratch memory pool */
+static void put_pool(struct mpa_scratch_mem_sync *sync)
+{
+	mutex_lock(&sync->mu);
+
+	assert(sync->owner == thread_get_id());
+	assert(sync->count > 0);
+
+	sync->count--;
+	if (!sync->count) {
+		sync->owner = THREAD_ID_INVALID;
+		condvar_signal(&sync->cv);
+		pool_postactions();
+	}
+
+	mutex_unlock(&sync->mu);
+}
+#elif defined(LTC_PTHREAD)
+#error NOT SUPPORTED
+#else
+static void get_pool(struct mpa_scratch_mem_sync *sync)
+{
+	sync->count++;
+}
+
+/* Put (release) exclusive access to scratch memory pool */
+static void put_pool(struct mpa_scratch_mem_sync *sync)
+{
+	sync->count--;
+	if (!sync->count)
+		pool_postactions();
+}
+#endif
+
+static void tee_ltc_alloc_mpa(void)
+{
+	mpa_scratch_mem pool;
+	size_t size_pool;
+
+	pool = get_mpa_scratch_memory_pool(&size_pool);
+	init_mpa_tomcrypt(pool);
+	mpa_init_scratch_mem_sync(pool, size_pool, LTC_MAX_BITS_PER_VARIABLE,
+				  get_pool, put_pool, &pool_sync);
+
+	mpa_set_random_generator(crypto_ops.prng.read);
+}
+
+static size_t num_bytes(struct bignum *a)
+{
+	return mp_unsigned_bin_size(a);
+}
+
+static size_t num_bits(struct bignum *a)
+{
+	return mp_count_bits(a);
+}
+
+static int32_t compare(struct bignum *a, struct bignum *b)
+{
+	return mp_cmp(a, b);
+}
+
+static void bn2bin(const struct bignum *from, uint8_t *to)
+{
+	mp_to_unsigned_bin((struct bignum *)from, to);
+}
+
+static TEE_Result bin2bn(const uint8_t *from, size_t fromsize,
+			 struct bignum *to)
+{
+	if (mp_read_unsigned_bin(to, (uint8_t *)from, fromsize) != CRYPT_OK)
+		return TEE_ERROR_BAD_PARAMETERS;
+	return TEE_SUCCESS;
+}
+
+static void copy(struct bignum *to, const struct bignum *from)
+{
+	mp_copy((void *)from, to);
+}
+
+static struct bignum *bn_allocate(size_t size_bits)
+{
+	size_t sz = mpa_StaticVarSizeInU32(size_bits) *	sizeof(uint32_t);
+	struct mpa_numbase_struct *bn = calloc(1, sz);
+
+	if (!bn)
+		return NULL;
+	bn->alloc = sz - MPA_NUMBASE_METADATA_SIZE_IN_U32 * sizeof(uint32_t);
+	return (struct bignum *)bn;
+}
+
+static void bn_free(struct bignum *s)
+{
+	free(s);
+}
+
+static void bn_clear(struct bignum *s)
+{
+	struct mpa_numbase_struct *bn = (struct mpa_numbase_struct *)s;
+
+	/* despite mpa_numbase_struct description, 'alloc' field a byte size */
+	memset(bn->d, 0, bn->alloc);
+}
+
+static bool bn_alloc_max(struct bignum **s)
+{
+	size_t sz = mpa_StaticVarSizeInU32(LTC_MAX_BITS_PER_VARIABLE) *
+			sizeof(uint32_t) * 8;
+
+	*s = bn_allocate(sz);
+	return !!(*s);
+}
+
+#if defined(CFG_CRYPTO_RSA)
+
+static TEE_Result alloc_rsa_keypair(struct rsa_keypair *s,
+				    size_t key_size_bits __unused)
+{
+	memset(s, 0, sizeof(*s));
+	if (!bn_alloc_max(&s->e)) {
+		return TEE_ERROR_OUT_OF_MEMORY;
+	}
+	if (!bn_alloc_max(&s->d))
+		goto err;
+	if (!bn_alloc_max(&s->n))
+		goto err;
+	if (!bn_alloc_max(&s->p))
+		goto err;
+	if (!bn_alloc_max(&s->q))
+		goto err;
+	if (!bn_alloc_max(&s->qp))
+		goto err;
+	if (!bn_alloc_max(&s->dp))
+		goto err;
+	if (!bn_alloc_max(&s->dq))
+		goto err;
+
+	return TEE_SUCCESS;
+err:
+	bn_free(s->e);
+	bn_free(s->d);
+	bn_free(s->n);
+	bn_free(s->p);
+	bn_free(s->q);
+	bn_free(s->qp);
+	bn_free(s->dp);
+
+	return TEE_ERROR_OUT_OF_MEMORY;
+}
+
+static TEE_Result alloc_rsa_public_key(struct rsa_public_key *s,
+				       size_t key_size_bits __unused)
+{
+	memset(s, 0, sizeof(*s));
+	if (!bn_alloc_max(&s->e)) {
+		return TEE_ERROR_OUT_OF_MEMORY;
+	}
+	if (!bn_alloc_max(&s->n))
+		goto err;
+	return TEE_SUCCESS;
+err:
+	bn_free(s->e);
+	return TEE_ERROR_OUT_OF_MEMORY;
+}
+
+static void free_rsa_public_key(struct rsa_public_key *s)
+{
+	if (!s)
+		return;
+	bn_free(s->n);
+	bn_free(s->e);
+}
+
+static TEE_Result gen_rsa_key(struct rsa_keypair *key, size_t key_size)
+{
+	TEE_Result res;
+	rsa_key ltc_tmp_key;
+	int ltc_res;
+	long e;
+	struct tee_ltc_prng *prng = tee_ltc_get_prng();
+
+	/* get the public exponent */
+	e = mp_get_int(key->e);
+
+	/* Generate a temporary RSA key */
+	ltc_res = rsa_make_key(&prng->state, prng->index, key_size/8, e,
+			       &ltc_tmp_key);
+	if (ltc_res != CRYPT_OK) {
+		res = TEE_ERROR_BAD_PARAMETERS;
+	} else if ((size_t)mp_count_bits(ltc_tmp_key.N) != key_size) {
+		rsa_free(&ltc_tmp_key);
+		res = TEE_ERROR_BAD_PARAMETERS;
+	} else {
+		/* Copy the key */
+		ltc_mp.copy(ltc_tmp_key.e,  key->e);
+		ltc_mp.copy(ltc_tmp_key.d,  key->d);
+		ltc_mp.copy(ltc_tmp_key.N,  key->n);
+		ltc_mp.copy(ltc_tmp_key.p,  key->p);
+		ltc_mp.copy(ltc_tmp_key.q,  key->q);
+		ltc_mp.copy(ltc_tmp_key.qP, key->qp);
+		ltc_mp.copy(ltc_tmp_key.dP, key->dp);
+		ltc_mp.copy(ltc_tmp_key.dQ, key->dq);
+
+		/* Free the temporary key */
+		rsa_free(&ltc_tmp_key);
+		res = TEE_SUCCESS;
+	}
+
+	return res;
+}
+
+
+static TEE_Result rsadorep(rsa_key *ltc_key, const uint8_t *src,
+			   size_t src_len, uint8_t *dst, size_t *dst_len)
+{
+	TEE_Result res = TEE_SUCCESS;
+	uint8_t *buf = NULL;
+	unsigned long blen, offset;
+	int ltc_res;
+
+	/*
+	 * Use a temporary buffer since we don't know exactly how large the
+	 * required size of the out buffer without doing a partial decrypt.
+	 * We know the upper bound though.
+	 */
+	blen = (mpa_StaticTempVarSizeInU32(LTC_MAX_BITS_PER_VARIABLE)) *
+	       sizeof(uint32_t);
+	buf = malloc(blen);
+	if (!buf) {
+		res = TEE_ERROR_OUT_OF_MEMORY;
+		goto out;
+	}
+
+	ltc_res = rsa_exptmod(src, src_len, buf, &blen, ltc_key->type,
+			      ltc_key);
+	switch (ltc_res) {
+	case CRYPT_PK_NOT_PRIVATE:
+	case CRYPT_PK_INVALID_TYPE:
+	case CRYPT_PK_INVALID_SIZE:
+	case CRYPT_INVALID_PACKET:
+		EMSG("rsa_exptmod() returned %d\n", ltc_res);
+		res = TEE_ERROR_BAD_PARAMETERS;
+		goto out;
+	case CRYPT_OK:
+		break;
+	default:
+		/* This will result in a panic */
+		EMSG("rsa_exptmod() returned %d\n", ltc_res);
+		res = TEE_ERROR_GENERIC;
+		goto out;
+	}
+
+	/* Remove the zero-padding (leave one zero if buff is all zeroes) */
+	offset = 0;
+	while ((offset < blen - 1) && (buf[offset] == 0))
+		offset++;
+
+	if (*dst_len < blen - offset) {
+		*dst_len = blen - offset;
+		res = TEE_ERROR_SHORT_BUFFER;
+		goto out;
+	}
+
+	res = TEE_SUCCESS;
+	*dst_len = blen - offset;
+	memcpy(dst, (char *)buf + offset, *dst_len);
+
+out:
+	if (buf)
+		free(buf);
+
+	return res;
+}
+
+static TEE_Result rsanopad_encrypt(struct rsa_public_key *key,
+				   const uint8_t *src, size_t src_len,
+				   uint8_t *dst, size_t *dst_len)
+{
+	TEE_Result res;
+	rsa_key ltc_key = { 0, };
+
+	ltc_key.type = PK_PUBLIC;
+	ltc_key.e = key->e;
+	ltc_key.N = key->n;
+
+	res = rsadorep(&ltc_key, src, src_len, dst, dst_len);
+	return res;
+}
+
+static TEE_Result rsanopad_decrypt(struct rsa_keypair *key,
+				   const uint8_t *src, size_t src_len,
+				   uint8_t *dst, size_t *dst_len)
+{
+	TEE_Result res;
+	rsa_key ltc_key = { 0, };
+
+	ltc_key.type = PK_PRIVATE;
+	ltc_key.e = key->e;
+	ltc_key.N = key->n;
+	ltc_key.d = key->d;
+	if (key->p && num_bytes(key->p)) {
+		ltc_key.p = key->p;
+		ltc_key.q = key->q;
+		ltc_key.qP = key->qp;
+		ltc_key.dP = key->dp;
+		ltc_key.dQ = key->dq;
+	}
+
+	res = rsadorep(&ltc_key, src, src_len, dst, dst_len);
+	return res;
+}
+
+static TEE_Result rsaes_decrypt(uint32_t algo, struct rsa_keypair *key,
+				    const uint8_t *label, size_t label_len,
+				    const uint8_t *src, size_t src_len,
+				    uint8_t *dst, size_t *dst_len)
+{
+	TEE_Result res = TEE_SUCCESS;
+	void *buf = NULL;
+	unsigned long blen;
+	int ltc_hashindex, ltc_res, ltc_stat, ltc_rsa_algo;
+	size_t mod_size;
+	rsa_key ltc_key = { 0, };
+
+	ltc_key.type = PK_PRIVATE;
+	ltc_key.e = key->e;
+	ltc_key.d = key->d;
+	ltc_key.N = key->n;
+	if (key->p && num_bytes(key->p)) {
+		ltc_key.p = key->p;
+		ltc_key.q = key->q;
+		ltc_key.qP = key->qp;
+		ltc_key.dP = key->dp;
+		ltc_key.dQ = key->dq;
+	}
+
+	/* Get the algorithm */
+	res = tee_algo_to_ltc_hashindex(algo, &ltc_hashindex);
+	if (res != TEE_SUCCESS) {
+		EMSG("tee_algo_to_ltc_hashindex() returned %d\n", (int)res);
+		goto out;
+	}
+
+	/*
+	 * Use a temporary buffer since we don't know exactly how large
+	 * the required size of the out buffer without doing a partial
+	 * decrypt. We know the upper bound though.
+	 */
+	if (algo == TEE_ALG_RSAES_PKCS1_V1_5) {
+		mod_size = ltc_mp.unsigned_size((void *)(ltc_key.N));
+		blen = mod_size - 11;
+		ltc_rsa_algo = LTC_PKCS_1_V1_5;
+	} else {
+		/* Decoded message is always shorter than encrypted message */
+		blen = src_len;
+		ltc_rsa_algo = LTC_PKCS_1_OAEP;
+	}
+
+	buf = malloc(blen);
+	if (!buf) {
+		res = TEE_ERROR_OUT_OF_MEMORY;
+		goto out;
+	}
+
+	ltc_res = rsa_decrypt_key_ex(src, src_len, buf, &blen,
+				     ((label_len == 0) ? 0 : label), label_len,
+				     ltc_hashindex, ltc_rsa_algo, &ltc_stat,
+				     &ltc_key);
+	switch (ltc_res) {
+	case CRYPT_PK_INVALID_PADDING:
+	case CRYPT_INVALID_PACKET:
+	case CRYPT_PK_INVALID_SIZE:
+		EMSG("rsa_decrypt_key_ex() returned %d\n", ltc_res);
+		res = TEE_ERROR_BAD_PARAMETERS;
+		goto out;
+	case CRYPT_OK:
+		break;
+	default:
+		/* This will result in a panic */
+		EMSG("rsa_decrypt_key_ex() returned %d\n", ltc_res);
+		res = TEE_ERROR_GENERIC;
+		goto out;
+	}
+	if (ltc_stat != 1) {
+		/* This will result in a panic */
+		EMSG("rsa_decrypt_key_ex() returned %d and %d\n",
+		     ltc_res, ltc_stat);
+		res = TEE_ERROR_GENERIC;
+		goto out;
+	}
+
+	if (*dst_len < blen) {
+		*dst_len = blen;
+		res = TEE_ERROR_SHORT_BUFFER;
+		goto out;
+	}
+
+	res = TEE_SUCCESS;
+	*dst_len = blen;
+	memcpy(dst, buf, blen);
+
+out:
+	if (buf)
+		free(buf);
+
+	return res;
+}
+
+static TEE_Result rsaes_encrypt(uint32_t algo, struct rsa_public_key *key,
+					const uint8_t *label, size_t label_len,
+					const uint8_t *src, size_t src_len,
+					uint8_t *dst, size_t *dst_len)
+{
+	TEE_Result res;
+	uint32_t mod_size;
+	int ltc_hashindex, ltc_res, ltc_rsa_algo;
+	rsa_key ltc_key = {
+		.type = PK_PUBLIC,
+		.e = key->e,
+		.N = key->n
+	};
+	struct tee_ltc_prng *prng = tee_ltc_get_prng();
+
+	mod_size =  ltc_mp.unsigned_size((void *)(ltc_key.N));
+	if (*dst_len < mod_size) {
+		*dst_len = mod_size;
+		res = TEE_ERROR_SHORT_BUFFER;
+		goto out;
+	}
+	*dst_len = mod_size;
+
+	/* Get the algorithm */
+	res = tee_algo_to_ltc_hashindex(algo, &ltc_hashindex);
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	if (algo == TEE_ALG_RSAES_PKCS1_V1_5)
+		ltc_rsa_algo = LTC_PKCS_1_V1_5;
+	else
+		ltc_rsa_algo = LTC_PKCS_1_OAEP;
+
+	ltc_res = rsa_encrypt_key_ex(src, src_len, dst,
+				     (unsigned long *)(dst_len), label,
+				     label_len, &prng->state, prng->index,
+				     ltc_hashindex, ltc_rsa_algo, &ltc_key);
+	switch (ltc_res) {
+	case CRYPT_PK_INVALID_PADDING:
+	case CRYPT_INVALID_PACKET:
+	case CRYPT_PK_INVALID_SIZE:
+		EMSG("rsa_encrypt_key_ex() returned %d\n", ltc_res);
+		res = TEE_ERROR_BAD_PARAMETERS;
+		goto out;
+	case CRYPT_OK:
+		break;
+	default:
+		/* This will result in a panic */
+		res = TEE_ERROR_GENERIC;
+		goto out;
+	}
+	res = TEE_SUCCESS;
+
+out:
+	return res;
+}
+
+static TEE_Result rsassa_sign(uint32_t algo, struct rsa_keypair *key,
+			      int salt_len, const uint8_t *msg,
+			      size_t msg_len, uint8_t *sig,
+			      size_t *sig_len)
+{
+	TEE_Result res;
+	size_t hash_size, mod_size;
+	int ltc_res, ltc_rsa_algo, ltc_hashindex;
+	unsigned long ltc_sig_len;
+	rsa_key ltc_key = { 0, };
+	struct tee_ltc_prng *prng = tee_ltc_get_prng();
+
+	ltc_key.type = PK_PRIVATE;
+	ltc_key.e = key->e;
+	ltc_key.N = key->n;
+	ltc_key.d = key->d;
+	if (key->p && num_bytes(key->p)) {
+		ltc_key.p = key->p;
+		ltc_key.q = key->q;
+		ltc_key.qP = key->qp;
+		ltc_key.dP = key->dp;
+		ltc_key.dQ = key->dq;
+	}
+
+	switch (algo) {
+	case TEE_ALG_RSASSA_PKCS1_V1_5_MD5:
+	case TEE_ALG_RSASSA_PKCS1_V1_5_SHA1:
+	case TEE_ALG_RSASSA_PKCS1_V1_5_SHA224:
+	case TEE_ALG_RSASSA_PKCS1_V1_5_SHA256:
+	case TEE_ALG_RSASSA_PKCS1_V1_5_SHA384:
+	case TEE_ALG_RSASSA_PKCS1_V1_5_SHA512:
+		ltc_rsa_algo = LTC_PKCS_1_V1_5;
+		break;
+	case TEE_ALG_RSASSA_PKCS1_PSS_MGF1_SHA1:
+	case TEE_ALG_RSASSA_PKCS1_PSS_MGF1_SHA224:
+	case TEE_ALG_RSASSA_PKCS1_PSS_MGF1_SHA256:
+	case TEE_ALG_RSASSA_PKCS1_PSS_MGF1_SHA384:
+	case TEE_ALG_RSASSA_PKCS1_PSS_MGF1_SHA512:
+		ltc_rsa_algo = LTC_PKCS_1_PSS;
+		break;
+	default:
+		res = TEE_ERROR_BAD_PARAMETERS;
+		goto err;
+	}
+
+	ltc_res = tee_algo_to_ltc_hashindex(algo, &ltc_hashindex);
+	if (ltc_res != CRYPT_OK) {
+		res = TEE_ERROR_BAD_PARAMETERS;
+		goto err;
+	}
+
+	res = tee_hash_get_digest_size(TEE_DIGEST_HASH_TO_ALGO(algo),
+				       &hash_size);
+	if (res != TEE_SUCCESS)
+		goto err;
+
+	if (msg_len != hash_size) {
+		res = TEE_ERROR_BAD_PARAMETERS;
+		goto err;
+	}
+
+	mod_size = ltc_mp.unsigned_size((void *)(ltc_key.N));
+
+	if (*sig_len < mod_size) {
+		*sig_len = mod_size;
+		res = TEE_ERROR_SHORT_BUFFER;
+		goto err;
+	}
+
+	ltc_sig_len = mod_size;
+
+	ltc_res = rsa_sign_hash_ex(msg, msg_len, sig, &ltc_sig_len,
+				   ltc_rsa_algo, &prng->state, prng->index,
+				   ltc_hashindex, salt_len, &ltc_key);
+
+	*sig_len = ltc_sig_len;
+
+	if (ltc_res != CRYPT_OK) {
+		res = TEE_ERROR_BAD_PARAMETERS;
+		goto err;
+	}
+	res = TEE_SUCCESS;
+
+err:
+	return res;
+}
+
+static TEE_Result rsassa_verify(uint32_t algo, struct rsa_public_key *key,
+				int salt_len, const uint8_t *msg,
+				size_t msg_len, const uint8_t *sig,
+				size_t sig_len)
+{
+	TEE_Result res;
+	uint32_t bigint_size;
+	size_t hash_size;
+	int stat, ltc_hashindex, ltc_res, ltc_rsa_algo;
+	rsa_key ltc_key = {
+		.type = PK_PUBLIC,
+		.e = key->e,
+		.N = key->n
+	};
+
+	res = tee_hash_get_digest_size(TEE_DIGEST_HASH_TO_ALGO(algo),
+				       &hash_size);
+	if (res != TEE_SUCCESS)
+		goto err;
+
+	if (msg_len != hash_size) {
+		res = TEE_ERROR_BAD_PARAMETERS;
+		goto err;
+	}
+
+	bigint_size = ltc_mp.unsigned_size(ltc_key.N);
+	if (sig_len < bigint_size) {
+		res = TEE_ERROR_SIGNATURE_INVALID;
+		goto err;
+	}
+
+	/* Get the algorithm */
+	res = tee_algo_to_ltc_hashindex(algo, &ltc_hashindex);
+	if (res != TEE_SUCCESS)
+		goto err;
+
+	switch (algo) {
+	case TEE_ALG_RSASSA_PKCS1_V1_5_MD5:
+	case TEE_ALG_RSASSA_PKCS1_V1_5_SHA1:
+	case TEE_ALG_RSASSA_PKCS1_V1_5_SHA224:
+	case TEE_ALG_RSASSA_PKCS1_V1_5_SHA256:
+	case TEE_ALG_RSASSA_PKCS1_V1_5_SHA384:
+	case TEE_ALG_RSASSA_PKCS1_V1_5_SHA512:
+		ltc_rsa_algo = LTC_PKCS_1_V1_5;
+		break;
+	case TEE_ALG_RSASSA_PKCS1_PSS_MGF1_SHA1:
+	case TEE_ALG_RSASSA_PKCS1_PSS_MGF1_SHA224:
+	case TEE_ALG_RSASSA_PKCS1_PSS_MGF1_SHA256:
+	case TEE_ALG_RSASSA_PKCS1_PSS_MGF1_SHA384:
+	case TEE_ALG_RSASSA_PKCS1_PSS_MGF1_SHA512:
+		ltc_rsa_algo = LTC_PKCS_1_PSS;
+		break;
+	default:
+		res = TEE_ERROR_BAD_PARAMETERS;
+		goto err;
+	}
+
+	ltc_res = rsa_verify_hash_ex(sig, sig_len, msg, msg_len, ltc_rsa_algo,
+				     ltc_hashindex, salt_len, &stat, &ltc_key);
+	if ((ltc_res != CRYPT_OK) || (stat != 1)) {
+		res = TEE_ERROR_SIGNATURE_INVALID;
+		goto err;
+	}
+	res = TEE_SUCCESS;
+
+err:
+	return res;
+}
+
+#endif /* CFG_CRYPTO_RSA */
+
+#if defined(CFG_CRYPTO_DSA)
+
+static TEE_Result alloc_dsa_keypair(struct dsa_keypair *s,
+				    size_t key_size_bits __unused)
+{
+	memset(s, 0, sizeof(*s));
+	if (!bn_alloc_max(&s->g)) {
+		return TEE_ERROR_OUT_OF_MEMORY;
+	}
+
+	if (!bn_alloc_max(&s->p))
+		goto err;
+	if (!bn_alloc_max(&s->q))
+		goto err;
+	if (!bn_alloc_max(&s->y))
+		goto err;
+	if (!bn_alloc_max(&s->x))
+		goto err;
+	return TEE_SUCCESS;
+err:
+	bn_free(s->g);
+	bn_free(s->p);
+	bn_free(s->q);
+	bn_free(s->y);
+	return TEE_ERROR_OUT_OF_MEMORY;
+}
+
+static TEE_Result alloc_dsa_public_key(struct dsa_public_key *s,
+				       size_t key_size_bits __unused)
+{
+	memset(s, 0, sizeof(*s));
+	if (!bn_alloc_max(&s->g)) {
+		return TEE_ERROR_OUT_OF_MEMORY;
+	}
+
+	if (!bn_alloc_max(&s->p))
+		goto err;
+	if (!bn_alloc_max(&s->q))
+		goto err;
+	if (!bn_alloc_max(&s->y))
+		goto err;
+	return TEE_SUCCESS;
+err:
+	bn_free(s->g);
+	bn_free(s->p);
+	bn_free(s->q);
+	return TEE_ERROR_OUT_OF_MEMORY;
+}
+
+static TEE_Result gen_dsa_key(struct dsa_keypair *key, size_t key_size)
+{
+	TEE_Result res;
+	dsa_key ltc_tmp_key;
+	size_t group_size, modulus_size = key_size/8;
+	int ltc_res;
+	struct tee_ltc_prng *prng = tee_ltc_get_prng();
+
+	if (modulus_size <= 128)
+		group_size = 20;
+	else if (modulus_size <= 256)
+		group_size = 30;
+	else if (modulus_size <= 384)
+		group_size = 35;
+	else
+		group_size = 40;
+
+	/* Generate the DSA key */
+	ltc_res = dsa_make_key(&prng->state, prng->index, group_size,
+			       modulus_size, &ltc_tmp_key);
+	if (ltc_res != CRYPT_OK) {
+		res = TEE_ERROR_BAD_PARAMETERS;
+	} else if ((size_t)mp_count_bits(ltc_tmp_key.p) != key_size) {
+		dsa_free(&ltc_tmp_key);
+		res = TEE_ERROR_BAD_PARAMETERS;
+	} else {
+		/* Copy the key */
+		ltc_mp.copy(ltc_tmp_key.g, key->g);
+		ltc_mp.copy(ltc_tmp_key.p, key->p);
+		ltc_mp.copy(ltc_tmp_key.q, key->q);
+		ltc_mp.copy(ltc_tmp_key.y, key->y);
+		ltc_mp.copy(ltc_tmp_key.x, key->x);
+
+		/* Free the tempory key */
+		dsa_free(&ltc_tmp_key);
+		res = TEE_SUCCESS;
+	}
+	return res;
+}
+
+static TEE_Result dsa_sign(uint32_t algo, struct dsa_keypair *key,
+			   const uint8_t *msg, size_t msg_len, uint8_t *sig,
+			   size_t *sig_len)
+{
+	TEE_Result res;
+	size_t hash_size;
+	int ltc_res;
+	void *r, *s;
+	dsa_key ltc_key = {
+		.type = PK_PRIVATE,
+		.qord = mp_unsigned_bin_size(key->g),
+		.g = key->g,
+		.p = key->p,
+		.q = key->q,
+		.y = key->y,
+		.x = key->x,
+	};
+	struct tee_ltc_prng *prng = tee_ltc_get_prng();
+
+	if (algo != TEE_ALG_DSA_SHA1 &&
+	    algo != TEE_ALG_DSA_SHA224 &&
+	    algo != TEE_ALG_DSA_SHA256) {
+		res = TEE_ERROR_NOT_IMPLEMENTED;
+		goto err;
+	}
+
+	res = tee_hash_get_digest_size(TEE_DIGEST_HASH_TO_ALGO(algo),
+				       &hash_size);
+	if (res != TEE_SUCCESS)
+		goto err;
+	if (mp_unsigned_bin_size(ltc_key.q) < hash_size)
+		hash_size = mp_unsigned_bin_size(ltc_key.q);
+	if (msg_len != hash_size) {
+		res = TEE_ERROR_SECURITY;
+		goto err;
+	}
+
+	if (*sig_len < 2 * mp_unsigned_bin_size(ltc_key.q)) {
+		*sig_len = 2 * mp_unsigned_bin_size(ltc_key.q);
+		res = TEE_ERROR_SHORT_BUFFER;
+		goto err;
+	}
+
+	ltc_res = mp_init_multi(&r, &s, NULL);
+	if (ltc_res != CRYPT_OK) {
+		res = TEE_ERROR_OUT_OF_MEMORY;
+		goto err;
+	}
+
+	ltc_res = dsa_sign_hash_raw(msg, msg_len, r, s, &prng->state,
+				    prng->index, &ltc_key);
+
+	if (ltc_res == CRYPT_OK) {
+		*sig_len = 2 * mp_unsigned_bin_size(ltc_key.q);
+		memset(sig, 0, *sig_len);
+		mp_to_unsigned_bin(r, (uint8_t *)sig + *sig_len/2 -
+				   mp_unsigned_bin_size(r));
+		mp_to_unsigned_bin(s, (uint8_t *)sig + *sig_len -
+				   mp_unsigned_bin_size(s));
+		res = TEE_SUCCESS;
+	} else {
+		res = TEE_ERROR_GENERIC;
+	}
+
+	mp_clear_multi(r, s, NULL);
+
+err:
+	return res;
+}
+
+static TEE_Result dsa_verify(uint32_t algo, struct dsa_public_key *key,
+			     const uint8_t *msg, size_t msg_len,
+			     const uint8_t *sig, size_t sig_len)
+{
+	TEE_Result res;
+	int ltc_stat, ltc_res;
+	void *r, *s;
+	dsa_key ltc_key = {
+		.type = PK_PUBLIC,
+		.qord = mp_unsigned_bin_size(key->g),
+		.g = key->g,
+		.p = key->p,
+		.q = key->q,
+		.y = key->y
+	};
+
+	if (algo != TEE_ALG_DSA_SHA1 &&
+	    algo != TEE_ALG_DSA_SHA224 &&
+	    algo != TEE_ALG_DSA_SHA256) {
+		res = TEE_ERROR_NOT_IMPLEMENTED;
+		goto err;
+	}
+
+	ltc_res = mp_init_multi(&r, &s, NULL);
+	if (ltc_res != CRYPT_OK) {
+		res = TEE_ERROR_OUT_OF_MEMORY;
+		goto err;
+	}
+	mp_read_unsigned_bin(r, (uint8_t *)sig, sig_len/2);
+	mp_read_unsigned_bin(s, (uint8_t *)sig + sig_len/2, sig_len/2);
+	ltc_res = dsa_verify_hash_raw(r, s, msg, msg_len, &ltc_stat, &ltc_key);
+	mp_clear_multi(r, s, NULL);
+
+	if ((ltc_res == CRYPT_OK) && (ltc_stat == 1))
+		res = TEE_SUCCESS;
+	else
+		res = TEE_ERROR_GENERIC;
+
+err:
+	return res;
+}
+
+#endif /* CFG_CRYPTO_DSA */
+
+#if defined(CFG_CRYPTO_DH)
+
+static TEE_Result alloc_dh_keypair(struct dh_keypair *s,
+				   size_t key_size_bits __unused)
+{
+	memset(s, 0, sizeof(*s));
+	if (!bn_alloc_max(&s->g)) {
+		return TEE_ERROR_OUT_OF_MEMORY;
+	}
+
+	if (!bn_alloc_max(&s->p))
+		goto err;
+	if (!bn_alloc_max(&s->y))
+		goto err;
+	if (!bn_alloc_max(&s->x))
+		goto err;
+	if (!bn_alloc_max(&s->q))
+		goto err;
+	return TEE_SUCCESS;
+err:
+	bn_free(s->g);
+	bn_free(s->p);
+	bn_free(s->y);
+	bn_free(s->x);
+	return TEE_ERROR_OUT_OF_MEMORY;
+}
+
+static TEE_Result gen_dh_key(struct dh_keypair *key, struct bignum *q,
+			     size_t xbits)
+{
+	TEE_Result res;
+	dh_key ltc_tmp_key;
+	int ltc_res;
+	struct tee_ltc_prng *prng = tee_ltc_get_prng();
+
+	/* Generate the DH key */
+	ltc_tmp_key.g = key->g;
+	ltc_tmp_key.p = key->p;
+	ltc_res = dh_make_key(&prng->state, prng->index, q, xbits,
+			      &ltc_tmp_key);
+	if (ltc_res != CRYPT_OK) {
+		res = TEE_ERROR_BAD_PARAMETERS;
+	} else {
+		ltc_mp.copy(ltc_tmp_key.y,  key->y);
+		ltc_mp.copy(ltc_tmp_key.x,  key->x);
+
+		/* Free the tempory key */
+		dh_free(&ltc_tmp_key);
+		res = TEE_SUCCESS;
+	}
+	return res;
+}
+
+static TEE_Result do_dh_shared_secret(struct dh_keypair *private_key,
+				      struct bignum *public_key,
+				      struct bignum *secret)
+{
+	int err;
+	dh_key pk = {
+		.type = PK_PRIVATE,
+		.g = private_key->g,
+		.p = private_key->p,
+		.y = private_key->y,
+		.x = private_key->x
+	};
+
+	err = dh_shared_secret(&pk, public_key, secret);
+	return ((err == CRYPT_OK) ? TEE_SUCCESS : TEE_ERROR_BAD_PARAMETERS);
+}
+
+#endif /* CFG_CRYPTO_DH */
+
+#if defined(CFG_CRYPTO_ECC)
+
+static TEE_Result alloc_ecc_keypair(struct ecc_keypair *s,
+				   size_t key_size_bits __unused)
+{
+	memset(s, 0, sizeof(*s));
+	if (!bn_alloc_max(&s->d))
+		goto err;
+	if (!bn_alloc_max(&s->x))
+		goto err;
+	if (!bn_alloc_max(&s->y))
+		goto err;
+	return TEE_SUCCESS;
+err:
+	bn_free(s->d);
+	bn_free(s->x);
+	bn_free(s->y);
+	return TEE_ERROR_OUT_OF_MEMORY;
+}
+
+static TEE_Result alloc_ecc_public_key(struct ecc_public_key *s,
+				   size_t key_size_bits __unused)
+{
+	memset(s, 0, sizeof(*s));
+	if (!bn_alloc_max(&s->x))
+		goto err;
+	if (!bn_alloc_max(&s->y))
+		goto err;
+	return TEE_SUCCESS;
+err:
+	bn_free(s->x);
+	bn_free(s->y);
+	return TEE_ERROR_OUT_OF_MEMORY;
+}
+
+static void free_ecc_public_key(struct ecc_public_key *s)
+{
+	if (!s)
+		return;
+
+	bn_free(s->x);
+	bn_free(s->y);
+}
+
+/*
+ * curve is part of TEE_ECC_CURVE_NIST_P192,...
+ * algo is part of TEE_ALG_ECDSA_P192,..., and 0 if we do not have it
+ */
+static TEE_Result ecc_get_keysize(uint32_t curve, uint32_t algo,
+				  size_t *key_size_bytes, size_t *key_size_bits)
+{
+	/*
+	 * Excerpt of libtomcrypt documentation:
+	 * ecc_make_key(... key_size ...): The keysize is the size of the
+	 * modulus in bytes desired. Currently directly supported values
+	 * are 12, 16, 20, 24, 28, 32, 48, and 65 bytes which correspond
+	 * to key sizes of 112, 128, 160, 192, 224, 256, 384, and 521 bits
+	 * respectively.
+	 */
+
+	/*
+	 * Note GPv1.1 indicates TEE_ALG_ECDH_NIST_P192_DERIVE_SHARED_SECRET
+	 * but defines TEE_ALG_ECDH_P192
+	 */
+
+	switch (curve) {
+	case TEE_ECC_CURVE_NIST_P192:
+		*key_size_bits = 192;
+		*key_size_bytes = 24;
+		if ((algo != 0) && (algo != TEE_ALG_ECDSA_P192) &&
+		    (algo != TEE_ALG_ECDH_P192))
+			return TEE_ERROR_BAD_PARAMETERS;
+		break;
+	case TEE_ECC_CURVE_NIST_P224:
+		*key_size_bits = 224;
+		*key_size_bytes = 28;
+		if ((algo != 0) && (algo != TEE_ALG_ECDSA_P224) &&
+		    (algo != TEE_ALG_ECDH_P224))
+			return TEE_ERROR_BAD_PARAMETERS;
+		break;
+	case TEE_ECC_CURVE_NIST_P256:
+		*key_size_bits = 256;
+		*key_size_bytes = 32;
+		if ((algo != 0) && (algo != TEE_ALG_ECDSA_P256) &&
+		    (algo != TEE_ALG_ECDH_P256))
+			return TEE_ERROR_BAD_PARAMETERS;
+		break;
+	case TEE_ECC_CURVE_NIST_P384:
+		*key_size_bits = 384;
+		*key_size_bytes = 48;
+		if ((algo != 0) && (algo != TEE_ALG_ECDSA_P384) &&
+		    (algo != TEE_ALG_ECDH_P384))
+			return TEE_ERROR_BAD_PARAMETERS;
+		break;
+	case TEE_ECC_CURVE_NIST_P521:
+		*key_size_bits = 521;
+		/*
+		 * set 66 instead of 65 wrt to Libtomcrypt documentation as
+		 * if it the real key size
+		 */
+		*key_size_bytes = 66;
+		if ((algo != 0) && (algo != TEE_ALG_ECDSA_P521) &&
+		    (algo != TEE_ALG_ECDH_P521))
+			return TEE_ERROR_BAD_PARAMETERS;
+		break;
+	default:
+		*key_size_bits = 0;
+		*key_size_bytes = 0;
+		return TEE_ERROR_NOT_SUPPORTED;
+	}
+
+	return TEE_SUCCESS;
+}
+
+static TEE_Result gen_ecc_key(struct ecc_keypair *key)
+{
+	TEE_Result res;
+	ecc_key ltc_tmp_key;
+	int ltc_res;
+	struct tee_ltc_prng *prng = tee_ltc_get_prng();
+	size_t key_size_bytes = 0;
+	size_t key_size_bits = 0;
+
+	res = ecc_get_keysize(key->curve, 0, &key_size_bytes, &key_size_bits);
+	if (res != TEE_SUCCESS) {
+		return res;
+	}
+
+	/* Generate the ECC key */
+	ltc_res = ecc_make_key(&prng->state, prng->index,
+			       key_size_bytes, &ltc_tmp_key);
+	if (ltc_res != CRYPT_OK) {
+		return TEE_ERROR_BAD_PARAMETERS;
+	}
+
+	/* check the size of the keys */
+	if (((size_t)mp_count_bits(ltc_tmp_key.pubkey.x) > key_size_bits) ||
+	    ((size_t)mp_count_bits(ltc_tmp_key.pubkey.y) > key_size_bits) ||
+	    ((size_t)mp_count_bits(ltc_tmp_key.k) > key_size_bits)) {
+		res = TEE_ERROR_BAD_PARAMETERS;
+		goto exit;
+	}
+
+	/* check LTC is returning z==1 */
+	if (mp_count_bits(ltc_tmp_key.pubkey.z) != 1) {
+		res = TEE_ERROR_BAD_PARAMETERS;
+		goto exit;
+	}
+
+	/* Copy the key */
+	ltc_mp.copy(ltc_tmp_key.k, key->d);
+	ltc_mp.copy(ltc_tmp_key.pubkey.x, key->x);
+	ltc_mp.copy(ltc_tmp_key.pubkey.y, key->y);
+
+	res = TEE_SUCCESS;
+
+exit:
+	ecc_free(&ltc_tmp_key);		/* Free the temporary key */
+	return res;
+}
+
+static TEE_Result ecc_compute_key_idx(ecc_key *ltc_key, size_t keysize)
+{
+	size_t x;
+
+	for (x = 0; ((int)keysize > ltc_ecc_sets[x].size) &&
+		    (ltc_ecc_sets[x].size != 0);
+	     x++)
+		;
+	keysize = (size_t)ltc_ecc_sets[x].size;
+
+	if ((keysize > ECC_MAXSIZE) || (ltc_ecc_sets[x].size == 0))
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	ltc_key->idx = -1;
+	ltc_key->dp  = &ltc_ecc_sets[x];
+
+	return TEE_SUCCESS;
+}
+
+/*
+ * Given a keypair "key", populate the Libtomcrypt private key "ltc_key"
+ * It also returns the key size, in bytes
+ */
+static TEE_Result ecc_populate_ltc_private_key(ecc_key *ltc_key,
+					       struct ecc_keypair *key,
+					       uint32_t algo,
+					       size_t *key_size_bytes)
+{
+	TEE_Result res;
+	size_t key_size_bits;
+
+	memset(ltc_key, 0, sizeof(*ltc_key));
+	ltc_key->type = PK_PRIVATE;
+	ltc_key->k = key->d;
+
+	/* compute the index of the ecc curve */
+	res = ecc_get_keysize(key->curve, algo,
+			      key_size_bytes, &key_size_bits);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	return ecc_compute_key_idx(ltc_key, *key_size_bytes);
+}
+
+/*
+ * Given a public "key", populate the Libtomcrypt public key "ltc_key"
+ * It also returns the key size, in bytes
+ */
+static TEE_Result ecc_populate_ltc_public_key(ecc_key *ltc_key,
+					      struct ecc_public_key *key,
+					      void *key_z,
+					      uint32_t algo,
+					      size_t *key_size_bytes)
+{
+	TEE_Result res;
+	size_t key_size_bits;
+	uint8_t one[1] = { 1 };
+
+
+	memset(ltc_key, 0, sizeof(*ltc_key));
+	ltc_key->type = PK_PUBLIC;
+	ltc_key->pubkey.x = key->x;
+	ltc_key->pubkey.y = key->y;
+	ltc_key->pubkey.z = key_z;
+	mp_read_unsigned_bin(ltc_key->pubkey.z, one, sizeof(one));
+
+	/* compute the index of the ecc curve */
+	res = ecc_get_keysize(key->curve, algo,
+			      key_size_bytes, &key_size_bits);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	return ecc_compute_key_idx(ltc_key, *key_size_bytes);
+}
+
+static TEE_Result ecc_sign(uint32_t algo, struct ecc_keypair *key,
+			   const uint8_t *msg, size_t msg_len, uint8_t *sig,
+			   size_t *sig_len)
+{
+	TEE_Result res;
+	int ltc_res;
+	void *r, *s;
+	size_t key_size_bytes;
+	ecc_key ltc_key;
+	struct tee_ltc_prng *prng = tee_ltc_get_prng();
+
+	if (algo == 0) {
+		res = TEE_ERROR_BAD_PARAMETERS;
+		goto err;
+	}
+
+	res = ecc_populate_ltc_private_key(&ltc_key, key, algo,
+					   &key_size_bytes);
+	if (res != TEE_SUCCESS)
+		goto err;
+
+	if (*sig_len < 2 * key_size_bytes) {
+		*sig_len = 2 * key_size_bytes;
+		res = TEE_ERROR_SHORT_BUFFER;
+		goto err;
+	}
+
+	ltc_res = mp_init_multi(&r, &s, NULL);
+	if (ltc_res != CRYPT_OK) {
+		res = TEE_ERROR_OUT_OF_MEMORY;
+		goto err;
+	}
+
+	ltc_res = ecc_sign_hash_raw(msg, msg_len, r, s,
+				    &prng->state, prng->index, &ltc_key);
+
+	if (ltc_res == CRYPT_OK) {
+		*sig_len = 2 * key_size_bytes;
+		memset(sig, 0, *sig_len);
+		mp_to_unsigned_bin(r, (uint8_t *)sig + *sig_len/2 -
+				   mp_unsigned_bin_size(r));
+		mp_to_unsigned_bin(s, (uint8_t *)sig + *sig_len -
+				   mp_unsigned_bin_size(s));
+		res = TEE_SUCCESS;
+	} else {
+		res = TEE_ERROR_GENERIC;
+	}
+
+	mp_clear_multi(r, s, NULL);
+
+err:
+	return res;
+}
+
+static TEE_Result ecc_verify(uint32_t algo, struct ecc_public_key *key,
+			     const uint8_t *msg, size_t msg_len,
+			     const uint8_t *sig, size_t sig_len)
+{
+	TEE_Result res;
+	int ltc_stat;
+	int ltc_res;
+	void *r;
+	void *s;
+	void *key_z;
+	size_t key_size_bytes;
+	ecc_key ltc_key;
+
+	if (algo == 0) {
+		return TEE_ERROR_BAD_PARAMETERS;
+	}
+
+	ltc_res = mp_init_multi(&key_z, &r, &s, NULL);
+	if (ltc_res != CRYPT_OK) {
+		return TEE_ERROR_OUT_OF_MEMORY;
+	}
+
+	res = ecc_populate_ltc_public_key(&ltc_key, key, key_z, algo,
+					  &key_size_bytes);
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	/* check keysize vs sig_len */
+	if ((key_size_bytes * 2) != sig_len) {
+		res = TEE_ERROR_BAD_PARAMETERS;
+		goto out;
+	}
+
+	mp_read_unsigned_bin(r, (uint8_t *)sig, sig_len/2);
+	mp_read_unsigned_bin(s, (uint8_t *)sig + sig_len/2, sig_len/2);
+
+	ltc_res = ecc_verify_hash_raw(r, s, msg, msg_len, &ltc_stat, &ltc_key);
+	if ((ltc_res == CRYPT_OK) && (ltc_stat == 1))
+		res = TEE_SUCCESS;
+	else
+		res = TEE_ERROR_GENERIC;
+
+out:
+	mp_clear_multi(key_z, r, s, NULL);
+	return res;
+}
+
+static TEE_Result do_ecc_shared_secret(struct ecc_keypair *private_key,
+				       struct ecc_public_key *public_key,
+				       void *secret, unsigned long *secret_len)
+{
+	TEE_Result res;
+	int ltc_res;
+	ecc_key ltc_private_key;
+	ecc_key ltc_public_key;
+	size_t key_size_bytes;
+	void *key_z;
+
+	/* Check the curves are the same */
+	if (private_key->curve != public_key->curve) {
+		return TEE_ERROR_BAD_PARAMETERS;
+	}
+
+	ltc_res = mp_init_multi(&key_z, NULL);
+	if (ltc_res != CRYPT_OK) {
+		return TEE_ERROR_OUT_OF_MEMORY;
+	}
+
+	res = ecc_populate_ltc_private_key(&ltc_private_key, private_key,
+					   0, &key_size_bytes);
+	if (res != TEE_SUCCESS)
+		goto out;
+	res = ecc_populate_ltc_public_key(&ltc_public_key, public_key, key_z,
+					  0, &key_size_bytes);
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	ltc_res = ecc_shared_secret(&ltc_private_key, &ltc_public_key,
+				    secret, secret_len);
+	if (ltc_res == CRYPT_OK)
+		res = TEE_SUCCESS;
+	else
+		res = TEE_ERROR_BAD_PARAMETERS;
+
+out:
+	mp_clear_multi(key_z, NULL);
+	return res;
+}
+#endif /* CFG_CRYPTO_ECC */
+
+#endif /* _CFG_CRYPTO_WITH_ACIPHER */
+
+/******************************************************************************
+ * Symmetric ciphers
+ ******************************************************************************/
+
+#if defined(_CFG_CRYPTO_WITH_CIPHER)
+/* From libtomcrypt doc:
+ *	Ciphertext stealing is a method of dealing with messages
+ *	in CBC mode which are not a multiple of the block
+ *	length.  This is accomplished by encrypting the last
+ *	ciphertext block in ECB mode, and XOR'ing the output
+ *	against the last partial block of plaintext. LibTomCrypt
+ *	does not support this mode directly but it is fairly
+ *	easy to emulate with a call to the cipher's
+ *	ecb encrypt() callback function.
+ *	The more sane way to deal with partial blocks is to pad
+ *	them with zeroes, and then use CBC normally
+ */
+
+/*
+ * From Global Platform: CTS = CBC-CS3
+ */
+
+#if defined(CFG_CRYPTO_CTS)
+struct tee_symmetric_cts {
+	symmetric_ECB ecb;
+	symmetric_CBC cbc;
+};
+#endif
+
+#if defined(CFG_CRYPTO_XTS)
+#define XTS_TWEAK_SIZE 16
+struct tee_symmetric_xts {
+	symmetric_xts ctx;
+	uint8_t tweak[XTS_TWEAK_SIZE];
+};
+#endif
+
+static TEE_Result cipher_get_block_size(uint32_t algo, size_t *size)
+{
+	TEE_Result res;
+	int ltc_cipherindex;
+
+	res = tee_algo_to_ltc_cipherindex(algo, &ltc_cipherindex);
+	if (res != TEE_SUCCESS)
+		return TEE_ERROR_NOT_SUPPORTED;
+
+	*size = cipher_descriptor[ltc_cipherindex]->block_length;
+	return TEE_SUCCESS;
+}
+
+static TEE_Result cipher_get_ctx_size(uint32_t algo, size_t *size)
+{
+	switch (algo) {
+#if defined(CFG_CRYPTO_AES)
+#if defined(CFG_CRYPTO_ECB)
+	case TEE_ALG_AES_ECB_NOPAD:
+		*size = sizeof(symmetric_ECB);
+		break;
+#endif
+#if defined(CFG_CRYPTO_CBC)
+	case TEE_ALG_AES_CBC_NOPAD:
+		*size = sizeof(symmetric_CBC);
+		break;
+#endif
+#if defined(CFG_CRYPTO_CTR)
+	case TEE_ALG_AES_CTR:
+		*size = sizeof(symmetric_CTR);
+		break;
+#endif
+#if defined(CFG_CRYPTO_CTS)
+	case TEE_ALG_AES_CTS:
+		*size = sizeof(struct tee_symmetric_cts);
+		break;
+#endif
+#if defined(CFG_CRYPTO_XTS)
+	case TEE_ALG_AES_XTS:
+		*size = sizeof(struct tee_symmetric_xts);
+		break;
+#endif
+#endif
+#if defined(CFG_CRYPTO_DES)
+#if defined(CFG_CRYPTO_ECB)
+	case TEE_ALG_DES_ECB_NOPAD:
+		*size = sizeof(symmetric_ECB);
+		break;
+	case TEE_ALG_DES3_ECB_NOPAD:
+		*size = sizeof(symmetric_ECB);
+		break;
+#endif
+#if defined(CFG_CRYPTO_CBC)
+	case TEE_ALG_DES_CBC_NOPAD:
+		*size = sizeof(symmetric_CBC);
+		break;
+	case TEE_ALG_DES3_CBC_NOPAD:
+		*size = sizeof(symmetric_CBC);
+		break;
+#endif
+#endif
+	default:
+		return TEE_ERROR_NOT_SUPPORTED;
+	}
+
+	return TEE_SUCCESS;
+}
+
+static void get_des2_key(const uint8_t *key, size_t key_len,
+			 uint8_t *key_intermediate,
+			 uint8_t **real_key, size_t *real_key_len)
+{
+	if (key_len == 16) {
+		/*
+		 * This corresponds to a 2DES key. The 2DES encryption
+		 * algorithm is similar to 3DES. Both perform and
+		 * encryption step, then a decryption step, followed
+		 * by another encryption step (EDE). However 2DES uses
+		 * the same key for both of the encryption (E) steps.
+		 */
+		memcpy(key_intermediate, key, 16);
+		memcpy(key_intermediate+16, key, 8);
+		*real_key = key_intermediate;
+		*real_key_len = 24;
+	} else {
+		*real_key = (uint8_t *)key;
+		*real_key_len = key_len;
+	}
+}
+
+static TEE_Result cipher_init(void *ctx, uint32_t algo,
+			      TEE_OperationMode mode __maybe_unused,
+			      const uint8_t *key1, size_t key1_len,
+			      const uint8_t *key2 __maybe_unused,
+			      size_t key2_len __maybe_unused,
+			      const uint8_t *iv __maybe_unused,
+			      size_t iv_len __maybe_unused)
+{
+	TEE_Result res;
+	int ltc_res, ltc_cipherindex;
+	uint8_t *real_key, key_array[24];
+	size_t real_key_len;
+#if defined(CFG_CRYPTO_CTS)
+	struct tee_symmetric_cts *cts;
+#endif
+#if defined(CFG_CRYPTO_XTS)
+	struct tee_symmetric_xts *xts;
+#endif
+
+	res = tee_algo_to_ltc_cipherindex(algo, &ltc_cipherindex);
+	if (res != TEE_SUCCESS)
+		return TEE_ERROR_NOT_SUPPORTED;
+
+	switch (algo) {
+#if defined(CFG_CRYPTO_ECB)
+	case TEE_ALG_AES_ECB_NOPAD:
+	case TEE_ALG_DES_ECB_NOPAD:
+		ltc_res = ecb_start(
+			ltc_cipherindex, key1, key1_len,
+			0, (symmetric_ECB *)ctx);
+		break;
+
+	case TEE_ALG_DES3_ECB_NOPAD:
+		/* either des3 or des2, depending on the size of the key */
+		get_des2_key(key1, key1_len, key_array,
+			     &real_key, &real_key_len);
+		ltc_res = ecb_start(
+			ltc_cipherindex, real_key, real_key_len,
+			0, (symmetric_ECB *)ctx);
+		break;
+#endif
+#if defined(CFG_CRYPTO_CBC)
+	case TEE_ALG_AES_CBC_NOPAD:
+	case TEE_ALG_DES_CBC_NOPAD:
+		if (iv_len !=
+		    (size_t)cipher_descriptor[ltc_cipherindex]->block_length)
+			return TEE_ERROR_BAD_PARAMETERS;
+		ltc_res = cbc_start(
+			ltc_cipherindex, iv, key1, key1_len,
+			0, (symmetric_CBC *)ctx);
+		break;
+
+	case TEE_ALG_DES3_CBC_NOPAD:
+		/* either des3 or des2, depending on the size of the key */
+		get_des2_key(key1, key1_len, key_array,
+			     &real_key, &real_key_len);
+		if (iv_len !=
+		    (size_t)cipher_descriptor[ltc_cipherindex]->block_length)
+			return TEE_ERROR_BAD_PARAMETERS;
+		ltc_res = cbc_start(
+			ltc_cipherindex, iv, real_key, real_key_len,
+			0, (symmetric_CBC *)ctx);
+		break;
+#endif
+#if defined(CFG_CRYPTO_CTR)
+	case TEE_ALG_AES_CTR:
+		if (iv_len !=
+		    (size_t)cipher_descriptor[ltc_cipherindex]->block_length)
+			return TEE_ERROR_BAD_PARAMETERS;
+		ltc_res = ctr_start(
+			ltc_cipherindex, iv, key1, key1_len,
+			0, CTR_COUNTER_BIG_ENDIAN, (symmetric_CTR *)ctx);
+		break;
+#endif
+#if defined(CFG_CRYPTO_CTS)
+	case TEE_ALG_AES_CTS:
+		cts = ctx;
+		res = cipher_init((void *)(&(cts->ecb)),
+					  TEE_ALG_AES_ECB_NOPAD, mode, key1,
+					  key1_len, key2, key2_len, iv,
+					  iv_len);
+		if (res != TEE_SUCCESS)
+			return res;
+		res = cipher_init((void *)(&(cts->cbc)),
+					  TEE_ALG_AES_CBC_NOPAD, mode, key1,
+					  key1_len, key2, key2_len, iv,
+					  iv_len);
+		if (res != TEE_SUCCESS)
+			return res;
+		ltc_res = CRYPT_OK;
+		break;
+#endif
+#if defined(CFG_CRYPTO_XTS)
+	case TEE_ALG_AES_XTS:
+		xts = ctx;
+		if (key1_len != key2_len)
+			return TEE_ERROR_BAD_PARAMETERS;
+		if (iv) {
+			if (iv_len != XTS_TWEAK_SIZE)
+				return TEE_ERROR_BAD_PARAMETERS;
+			memcpy(xts->tweak, iv, iv_len);
+		} else {
+			memset(xts->tweak, 0, XTS_TWEAK_SIZE);
+		}
+		ltc_res = xts_start(
+			ltc_cipherindex, key1, key2, key1_len,
+			0, &xts->ctx);
+		break;
+#endif
+	default:
+		return TEE_ERROR_NOT_SUPPORTED;
+	}
+
+	if (ltc_res == CRYPT_OK)
+		return TEE_SUCCESS;
+	else
+		return TEE_ERROR_BAD_STATE;
+}
+
+static TEE_Result cipher_update(void *ctx, uint32_t algo,
+				TEE_OperationMode mode,
+				bool last_block __maybe_unused,
+				const uint8_t *data, size_t len, uint8_t *dst)
+{
+	int ltc_res = CRYPT_OK;
+#if defined(CFG_CRYPTO_CTS)
+	struct tee_symmetric_cts *cts;
+#endif
+#if defined(CFG_CRYPTO_XTS)
+	struct tee_symmetric_xts *xts;
+#endif
+
+	switch (algo) {
+#if defined(CFG_CRYPTO_ECB)
+	case TEE_ALG_AES_ECB_NOPAD:
+	case TEE_ALG_DES_ECB_NOPAD:
+	case TEE_ALG_DES3_ECB_NOPAD:
+		if (mode == TEE_MODE_ENCRYPT)
+			ltc_res = ecb_encrypt(data, dst, len, ctx);
+		else
+			ltc_res = ecb_decrypt(data, dst, len, ctx);
+		break;
+#endif
+#if defined(CFG_CRYPTO_CBC)
+	case TEE_ALG_AES_CBC_NOPAD:
+	case TEE_ALG_DES_CBC_NOPAD:
+	case TEE_ALG_DES3_CBC_NOPAD:
+		if (mode == TEE_MODE_ENCRYPT)
+			ltc_res = cbc_encrypt(data, dst, len, ctx);
+		else
+			ltc_res = cbc_decrypt(data, dst, len, ctx);
+		break;
+#endif
+#if defined(CFG_CRYPTO_CTR)
+	case TEE_ALG_AES_CTR:
+		if (mode == TEE_MODE_ENCRYPT)
+			ltc_res = ctr_encrypt(data, dst, len, ctx);
+		else
+			ltc_res = ctr_decrypt(data, dst, len, ctx);
+		break;
+#endif
+#if defined(CFG_CRYPTO_XTS)
+	case TEE_ALG_AES_XTS:
+		xts = ctx;
+		if (mode == TEE_MODE_ENCRYPT)
+			ltc_res = xts_encrypt(data, len, dst, xts->tweak,
+					      &xts->ctx);
+		else
+			ltc_res = xts_decrypt(data, len, dst, xts->tweak,
+					      &xts->ctx);
+		break;
+#endif
+#if defined(CFG_CRYPTO_CTS)
+	case TEE_ALG_AES_CTS:
+		cts = ctx;
+		return tee_aes_cbc_cts_update(&cts->cbc, &cts->ecb, mode,
+					      last_block, data, len, dst);
+#endif
+	default:
+		return TEE_ERROR_NOT_SUPPORTED;
+	}
+
+	if (ltc_res == CRYPT_OK)
+		return TEE_SUCCESS;
+	else
+		return TEE_ERROR_BAD_STATE;
+}
+
+static void cipher_final(void *ctx, uint32_t algo)
+{
+	switch (algo) {
+#if defined(CFG_CRYPTO_ECB)
+	case TEE_ALG_AES_ECB_NOPAD:
+	case TEE_ALG_DES_ECB_NOPAD:
+	case TEE_ALG_DES3_ECB_NOPAD:
+		ecb_done(ctx);
+		break;
+#endif
+#if defined(CFG_CRYPTO_CBC)
+	case TEE_ALG_AES_CBC_NOPAD:
+	case TEE_ALG_DES_CBC_NOPAD:
+	case TEE_ALG_DES3_CBC_NOPAD:
+	case TEE_ALG_AES_CBC_MAC_NOPAD:
+	case TEE_ALG_AES_CBC_MAC_PKCS5:
+	case TEE_ALG_DES_CBC_MAC_NOPAD:
+	case TEE_ALG_DES_CBC_MAC_PKCS5:
+	case TEE_ALG_DES3_CBC_MAC_NOPAD:
+	case TEE_ALG_DES3_CBC_MAC_PKCS5:
+		cbc_done(ctx);
+		break;
+#endif
+#if defined(CFG_CRYPTO_CTR)
+	case TEE_ALG_AES_CTR:
+		ctr_done(ctx);
+		break;
+#endif
+#if defined(CFG_CRYPTO_XTS)
+	case TEE_ALG_AES_XTS:
+		xts_done(&(((struct tee_symmetric_xts *)ctx)->ctx));
+		break;
+#endif
+#if defined(CFG_CRYPTO_CTS)
+	case TEE_ALG_AES_CTS:
+		cbc_done(&(((struct tee_symmetric_cts *)ctx)->cbc));
+		ecb_done(&(((struct tee_symmetric_cts *)ctx)->ecb));
+		break;
+#endif
+	default:
+		assert(!"Unhandled algo");
+		break;
+	}
+}
+#endif /* _CFG_CRYPTO_WITH_CIPHER */
+
+/*****************************************************************************
+ * Message Authentication Code functions
+ *****************************************************************************/
+
+#if defined(_CFG_CRYPTO_WITH_MAC)
+
+#if defined(CFG_CRYPTO_CBC_MAC)
+/*
+ * CBC-MAC is not implemented in Libtomcrypt
+ * This is implemented here as being the plain text which is encoded with IV=0.
+ * Result of the CBC-MAC is the last 16-bytes cipher.
+ */
+
+#define CBCMAC_MAX_BLOCK_LEN 16
+struct cbc_state {
+	symmetric_CBC cbc;
+	uint8_t block[CBCMAC_MAX_BLOCK_LEN];
+	uint8_t digest[CBCMAC_MAX_BLOCK_LEN];
+	size_t current_block_len, block_len;
+	int is_computed;
+};
+#endif
+
+static TEE_Result mac_get_ctx_size(uint32_t algo, size_t *size)
+{
+	switch (algo) {
+#if defined(CFG_CRYPTO_HMAC)
+	case TEE_ALG_HMAC_MD5:
+	case TEE_ALG_HMAC_SHA224:
+	case TEE_ALG_HMAC_SHA1:
+	case TEE_ALG_HMAC_SHA256:
+	case TEE_ALG_HMAC_SHA384:
+	case TEE_ALG_HMAC_SHA512:
+		*size = sizeof(hmac_state);
+		break;
+#endif
+#if defined(CFG_CRYPTO_CBC_MAC)
+	case TEE_ALG_AES_CBC_MAC_NOPAD:
+	case TEE_ALG_AES_CBC_MAC_PKCS5:
+	case TEE_ALG_DES_CBC_MAC_NOPAD:
+	case TEE_ALG_DES_CBC_MAC_PKCS5:
+	case TEE_ALG_DES3_CBC_MAC_NOPAD:
+	case TEE_ALG_DES3_CBC_MAC_PKCS5:
+		*size = sizeof(struct cbc_state);
+		break;
+#endif
+#if defined(CFG_CRYPTO_CMAC)
+	case TEE_ALG_AES_CMAC:
+		*size = sizeof(omac_state);
+		break;
+#endif
+	default:
+		return TEE_ERROR_NOT_SUPPORTED;
+	}
+
+	return TEE_SUCCESS;
+}
+
+static TEE_Result mac_init(void *ctx, uint32_t algo, const uint8_t *key,
+			   size_t len)
+{
+	TEE_Result res;
+#if defined(CFG_CRYPTO_HMAC)
+	int ltc_hashindex;
+#endif
+#if defined(CFG_CRYPTO_CBC_MAC) || defined(CFG_CRYPTO_CMAC)
+	int ltc_cipherindex;
+#endif
+#if defined(CFG_CRYPTO_CBC_MAC)
+	uint8_t *real_key;
+	uint8_t key_array[24];
+	size_t real_key_len;
+	uint8_t iv[CBCMAC_MAX_BLOCK_LEN];
+	struct cbc_state *cbc;
+#endif
+
+	switch (algo) {
+#if defined(CFG_CRYPTO_HMAC)
+	case TEE_ALG_HMAC_MD5:
+	case TEE_ALG_HMAC_SHA224:
+	case TEE_ALG_HMAC_SHA1:
+	case TEE_ALG_HMAC_SHA256:
+	case TEE_ALG_HMAC_SHA384:
+	case TEE_ALG_HMAC_SHA512:
+		res = tee_algo_to_ltc_hashindex(algo, &ltc_hashindex);
+		if (res != TEE_SUCCESS)
+			return res;
+		if (CRYPT_OK !=
+		    hmac_init((hmac_state *)ctx, ltc_hashindex, key, len))
+			return TEE_ERROR_BAD_STATE;
+		break;
+#endif
+#if defined(CFG_CRYPTO_CBC_MAC)
+	case TEE_ALG_AES_CBC_MAC_NOPAD:
+	case TEE_ALG_AES_CBC_MAC_PKCS5:
+	case TEE_ALG_DES_CBC_MAC_NOPAD:
+	case TEE_ALG_DES_CBC_MAC_PKCS5:
+	case TEE_ALG_DES3_CBC_MAC_NOPAD:
+	case TEE_ALG_DES3_CBC_MAC_PKCS5:
+		cbc = (struct cbc_state *)ctx;
+
+		res = tee_algo_to_ltc_cipherindex(algo, &ltc_cipherindex);
+		if (res != TEE_SUCCESS)
+			return res;
+
+		cbc->block_len =
+			cipher_descriptor[ltc_cipherindex]->block_length;
+		if (CBCMAC_MAX_BLOCK_LEN < cbc->block_len)
+			return TEE_ERROR_BAD_PARAMETERS;
+		memset(iv, 0, cbc->block_len);
+
+		if (algo == TEE_ALG_DES3_CBC_MAC_NOPAD ||
+		    algo == TEE_ALG_DES3_CBC_MAC_PKCS5) {
+			get_des2_key(key, len, key_array,
+				     &real_key, &real_key_len);
+			key = real_key;
+			len = real_key_len;
+		}
+		if (CRYPT_OK != cbc_start(
+			ltc_cipherindex, iv, key, len, 0, &cbc->cbc))
+				return TEE_ERROR_BAD_STATE;
+		cbc->is_computed = 0;
+		cbc->current_block_len = 0;
+		break;
+#endif
+#if defined(CFG_CRYPTO_CMAC)
+	case TEE_ALG_AES_CMAC:
+		res = tee_algo_to_ltc_cipherindex(algo, &ltc_cipherindex);
+		if (res != TEE_SUCCESS)
+			return res;
+		if (CRYPT_OK != omac_init((omac_state *)ctx, ltc_cipherindex,
+					  key, len))
+			return TEE_ERROR_BAD_STATE;
+		break;
+#endif
+	default:
+		return TEE_ERROR_NOT_SUPPORTED;
+	}
+
+	return TEE_SUCCESS;
+}
+
+static TEE_Result mac_update(void *ctx, uint32_t algo, const uint8_t *data,
+			     size_t len)
+{
+#if defined(CFG_CRYPTO_CBC_MAC)
+	int ltc_res;
+	struct cbc_state *cbc;
+	size_t pad_len;
+#endif
+
+	if (!data || !len)
+		return TEE_SUCCESS;
+
+	switch (algo) {
+#if defined(CFG_CRYPTO_HMAC)
+	case TEE_ALG_HMAC_MD5:
+	case TEE_ALG_HMAC_SHA224:
+	case TEE_ALG_HMAC_SHA1:
+	case TEE_ALG_HMAC_SHA256:
+	case TEE_ALG_HMAC_SHA384:
+	case TEE_ALG_HMAC_SHA512:
+		if (CRYPT_OK != hmac_process((hmac_state *)ctx, data, len))
+			return TEE_ERROR_BAD_STATE;
+		break;
+#endif
+#if defined(CFG_CRYPTO_CBC_MAC)
+	case TEE_ALG_AES_CBC_MAC_NOPAD:
+	case TEE_ALG_AES_CBC_MAC_PKCS5:
+	case TEE_ALG_DES_CBC_MAC_NOPAD:
+	case TEE_ALG_DES_CBC_MAC_PKCS5:
+	case TEE_ALG_DES3_CBC_MAC_NOPAD:
+	case TEE_ALG_DES3_CBC_MAC_PKCS5:
+		cbc = ctx;
+
+		if ((cbc->current_block_len > 0) &&
+		    (len + cbc->current_block_len >= cbc->block_len)) {
+			pad_len = cbc->block_len - cbc->current_block_len;
+			memcpy(cbc->block + cbc->current_block_len,
+			       data, pad_len);
+			data += pad_len;
+			len -= pad_len;
+			ltc_res = cbc_encrypt(cbc->block, cbc->digest,
+					      cbc->block_len, &cbc->cbc);
+			if (CRYPT_OK != ltc_res)
+				return TEE_ERROR_BAD_STATE;
+			cbc->is_computed = 1;
+		}
+
+		while (len >= cbc->block_len) {
+			ltc_res = cbc_encrypt(data, cbc->digest,
+					      cbc->block_len, &cbc->cbc);
+			if (CRYPT_OK != ltc_res)
+				return TEE_ERROR_BAD_STATE;
+			cbc->is_computed = 1;
+			data += cbc->block_len;
+			len -= cbc->block_len;
+		}
+
+		if (len > 0)
+			memcpy(cbc->block, data, len);
+		cbc->current_block_len = len;
+		break;
+#endif
+#if defined(CFG_CRYPTO_CMAC)
+	case TEE_ALG_AES_CMAC:
+		if (CRYPT_OK != omac_process((omac_state *)ctx, data, len))
+			return TEE_ERROR_BAD_STATE;
+		break;
+#endif
+	default:
+		return TEE_ERROR_NOT_SUPPORTED;
+	}
+
+	return TEE_SUCCESS;
+}
+
+static TEE_Result mac_final(void *ctx, uint32_t algo, uint8_t *digest,
+			    size_t digest_len)
+{
+#if defined(CFG_CRYPTO_CBC_MAC)
+	struct cbc_state *cbc;
+	size_t pad_len;
+#endif
+	unsigned long ltc_digest_len = digest_len;
+
+	switch (algo) {
+#if defined(CFG_CRYPTO_HMAC)
+	case TEE_ALG_HMAC_MD5:
+	case TEE_ALG_HMAC_SHA224:
+	case TEE_ALG_HMAC_SHA1:
+	case TEE_ALG_HMAC_SHA256:
+	case TEE_ALG_HMAC_SHA384:
+	case TEE_ALG_HMAC_SHA512:
+		if (CRYPT_OK != hmac_done((hmac_state *)ctx, digest,
+					  &ltc_digest_len))
+			return TEE_ERROR_BAD_STATE;
+		break;
+#endif
+#if defined(CFG_CRYPTO_CBC_MAC)
+	case TEE_ALG_AES_CBC_MAC_NOPAD:
+	case TEE_ALG_AES_CBC_MAC_PKCS5:
+	case TEE_ALG_DES_CBC_MAC_NOPAD:
+	case TEE_ALG_DES_CBC_MAC_PKCS5:
+	case TEE_ALG_DES3_CBC_MAC_NOPAD:
+	case TEE_ALG_DES3_CBC_MAC_PKCS5:
+		cbc = (struct cbc_state *)ctx;
+
+		/* Padding is required */
+		switch (algo) {
+		case TEE_ALG_AES_CBC_MAC_PKCS5:
+		case TEE_ALG_DES_CBC_MAC_PKCS5:
+		case TEE_ALG_DES3_CBC_MAC_PKCS5:
+			/*
+			 * Padding is in whole bytes. The value of each added
+			 * byte is the number of bytes that are added, i.e. N
+			 * bytes, each of value N are added
+			 */
+			pad_len = cbc->block_len - cbc->current_block_len;
+			memset(cbc->block+cbc->current_block_len,
+			       pad_len, pad_len);
+			cbc->current_block_len = 0;
+			if (TEE_SUCCESS != mac_update(
+				ctx, algo, cbc->block, cbc->block_len))
+					return TEE_ERROR_BAD_STATE;
+			break;
+		default:
+			/* nothing to do */
+			break;
+		}
+
+		if ((!cbc->is_computed) || (cbc->current_block_len != 0))
+			return TEE_ERROR_BAD_STATE;
+
+		memcpy(digest, cbc->digest, MIN(ltc_digest_len,
+						cbc->block_len));
+		cipher_final(&cbc->cbc, algo);
+		break;
+#endif
+#if defined(CFG_CRYPTO_CMAC)
+	case TEE_ALG_AES_CMAC:
+		if (CRYPT_OK != omac_done((omac_state *)ctx, digest,
+					  &ltc_digest_len))
+			return TEE_ERROR_BAD_STATE;
+		break;
+#endif
+	default:
+		return TEE_ERROR_NOT_SUPPORTED;
+	}
+
+	return TEE_SUCCESS;
+}
+#endif /* _CFG_CRYPTO_WITH_MAC */
+
+/******************************************************************************
+ * Authenticated encryption
+ ******************************************************************************/
+
+#if defined(_CFG_CRYPTO_WITH_AUTHENC)
+
+#define TEE_CCM_KEY_MAX_LENGTH		32
+#define TEE_CCM_NONCE_MAX_LENGTH	13
+#define TEE_CCM_TAG_MAX_LENGTH		16
+#define TEE_GCM_TAG_MAX_LENGTH		16
+#define TEE_xCM_TAG_MAX_LENGTH		16
+
+#if defined(CFG_CRYPTO_CCM)
+struct tee_ccm_state {
+	ccm_state ctx;			/* the ccm state as defined by LTC */
+	size_t tag_len;			/* tag length */
+};
+#endif
+
+#if defined(CFG_CRYPTO_GCM)
+struct tee_gcm_state {
+	gcm_state ctx;			/* the gcm state as defined by LTC */
+	size_t tag_len;			/* tag length */
+};
+#endif
+
+static TEE_Result authenc_get_ctx_size(uint32_t algo, size_t *size)
+{
+	switch (algo) {
+#if defined(CFG_CRYPTO_CCM)
+	case TEE_ALG_AES_CCM:
+		*size = sizeof(struct tee_ccm_state);
+		break;
+#endif
+#if defined(CFG_CRYPTO_GCM)
+	case TEE_ALG_AES_GCM:
+		*size = sizeof(struct tee_gcm_state);
+		break;
+#endif
+	default:
+		return TEE_ERROR_NOT_SUPPORTED;
+	}
+	return TEE_SUCCESS;
+}
+
+static TEE_Result authenc_init(void *ctx, uint32_t algo,
+			       TEE_OperationMode mode __unused,
+			       const uint8_t *key, size_t key_len,
+			       const uint8_t *nonce, size_t nonce_len,
+			       size_t tag_len, size_t aad_len __maybe_unused,
+			       size_t payload_len __maybe_unused)
+{
+	TEE_Result res;
+	int ltc_res;
+	int ltc_cipherindex;
+#if defined(CFG_CRYPTO_CCM)
+	struct tee_ccm_state *ccm;
+#endif
+#if defined(CFG_CRYPTO_GCM)
+	struct tee_gcm_state *gcm;
+#endif
+
+	res = tee_algo_to_ltc_cipherindex(algo, &ltc_cipherindex);
+	if (res != TEE_SUCCESS)
+		return TEE_ERROR_NOT_SUPPORTED;
+	switch (algo) {
+#if defined(CFG_CRYPTO_CCM)
+	case TEE_ALG_AES_CCM:
+		/* reset the state */
+		ccm = ctx;
+		memset(ccm, 0, sizeof(struct tee_ccm_state));
+		ccm->tag_len = tag_len;
+
+		/* Check the key length */
+		if ((!key) || (key_len > TEE_CCM_KEY_MAX_LENGTH))
+			return TEE_ERROR_BAD_PARAMETERS;
+
+		/* check the nonce */
+		if (nonce_len > TEE_CCM_NONCE_MAX_LENGTH)
+			return TEE_ERROR_BAD_PARAMETERS;
+
+		/* check the tag len */
+		if ((tag_len < 4) ||
+		    (tag_len > TEE_CCM_TAG_MAX_LENGTH) ||
+		    (tag_len % 2 != 0))
+			return TEE_ERROR_NOT_SUPPORTED;
+
+		ltc_res = ccm_init(&ccm->ctx, ltc_cipherindex, key, key_len,
+				   payload_len, tag_len, aad_len);
+		if (ltc_res != CRYPT_OK)
+			return TEE_ERROR_BAD_STATE;
+
+		/* Add the IV */
+		ltc_res = ccm_add_nonce(&ccm->ctx, nonce, nonce_len);
+		if (ltc_res != CRYPT_OK)
+			return TEE_ERROR_BAD_STATE;
+		break;
+#endif
+#if defined(CFG_CRYPTO_GCM)
+	case TEE_ALG_AES_GCM:
+		/* reset the state */
+		gcm = ctx;
+		memset(gcm, 0, sizeof(struct tee_gcm_state));
+		gcm->tag_len = tag_len;
+
+		ltc_res = gcm_init(&gcm->ctx, ltc_cipherindex, key, key_len);
+		if (ltc_res != CRYPT_OK)
+			return TEE_ERROR_BAD_STATE;
+
+		/* Add the IV */
+		ltc_res = gcm_add_iv(&gcm->ctx, nonce, nonce_len);
+		if (ltc_res != CRYPT_OK)
+			return TEE_ERROR_BAD_STATE;
+		break;
+#endif
+	default:
+		return TEE_ERROR_NOT_SUPPORTED;
+	}
+
+	return TEE_SUCCESS;
+}
+
+static TEE_Result authenc_update_aad(void *ctx, uint32_t algo,
+				     TEE_OperationMode mode __unused,
+				     const uint8_t *data, size_t len)
+{
+#if defined(CFG_CRYPTO_CCM)
+	struct tee_ccm_state *ccm;
+#endif
+#if defined(CFG_CRYPTO_GCM)
+	struct tee_gcm_state *gcm;
+#endif
+	int ltc_res;
+
+	switch (algo) {
+#if defined(CFG_CRYPTO_CCM)
+	case TEE_ALG_AES_CCM:
+		/* Add the AAD (note: aad can be NULL if aadlen == 0) */
+		ccm = ctx;
+		ltc_res = ccm_add_aad(&ccm->ctx, data, len);
+		if (ltc_res != CRYPT_OK)
+			return TEE_ERROR_BAD_STATE;
+		break;
+#endif
+#if defined(CFG_CRYPTO_GCM)
+	case TEE_ALG_AES_GCM:
+		/* Add the AAD (note: aad can be NULL if aadlen == 0) */
+		gcm = ctx;
+		ltc_res = gcm_add_aad(&gcm->ctx, data, len);
+		if (ltc_res != CRYPT_OK)
+			return TEE_ERROR_BAD_STATE;
+		break;
+#endif
+	default:
+		return TEE_ERROR_NOT_SUPPORTED;
+	}
+
+	return TEE_SUCCESS;
+}
+
+static TEE_Result authenc_update_payload(void *ctx, uint32_t algo,
+					 TEE_OperationMode mode,
+					 const uint8_t *src_data,
+					 size_t src_len,
+					 uint8_t *dst_data,
+					 size_t *dst_len)
+{
+#if defined(CFG_CRYPTO_GCM)
+	TEE_Result res;
+#endif
+	int ltc_res, dir;
+#if defined(CFG_CRYPTO_CCM)
+	struct tee_ccm_state *ccm;
+#endif
+#if defined(CFG_CRYPTO_GCM)
+	struct tee_gcm_state *gcm;
+#endif
+	unsigned char *pt, *ct;	/* the plain and the cipher text */
+
+	if (mode == TEE_MODE_ENCRYPT) {
+		pt = (unsigned char *)src_data;
+		ct = dst_data;
+	} else {
+		pt = dst_data;
+		ct = (unsigned char *)src_data;
+	}
+
+	switch (algo) {
+#if defined(CFG_CRYPTO_CCM)
+	case TEE_ALG_AES_CCM:
+		ccm = ctx;
+		dir = (mode == TEE_MODE_ENCRYPT ? CCM_ENCRYPT : CCM_DECRYPT);
+		ltc_res = ccm_process(&ccm->ctx, pt, src_len, ct, dir);
+		if (ltc_res != CRYPT_OK)
+			return TEE_ERROR_BAD_STATE;
+		*dst_len = src_len;
+		break;
+#endif
+#if defined(CFG_CRYPTO_GCM)
+	case TEE_ALG_AES_GCM:
+		/* aad is optional ==> add one without length */
+		gcm = ctx;
+		if (gcm->ctx.mode == LTC_GCM_MODE_IV) {
+			res = authenc_update_aad(gcm, algo, mode, 0, 0);
+			if (res != TEE_SUCCESS)
+				return res;
+		}
+
+		/* process the data */
+		dir = (mode == TEE_MODE_ENCRYPT ? GCM_ENCRYPT : GCM_DECRYPT);
+		ltc_res = gcm_process(&gcm->ctx, pt, src_len, ct, dir);
+		if (ltc_res != CRYPT_OK)
+			return TEE_ERROR_BAD_STATE;
+		*dst_len = src_len;
+		break;
+#endif
+	default:
+		return TEE_ERROR_NOT_SUPPORTED;
+	}
+
+	return TEE_SUCCESS;
+}
+
+static TEE_Result authenc_enc_final(void *ctx, uint32_t algo,
+				    const uint8_t *src_data,
+				    size_t src_len, uint8_t *dst_data,
+				    size_t *dst_len, uint8_t *dst_tag,
+				    size_t *dst_tag_len)
+{
+	TEE_Result res;
+#if defined(CFG_CRYPTO_CCM)
+	struct tee_ccm_state *ccm;
+#endif
+#if defined(CFG_CRYPTO_GCM)
+	struct tee_gcm_state *gcm;
+#endif
+	size_t digest_size;
+	int ltc_res;
+
+	/* Check the resulting buffer is not too short */
+	res = cipher_get_block_size(algo, &digest_size);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	/* Finalize the remaining buffer */
+	res = authenc_update_payload(ctx, algo, TEE_MODE_ENCRYPT, src_data,
+				     src_len, dst_data, dst_len);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	switch (algo) {
+#if defined(CFG_CRYPTO_CCM)
+	case TEE_ALG_AES_CCM:
+		/* Check the tag length */
+		ccm = ctx;
+		if (*dst_tag_len < ccm->tag_len) {
+			*dst_tag_len = ccm->tag_len;
+			return TEE_ERROR_SHORT_BUFFER;
+		}
+		*dst_tag_len = ccm->tag_len;
+
+		/* Compute the tag */
+		ltc_res = ccm_done(&ccm->ctx, dst_tag,
+				   (unsigned long *)dst_tag_len);
+		if (ltc_res != CRYPT_OK)
+			return TEE_ERROR_BAD_STATE;
+		break;
+#endif
+#if defined(CFG_CRYPTO_GCM)
+	case TEE_ALG_AES_GCM:
+		/* Check the tag length */
+		gcm = ctx;
+		if (*dst_tag_len < gcm->tag_len) {
+			*dst_tag_len = gcm->tag_len;
+			return TEE_ERROR_SHORT_BUFFER;
+		}
+		*dst_tag_len = gcm->tag_len;
+
+		/* Compute the tag */
+		ltc_res = gcm_done(&gcm->ctx, dst_tag,
+				   (unsigned long *)dst_tag_len);
+		if (ltc_res != CRYPT_OK)
+			return TEE_ERROR_BAD_STATE;
+		break;
+#endif
+	default:
+		return TEE_ERROR_NOT_SUPPORTED;
+	}
+
+	return TEE_SUCCESS;
+}
+
+static TEE_Result authenc_dec_final(void *ctx, uint32_t algo,
+				    const uint8_t *src_data, size_t src_len,
+				    uint8_t *dst_data, size_t *dst_len,
+				    const uint8_t *tag, size_t tag_len)
+{
+	TEE_Result res = TEE_ERROR_BAD_STATE;
+#if defined(CFG_CRYPTO_CCM)
+	struct tee_ccm_state *ccm;
+#endif
+#if defined(CFG_CRYPTO_GCM)
+	struct tee_gcm_state *gcm;
+#endif
+	int ltc_res;
+	uint8_t dst_tag[TEE_xCM_TAG_MAX_LENGTH];
+	unsigned long ltc_tag_len = tag_len;
+
+	if (tag_len == 0)
+		return TEE_ERROR_SHORT_BUFFER;
+	if (tag_len > TEE_xCM_TAG_MAX_LENGTH)
+		return TEE_ERROR_BAD_STATE;
+
+	/* Process the last buffer, if any */
+	res = authenc_update_payload(ctx, algo, TEE_MODE_DECRYPT, src_data,
+				     src_len, dst_data, dst_len);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	switch (algo) {
+#if defined(CFG_CRYPTO_CCM)
+	case TEE_ALG_AES_CCM:
+		/* Finalize the authentication */
+		ccm = ctx;
+		ltc_res = ccm_done(&ccm->ctx, dst_tag, &ltc_tag_len);
+		if (ltc_res != CRYPT_OK)
+			return TEE_ERROR_BAD_STATE;
+		break;
+#endif
+#if defined(CFG_CRYPTO_GCM)
+	case TEE_ALG_AES_GCM:
+		/* Finalize the authentication */
+		gcm = ctx;
+		ltc_res = gcm_done(&gcm->ctx, dst_tag, &ltc_tag_len);
+		if (ltc_res != CRYPT_OK)
+			return TEE_ERROR_BAD_STATE;
+		break;
+#endif
+	default:
+		return TEE_ERROR_NOT_SUPPORTED;
+	}
+
+	if (buf_compare_ct(dst_tag, tag, tag_len) != 0)
+		res = TEE_ERROR_MAC_INVALID;
+	else
+		res = TEE_SUCCESS;
+	return res;
+}
+
+static void authenc_final(void *ctx, uint32_t algo)
+{
+#if defined(CFG_CRYPTO_CCM)
+	struct tee_ccm_state *ccm;
+#endif
+#if defined(CFG_CRYPTO_GCM)
+	struct tee_gcm_state *gcm;
+#endif
+
+	switch (algo) {
+#if defined(CFG_CRYPTO_CCM)
+	case TEE_ALG_AES_CCM:
+		ccm = ctx;
+		ccm_reset(&ccm->ctx);
+		break;
+#endif
+#if defined(CFG_CRYPTO_GCM)
+	case TEE_ALG_AES_GCM:
+		gcm = ctx;
+		gcm_reset(&gcm->ctx);
+		break;
+#endif
+	default:
+		break;
+	}
+}
+#endif /* _CFG_CRYPTO_WITH_AUTHENC */
+
+/******************************************************************************
+ * Pseudo Random Number Generator
+ ******************************************************************************/
+static TEE_Result prng_read(void *buf, size_t blen)
+{
+	int err;
+	struct tee_ltc_prng *prng = tee_ltc_get_prng();
+
+	err = prng_is_valid(prng->index);
+
+	if (err != CRYPT_OK)
+		return TEE_ERROR_BAD_STATE;
+
+	if (prng_descriptor[prng->index]->read(buf, blen, &prng->state) !=
+			(unsigned long)blen)
+		return TEE_ERROR_BAD_STATE;
+
+	return TEE_SUCCESS;
+}
+
+static TEE_Result prng_add_entropy(const uint8_t *inbuf, size_t len)
+{
+	int err;
+	struct tee_ltc_prng *prng = tee_ltc_get_prng();
+
+	err = prng_is_valid(prng->index);
+
+	if (err != CRYPT_OK)
+		return TEE_ERROR_BAD_STATE;
+
+	err = prng_descriptor[prng->index]->add_entropy(
+			inbuf, len, &prng->state);
+
+	if (err != CRYPT_OK)
+		return TEE_ERROR_BAD_STATE;
+
+	return TEE_SUCCESS;
+}
+
+static TEE_Result tee_ltc_init(void)
+{
+#if defined(_CFG_CRYPTO_WITH_ACIPHER)
+	tee_ltc_alloc_mpa();
+#endif
+	tee_ltc_reg_algs();
+
+	return tee_ltc_prng_init(tee_ltc_get_prng());
+}
+
+const struct crypto_ops crypto_ops = {
+	.name = "LibTomCrypt provider",
+	.init = tee_ltc_init,
+#if defined(_CFG_CRYPTO_WITH_HASH)
+	.hash = {
+		.get_ctx_size = hash_get_ctx_size,
+		.init = hash_init,
+		.update = hash_update,
+		.final = hash_final,
+	},
+#endif
+#if defined(_CFG_CRYPTO_WITH_CIPHER)
+	.cipher = {
+		.final = cipher_final,
+		.get_block_size = cipher_get_block_size,
+		.get_ctx_size = cipher_get_ctx_size,
+		.init = cipher_init,
+		.update = cipher_update,
+	},
+#endif
+#if defined(_CFG_CRYPTO_WITH_MAC)
+	.mac = {
+		.get_ctx_size = mac_get_ctx_size,
+		.init = mac_init,
+		.update = mac_update,
+		.final = mac_final,
+	},
+#endif
+#if defined(_CFG_CRYPTO_WITH_AUTHENC)
+	.authenc = {
+		.dec_final = authenc_dec_final,
+		.enc_final = authenc_enc_final,
+		.final = authenc_final,
+		.get_ctx_size = authenc_get_ctx_size,
+		.init = authenc_init,
+		.update_aad = authenc_update_aad,
+		.update_payload = authenc_update_payload,
+	},
+#endif
+#if defined(_CFG_CRYPTO_WITH_ACIPHER)
+	.acipher = {
+#if defined(CFG_CRYPTO_RSA)
+		.alloc_rsa_keypair = alloc_rsa_keypair,
+		.alloc_rsa_public_key = alloc_rsa_public_key,
+		.free_rsa_public_key = free_rsa_public_key,
+		.gen_rsa_key = gen_rsa_key,
+		.rsaes_decrypt = rsaes_decrypt,
+		.rsaes_encrypt = rsaes_encrypt,
+		.rsanopad_decrypt = rsanopad_decrypt,
+		.rsanopad_encrypt = rsanopad_encrypt,
+		.rsassa_sign = rsassa_sign,
+		.rsassa_verify = rsassa_verify,
+#endif
+#if defined(CFG_CRYPTO_DH)
+		.alloc_dh_keypair = alloc_dh_keypair,
+		.gen_dh_key = gen_dh_key,
+		.dh_shared_secret = do_dh_shared_secret,
+#endif
+#if defined(CFG_CRYPTO_DSA)
+		.alloc_dsa_keypair = alloc_dsa_keypair,
+		.alloc_dsa_public_key = alloc_dsa_public_key,
+		.gen_dsa_key = gen_dsa_key,
+		.dsa_sign = dsa_sign,
+		.dsa_verify = dsa_verify,
+#endif
+#if defined(CFG_CRYPTO_ECC)
+		/* ECDSA and ECDH */
+		.alloc_ecc_keypair = alloc_ecc_keypair,
+		.alloc_ecc_public_key = alloc_ecc_public_key,
+		.gen_ecc_key = gen_ecc_key,
+		.free_ecc_public_key = free_ecc_public_key,
+
+		/* ECDSA only */
+		.ecc_sign = ecc_sign,
+		.ecc_verify = ecc_verify,
+		/* ECDH only */
+		.ecc_shared_secret = do_ecc_shared_secret,
+#endif
+	},
+	.bignum = {
+		.allocate = bn_allocate,
+		.num_bytes = num_bytes,
+		.num_bits = num_bits,
+		.compare = compare,
+		.bn2bin = bn2bin,
+		.bin2bn = bin2bn,
+		.copy = copy,
+		.free = bn_free,
+		.clear = bn_clear
+	},
+#endif /* _CFG_CRYPTO_WITH_ACIPHER */
+	.prng = {
+		.add_entropy = prng_add_entropy,
+		.read = prng_read,
+	}
+};
+
+#if defined(CFG_WITH_VFP)
+void tomcrypt_arm_neon_enable(struct tomcrypt_arm_neon_state *state)
+{
+	state->state = thread_kernel_enable_vfp();
+}
+
+void tomcrypt_arm_neon_disable(struct tomcrypt_arm_neon_state *state)
+{
+	thread_kernel_disable_vfp(state->state);
+}
+#endif
+
+#if defined(CFG_CRYPTO_SHA256)
+TEE_Result hash_sha256_check(const uint8_t *hash, const uint8_t *data,
+		size_t data_size)
+{
+	hash_state hs;
+	uint8_t digest[TEE_SHA256_HASH_SIZE];
+
+	if (sha256_init(&hs) != CRYPT_OK)
+		return TEE_ERROR_GENERIC;
+	if (sha256_process(&hs, data, data_size) != CRYPT_OK)
+		return TEE_ERROR_GENERIC;
+	if (sha256_done(&hs, digest) != CRYPT_OK)
+		return TEE_ERROR_GENERIC;
+	if (buf_compare_ct(digest, hash, sizeof(digest)) != 0)
+		return TEE_ERROR_SECURITY;
+	return TEE_SUCCESS;
+}
+#endif
+
+TEE_Result rng_generate(void *buffer, size_t len)
+{
+#if defined(CFG_WITH_SOFTWARE_PRNG)
+#ifdef _CFG_CRYPTO_WITH_FORTUNA_PRNG
+	int (*start)(prng_state *) = fortuna_start;
+	int (*ready)(prng_state *) = fortuna_ready;
+	unsigned long (*read)(unsigned char *, unsigned long, prng_state *) =
+		fortuna_read;
+#else
+	int (*start)(prng_state *) = rc4_start;
+	int (*ready)(prng_state *) = rc4_ready;
+	unsigned long (*read)(unsigned char *, unsigned long, prng_state *) =
+		rc4_read;
+#endif
+
+	if (!_tee_ltc_prng.inited) {
+		if (start(&_tee_ltc_prng.state) != CRYPT_OK)
+			return TEE_ERROR_BAD_STATE;
+		if (ready(&_tee_ltc_prng.state) != CRYPT_OK)
+			return TEE_ERROR_BAD_STATE;
+		_tee_ltc_prng.inited = true;
+	}
+	if (read(buffer, len, &_tee_ltc_prng.state) != len)
+		return TEE_ERROR_BAD_STATE;
+	return TEE_SUCCESS;
+
+
+#else
+	return get_rng_array(buffer, len);
+#endif
+}
diff --git a/core/lib/libtomcrypt/sub.mk b/core/lib/libtomcrypt/sub.mk
new file mode 100644
index 0000000..e1cfa65
--- /dev/null
+++ b/core/lib/libtomcrypt/sub.mk
@@ -0,0 +1,128 @@
+CFG_CRYPTO ?= y
+CFG_CRYPTO_SIZE_OPTIMIZATION ?= y
+
+ifeq (y,$(CFG_CRYPTO))
+
+# Ciphers
+CFG_CRYPTO_AES ?= y
+CFG_CRYPTO_DES ?= y
+
+# Cipher block modes
+CFG_CRYPTO_ECB ?= y
+CFG_CRYPTO_CBC ?= y
+CFG_CRYPTO_CTR ?= y
+CFG_CRYPTO_CTS ?= y
+CFG_CRYPTO_XTS ?= y
+
+# Message authentication codes
+CFG_CRYPTO_HMAC ?= y
+CFG_CRYPTO_CMAC ?= y
+CFG_CRYPTO_CBC_MAC ?= y
+
+# Hashes
+CFG_CRYPTO_MD5 ?= y
+CFG_CRYPTO_SHA1 ?= y
+CFG_CRYPTO_SHA224 ?= y
+CFG_CRYPTO_SHA256 ?= y
+CFG_CRYPTO_SHA384 ?= y
+CFG_CRYPTO_SHA512 ?= y
+
+# Asymmetric ciphers
+CFG_CRYPTO_DSA ?= y
+CFG_CRYPTO_RSA ?= y
+CFG_CRYPTO_DH ?= y
+CFG_CRYPTO_ECC ?= y
+
+# Authenticated encryption
+CFG_CRYPTO_CCM ?= y
+CFG_CRYPTO_GCM ?= y
+
+endif
+
+ifeq ($(CFG_WITH_PAGER),y)
+ifneq ($(CFG_CRYPTO_SHA256),y)
+$(warning Warning: Enabling CFG_CRYPTO_SHA256 [required by CFG_WITH_PAGER])
+CFG_CRYPTO_SHA256:=y
+endif
+endif
+
+ifeq ($(CFG_CRYPTO_WITH_CE),y)
+ifeq ($(CFG_ARM32_core),y)
+CFG_CRYPTO_AES_ARM32_CE ?= $(CFG_CRYPTO_AES)
+CFG_CRYPTO_SHA1_ARM32_CE ?= $(CFG_CRYPTO_SHA1)
+CFG_CRYPTO_SHA256_ARM32_CE ?= $(CFG_CRYPTO_SHA256)
+endif
+ifeq ($(CFG_ARM64_core),y)
+CFG_CRYPTO_AES_ARM64_CE ?= $(CFG_CRYPTO_AES)
+CFG_CRYPTO_SHA1_ARM64_CE ?= $(CFG_CRYPTO_SHA1)
+CFG_CRYPTO_SHA256_ARM64_CE ?= $(CFG_CRYPTO_SHA256)
+endif
+endif
+
+# Cryptographic extensions can only be used safely when OP-TEE knows how to
+# preserve the VFP context
+ifeq ($(CFG_CRYPTO_SHA256_ARM32_CE),y)
+$(call force,CFG_WITH_VFP,y,required by CFG_CRYPTO_SHA256_ARM32_CE)
+endif
+ifeq ($(CFG_CRYPTO_SHA256_ARM64_CE),y)
+$(call force,CFG_WITH_VFP,y,required by CFG_CRYPTO_SHA256_ARM64_CE)
+endif
+ifeq ($(CFG_CRYPTO_SHA1_ARM32_CE),y)
+$(call force,CFG_WITH_VFP,y,required by CFG_CRYPTO_SHA1_ARM32_CE)
+endif
+ifeq ($(CFG_CRYPTO_SHA1_ARM64_CE),y)
+$(call force,CFG_WITH_VFP,y,required by CFG_CRYPTO_SHA1_ARM64_CE)
+endif
+ifeq ($(CFG_CRYPTO_AES_ARM64_CE),y)
+$(call force,CFG_WITH_VFP,y,required by CFG_CRYPTO_AES_ARM64_CE)
+endif
+
+cryp-enable-all-depends = $(call cfg-enable-all-depends,$(strip $(1)),$(foreach v,$(2),CFG_CRYPTO_$(v)))
+$(eval $(call cryp-enable-all-depends,CFG_REE_FS, AES ECB CTR HMAC SHA256 GCM))
+$(eval $(call cryp-enable-all-depends,CFG_SQL_FS, AES ECB CTR HMAC SHA256 GCM))
+$(eval $(call cryp-enable-all-depends,CFG_RPMB_FS, AES ECB CTR HMAC SHA256 GCM))
+
+# Dependency checks: warn and disable some features if dependencies are not met
+
+cryp-dep-one = $(call cfg-depends-one,CFG_CRYPTO_$(strip $(1)),$(patsubst %, CFG_CRYPTO_%,$(strip $(2))))
+cryp-dep-all = $(call cfg-depends-all,CFG_CRYPTO_$(strip $(1)),$(patsubst %, CFG_CRYPTO_%,$(strip $(2))))
+
+$(eval $(call cryp-dep-one, ECB, AES DES))
+$(eval $(call cryp-dep-one, CBC, AES DES))
+$(eval $(call cryp-dep-one, CTR, AES))
+# CTS is implemented with ECB and CBC
+$(eval $(call cryp-dep-all, CTS, AES ECB CBC))
+$(eval $(call cryp-dep-one, XTS, AES))
+$(eval $(call cryp-dep-one, HMAC, AES DES))
+$(eval $(call cryp-dep-one, HMAC, MD5 SHA1 SHA224 SHA256 SHA384 SHA512))
+$(eval $(call cryp-dep-one, CMAC, AES))
+$(eval $(call cryp-dep-one, CBC_MAC, AES DES))
+$(eval $(call cryp-dep-one, CCM, AES))
+$(eval $(call cryp-dep-one, GCM, AES))
+# If no AES cipher mode is left, disable AES
+$(eval $(call cryp-dep-one, AES, ECB CBC CTR CTS XTS))
+# If no DES cipher mode is left, disable DES
+$(eval $(call cryp-dep-one, DES, ECB CBC))
+
+# dsa_make_params() needs all three SHA-2 algorithms.
+# Disable DSA if any is missing.
+$(eval $(call cryp-dep-all, DSA, SHA256 SHA384 SHA512))
+
+cryp-one-enabled = $(call cfg-one-enabled,$(foreach v,$(1),CFG_CRYPTO_$(v)))
+cryp-all-enabled = $(call cfg-all-enabled,$(foreach v,$(1),CFG_CRYPTO_$(v)))
+
+_CFG_CRYPTO_WITH_ACIPHER := $(call cryp-one-enabled, RSA DSA DH ECC)
+_CFG_CRYPTO_WITH_AUTHENC := $(and $(filter y,$(CFG_CRYPTO_AES)), $(call cryp-one-enabled, CCM GCM))
+_CFG_CRYPTO_WITH_CIPHER := $(call cryp-one-enabled, AES DES)
+_CFG_CRYPTO_WITH_HASH := $(call cryp-one-enabled, MD5 SHA1 SHA224 SHA256 SHA384 SHA512)
+_CFG_CRYPTO_WITH_MAC := $(call cryp-one-enabled, HMAC CMAC CBC_MAC)
+_CFG_CRYPTO_WITH_CBC := $(call cryp-one-enabled, CBC CBC_MAC)
+_CFG_CRYPTO_WITH_ASN1 := $(call cryp-one-enabled, RSA DSA ECC)
+_CFG_CRYPTO_WITH_FORTUNA_PRNG := $(call cryp-all-enabled, AES SHA256)
+
+cppflags-lib-$(CFG_CRYPTO_SIZE_OPTIMIZATION) += -DLTC_SMALL_CODE
+cflags-lib-$(CFG_CRYPTO_SIZE_OPTIMIZATION) += -Os
+
+global-incdirs-y += include
+
+subdirs-y += src
diff --git a/core/sub.mk b/core/sub.mk
new file mode 100644
index 0000000..0732905
--- /dev/null
+++ b/core/sub.mk
@@ -0,0 +1,10 @@
+subdirs-y += kernel
+subdirs-y += tee
+subdirs-y += drivers
+
+gensrcs-y += ta_pub_key
+produce-ta_pub_key = ta_pub_key.c
+depends-ta_pub_key = $(TA_SIGN_KEY)
+recipe-ta_pub_key = scripts/pem_to_pub_c.py --prefix ta_pub_key \
+		--key $(TA_SIGN_KEY) --out $(sub-dir-out)/ta_pub_key.c
+cleanfiles += $(sub-dir-out)/ta_pub_key.c
diff --git a/core/tee/se/aid.c b/core/tee/se/aid.c
new file mode 100644
index 0000000..47c5c94
--- /dev/null
+++ b/core/tee/se/aid.c
@@ -0,0 +1,95 @@
+/*
+ * Copyright (c) 2014, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <assert.h>
+#include <kernel/panic.h>
+#include <stdlib.h>
+#include <string.h>
+#include <tee_api_types.h>
+#include <trace.h>
+
+#include <tee/se/aid.h>
+#include <tee/se/util.h>
+
+#include "aid_priv.h"
+
+TEE_Result tee_se_aid_create(const char *name, struct tee_se_aid **aid)
+{
+	size_t str_length = strlen(name);
+	size_t aid_length = str_length / 2;
+
+	assert(aid);
+	if (*aid)
+		panic("aid already allocated");
+
+	if (str_length < MIN_AID_LENGTH || str_length > MAX_AID_LENGTH)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	*aid = malloc(sizeof(struct tee_se_aid));
+	if (!(*aid))
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	hex_decode(name, str_length, (*aid)->aid);
+	(*aid)->length = aid_length;
+	(*aid)->refcnt = 1;
+	return TEE_SUCCESS;
+}
+
+TEE_Result tee_se_aid_create_from_buffer(uint8_t *id, size_t length,
+		struct tee_se_aid **aid)
+{
+	*aid = malloc(sizeof(struct tee_se_aid));
+	if (!(*aid))
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	memcpy((*aid)->aid, id, length);
+	(*aid)->length = length;
+	(*aid)->refcnt = 1;
+	return TEE_SUCCESS;
+}
+
+void tee_se_aid_acquire(struct tee_se_aid *aid)
+{
+	assert(aid);
+	aid->refcnt++;
+}
+
+int tee_se_aid_get_refcnt(struct tee_se_aid *aid)
+{
+	assert(aid);
+	return aid->refcnt;
+}
+
+void tee_se_aid_release(struct tee_se_aid *aid)
+{
+	assert(aid);
+	if (aid->refcnt <= 0)
+		panic();
+	aid->refcnt--;
+	if (!aid->refcnt)
+		free(aid);
+}
diff --git a/core/tee/se/aid_priv.h b/core/tee/se/aid_priv.h
new file mode 100644
index 0000000..555246f
--- /dev/null
+++ b/core/tee/se/aid_priv.h
@@ -0,0 +1,43 @@
+/*
+ * Copyright (c) 2014, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef TEE_SE_AID_PRIV_H
+#define TEE_SE_AID_PRIV_H
+
+struct tee_se_aid {
+	uint8_t aid[MAX_AID_LENGTH];
+	size_t length;
+	int refcnt;
+};
+
+int tee_se_aid_get_refcnt(struct tee_se_aid *aid);
+
+TEE_Result tee_se_aid_create(const char *name, struct tee_se_aid **aid);
+
+void tee_se_aid_acquire(struct tee_se_aid *aid);
+
+#endif
diff --git a/core/tee/se/apdu.c b/core/tee/se/apdu.c
new file mode 100644
index 0000000..ddef30d
--- /dev/null
+++ b/core/tee/se/apdu.c
@@ -0,0 +1,183 @@
+/*
+ * Copyright (c) 2014, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <assert.h>
+#include <stdlib.h>
+#include <string.h>
+#include <tee_api_types.h>
+#include <trace.h>
+
+#include <tee/se/apdu.h>
+#include <tee/se/util.h>
+
+#include "apdu_priv.h"
+
+/*
+ * APDU format, [..] means optional fields
+ *
+ * CMD_APDU: CLA, INS, P1, P2, [LC, DATA, LE]
+ * RESP_APDU: [DATA], SW1, SW2
+ *
+ */
+#define CMD_APDU_SIZE(lc) ((lc) + 4)
+#define RESP_APDU_SIZE(le) ((le) + 2)
+
+struct cmd_apdu *alloc_cmd_apdu(uint8_t cla, uint8_t ins, uint8_t p1,
+		uint8_t p2, uint8_t lc, uint8_t le, uint8_t *data)
+{
+	size_t apdu_length = CMD_APDU_SIZE(lc);
+	size_t total_length;
+	struct cmd_apdu *apdu;
+	uint8_t *buf;
+
+	/*
+	 * check if we need to reserve space for LC/LE
+	 * (both fields are optional)
+	 */
+	if (lc)
+		apdu_length++;
+	if (le)
+		apdu_length++;
+
+	total_length = sizeof(struct cmd_apdu) + apdu_length;
+	apdu = malloc(total_length);
+
+	if (!apdu)
+		return NULL;
+
+	apdu->base.length = apdu_length;
+	apdu->base.data_buf = (uint8_t *)(apdu + 1);
+	apdu->base.refcnt = 1;
+
+	buf = apdu->base.data_buf;
+	buf[CLA] = cla;
+	buf[INS] = ins;
+	buf[P1] = p1;
+	buf[P2] = p2;
+	if (lc)
+		buf[LC] = lc;
+	if (data != NULL)
+		memmove(&buf[CDATA], data, lc);
+	if (le)
+		buf[CDATA + lc + OFF_LE] = le;
+
+	return apdu;
+}
+
+struct cmd_apdu *alloc_cmd_apdu_from_buf(uint8_t *buf, size_t length)
+{
+	struct cmd_apdu *apdu = malloc(sizeof(struct cmd_apdu));
+
+	if (!apdu)
+		return NULL;
+	apdu->base.length = length;
+	apdu->base.data_buf = buf;
+	apdu->base.refcnt = 1;
+	return apdu;
+}
+
+struct resp_apdu *alloc_resp_apdu(uint8_t le)
+{
+	size_t total_length = sizeof(struct resp_apdu) + RESP_APDU_SIZE(le);
+	struct resp_apdu *apdu;
+
+	apdu = malloc(total_length);
+
+	if (!apdu)
+		return NULL;
+
+	apdu->base.length = RESP_APDU_SIZE(le);
+	apdu->base.data_buf = (uint8_t *)(apdu + 1);
+	apdu->base.refcnt = 1;
+
+	return apdu;
+}
+
+uint8_t *resp_apdu_get_data(struct resp_apdu *apdu)
+{
+	assert(apdu);
+	return apdu->resp_data;
+}
+
+size_t resp_apdu_get_data_len(struct resp_apdu *apdu)
+{
+	assert(apdu);
+	return apdu->resp_data_len;
+}
+
+uint8_t resp_apdu_get_sw1(struct resp_apdu *apdu)
+{
+	assert(apdu);
+	return apdu->sw1;
+}
+
+uint8_t resp_apdu_get_sw2(struct resp_apdu *apdu)
+{
+	assert(apdu);
+	return apdu->sw2;
+}
+
+uint8_t *apdu_get_data(struct apdu_base *apdu)
+{
+	assert(apdu);
+	return apdu->data_buf;
+}
+size_t apdu_get_length(struct apdu_base *apdu)
+{
+	assert(apdu);
+	return apdu->length;
+}
+int apdu_get_refcnt(struct apdu_base *apdu)
+{
+	assert(apdu);
+	return apdu->refcnt;
+}
+void apdu_acquire(struct apdu_base *apdu)
+{
+	assert(apdu);
+	apdu->refcnt++;
+}
+void apdu_release(struct apdu_base *apdu)
+{
+	assert(apdu);
+	apdu->refcnt--;
+	if (apdu->refcnt == 0)
+		free(apdu);
+}
+
+void parse_resp_apdu(struct resp_apdu *apdu)
+{
+	uint8_t *buf = apdu->base.data_buf;
+	/* resp data length =  resp buf length - SW1 - SW2 */
+	apdu->resp_data_len = apdu->base.length - 2;
+	if (apdu->resp_data_len > 0)
+		apdu->resp_data = &buf[RDATA];
+	else
+		apdu->resp_data = NULL;
+	apdu->sw1 = buf[RDATA + apdu->resp_data_len + OFF_SW1];
+	apdu->sw2 = buf[RDATA + apdu->resp_data_len + OFF_SW2];
+}
diff --git a/core/tee/se/apdu_priv.h b/core/tee/se/apdu_priv.h
new file mode 100644
index 0000000..3d30050
--- /dev/null
+++ b/core/tee/se/apdu_priv.h
@@ -0,0 +1,69 @@
+/*
+ * Copyright (c) 2014, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef TEE_SE_APDU_PRIV_H
+#define TEE_SE_APDU_PRIV_H
+
+enum {
+	/* command APDU */
+	CLA = 0,
+	INS = 1,
+	P1 = 2,
+	P2 = 3,
+	LC = 4,
+	CDATA = 5,
+	OFF_LE = 0,
+
+	/* response APDU */
+	RDATA = 0,
+	OFF_SW1 = 0,
+	OFF_SW2 = 1,
+};
+
+struct apdu_base {
+	uint8_t *data_buf;
+	size_t length;
+	int refcnt;
+};
+
+struct cmd_apdu {
+	struct apdu_base base;
+};
+
+struct resp_apdu {
+	struct apdu_base base;
+	uint8_t sw1;
+	uint8_t sw2;
+	uint8_t *resp_data;
+	size_t resp_data_len;
+};
+
+void parse_resp_apdu(struct resp_apdu *apdu);
+
+int apdu_get_refcnt(struct apdu_base *apdu);
+
+#endif
diff --git a/core/tee/se/channel.c b/core/tee/se/channel.c
new file mode 100644
index 0000000..5df7084
--- /dev/null
+++ b/core/tee/se/channel.c
@@ -0,0 +1,142 @@
+/*
+ * Copyright (c) 2014, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <assert.h>
+#include <tee_api_types.h>
+#include <trace.h>
+
+#include <tee/se/session.h>
+#include <tee/se/channel.h>
+#include <tee/se/iso7816.h>
+#include <tee/se/aid.h>
+#include <tee/se/apdu.h>
+
+#include <stdlib.h>
+#include <string.h>
+
+#include "aid_priv.h"
+#include "channel_priv.h"
+
+struct tee_se_channel *tee_se_channel_alloc(struct tee_se_session *s,
+		int channel_id)
+{
+	struct tee_se_channel *c;
+
+	c = malloc(sizeof(struct tee_se_channel));
+	if (c) {
+		c->session = s;
+		c->channel_id = channel_id;
+		c->aid = NULL;
+		c->select_resp = NULL;
+	}
+	return c;
+}
+
+void tee_se_channel_free(struct tee_se_channel *c)
+{
+	assert(c);
+	if (c->aid)
+		tee_se_aid_release(c->aid);
+	if (c->select_resp)
+		apdu_release(to_apdu_base(c->select_resp));
+}
+
+struct tee_se_session *tee_se_channel_get_session(struct tee_se_channel *c)
+{
+	assert(c);
+	return c->session;
+}
+
+int tee_se_channel_get_id(struct tee_se_channel *c)
+{
+	assert(c);
+	return c->channel_id;
+}
+
+void tee_se_channel_set_select_response(struct tee_se_channel *c,
+		struct resp_apdu *resp)
+{
+	assert(c);
+
+	if (c->select_resp)
+		apdu_release(to_apdu_base(c->select_resp));
+	apdu_acquire(to_apdu_base(resp));
+	c->select_resp = resp;
+}
+
+TEE_Result tee_se_channel_get_select_response(struct tee_se_channel *c,
+		struct resp_apdu **resp)
+{
+	assert(c && resp);
+
+	if (c->select_resp) {
+		*resp = c->select_resp;
+		return TEE_SUCCESS;
+	} else {
+		return TEE_ERROR_NO_DATA;
+	}
+}
+
+void tee_se_channel_set_aid(struct tee_se_channel *c,
+		struct tee_se_aid *aid)
+{
+	assert(c);
+	if (c->aid)
+		tee_se_aid_release(c->aid);
+	tee_se_aid_acquire(aid);
+	c->aid = aid;
+}
+
+
+TEE_Result tee_se_channel_select(struct tee_se_channel *c,
+		struct tee_se_aid *aid)
+{
+	assert(c);
+	return iso7816_select(c, aid);
+}
+
+TEE_Result tee_se_channel_select_next(struct tee_se_channel *c)
+{
+	assert(c);
+	return iso7816_select_next(c);
+}
+
+TEE_Result tee_se_channel_transmit(struct tee_se_channel *c,
+		struct cmd_apdu *cmd_apdu, struct resp_apdu *resp_apdu)
+{
+	struct tee_se_session *s;
+	uint8_t *cmd_buf;
+	int cla_channel;
+
+	assert(c && cmd_apdu && resp_apdu);
+
+	s = c->session;
+	cla_channel = iso7816_get_cla_channel(c->channel_id);
+	cmd_buf = apdu_get_data(to_apdu_base(cmd_apdu));
+	cmd_buf[ISO7816_CLA_OFFSET] = ISO7816_CLA | cla_channel;
+	return tee_se_session_transmit(s, cmd_apdu, resp_apdu);
+}
diff --git a/core/tee/se/channel_priv.h b/core/tee/se/channel_priv.h
new file mode 100644
index 0000000..984a365
--- /dev/null
+++ b/core/tee/se/channel_priv.h
@@ -0,0 +1,47 @@
+/*
+ * Copyright (c) 2014, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef TEE_SE_CHANNEL_PRIV_H
+#define TEE_SE_CHANNEL_PRIV_H
+
+struct tee_se_aid;
+
+struct tee_se_channel {
+	int channel_id;
+	struct tee_se_session *session;
+	struct tee_se_aid *aid;
+	struct resp_apdu *select_resp;
+
+	TAILQ_ENTRY(tee_se_channel) link;
+};
+
+struct tee_se_channel *tee_se_channel_alloc(struct tee_se_session *s,
+		int channel_id);
+
+void tee_se_channel_free(struct tee_se_channel *c);
+
+#endif
diff --git a/core/tee/se/iso7816.c b/core/tee/se/iso7816.c
new file mode 100644
index 0000000..329c425
--- /dev/null
+++ b/core/tee/se/iso7816.c
@@ -0,0 +1,205 @@
+/*
+ * Copyright (c) 2014, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <assert.h>
+#include <kernel/panic.h>
+#include <malloc.h>
+#include <stdlib.h>
+#include <string.h>
+#include <tee_api_types.h>
+#include <tee/se/reader.h>
+#include <tee/se/session.h>
+#include <tee/se/iso7816.h>
+#include <tee/se/aid.h>
+#include <tee/se/apdu.h>
+#include <tee/se/channel.h>
+#include <tee/se/util.h>
+#include <tee/se/reader/interface.h>
+#include <trace.h>
+
+#include "session_priv.h"
+#include "aid_priv.h"
+#include "apdu_priv.h"
+
+TEE_Result iso7816_exchange_apdu(struct tee_se_reader_proxy *proxy,
+		struct cmd_apdu *cmd, struct resp_apdu *resp)
+{
+	TEE_Result ret;
+
+	assert(cmd && resp);
+	ret = tee_se_reader_transmit(proxy,
+			cmd->base.data_buf, cmd->base.length,
+			resp->base.data_buf, &resp->base.length);
+
+	if (ret == TEE_SUCCESS)
+		parse_resp_apdu(resp);
+
+	return ret;
+}
+
+int iso7816_get_cla_channel(int channel_id)
+{
+	int cla_channel;
+	/*
+	 * From GP Card Spec,
+	 * the logical channel number 0~3 should have CLA: 0x00 ~ 0x03,
+	 * for channel number 4~19 should have CLA: 0x40 ~ 0x4f
+	 */
+	if (channel_id < 4)
+		cla_channel = channel_id;
+	else
+		cla_channel = 0x40 | (channel_id - 4);
+
+	return cla_channel;
+}
+
+static TEE_Result internal_select(struct tee_se_channel *c,
+		struct tee_se_aid *aid, int select_ops)
+{
+	struct cmd_apdu *cmd;
+	struct resp_apdu *resp;
+	struct tee_se_session *s;
+	TEE_Result ret;
+	TEE_SEReaderProperties prop;
+	size_t rx_buf_len = 0;
+	int channel_id;
+	uint8_t cla_channel;
+
+	assert(c);
+
+	s = tee_se_channel_get_session(c);
+	channel_id = tee_se_channel_get_id(c);
+
+	if (channel_id >= MAX_LOGICAL_CHANNEL)
+		panic("invalid channel id");
+
+	cla_channel = iso7816_get_cla_channel(channel_id);
+	if (select_ops == FIRST_OR_ONLY_OCCURRENCE) {
+		assert(aid);
+		cmd = alloc_cmd_apdu(ISO7816_CLA | cla_channel,
+				SELECT_CMD, SELECT_BY_AID,
+				select_ops, aid->length,
+				rx_buf_len, aid->aid);
+	} else {
+		cmd = alloc_cmd_apdu(ISO7816_CLA | cla_channel,
+				SELECT_CMD, SELECT_BY_AID,
+				select_ops, 0, rx_buf_len, NULL);
+	}
+
+	resp = alloc_resp_apdu(rx_buf_len);
+
+	ret = tee_se_session_transmit(s, cmd, resp);
+	if (ret != TEE_SUCCESS) {
+		EMSG("exchange apdu failed: %d", ret);
+		return ret;
+	}
+
+	tee_se_reader_get_properties(s->reader_proxy, &prop);
+	if (prop.selectResponseEnable)
+		tee_se_channel_set_select_response(c, resp);
+	if (aid)
+		tee_se_channel_set_aid(c, aid);
+
+	if (resp->sw1 == CMD_OK_SW1 && resp->sw2 == CMD_OK_SW2) {
+		ret = TEE_SUCCESS;
+	} else {
+		EMSG("operation failed, sw1:%02X, sw2:%02X",
+				resp->sw1, resp->sw2);
+		if (resp->sw1 == 0x6A && resp->sw2 == 0x83)
+			ret = TEE_ERROR_ITEM_NOT_FOUND;
+		else
+			ret = TEE_ERROR_NOT_SUPPORTED;
+	}
+
+	apdu_release(to_apdu_base(cmd));
+	apdu_release(to_apdu_base(resp));
+
+	return ret;
+}
+
+static TEE_Result internal_manage_channel(struct tee_se_session *s,
+		bool open_ops, int *channel_id)
+{
+	struct cmd_apdu *cmd;
+	struct resp_apdu *resp;
+	TEE_Result ret;
+	size_t tx_buf_len = 0, rx_buf_len = 1;
+
+	uint8_t open_flag = (open_ops) ? OPEN_CHANNEL : CLOSE_CHANNEL;
+	uint8_t channel_flag =
+		(open_flag == OPEN_CHANNEL) ? OPEN_NEXT_AVAILABLE : *channel_id;
+
+	assert(s);
+
+	cmd = alloc_cmd_apdu(ISO7816_CLA, MANAGE_CHANNEL_CMD, open_flag,
+			channel_flag, tx_buf_len, rx_buf_len, NULL);
+
+	resp = alloc_resp_apdu(rx_buf_len);
+
+	ret = tee_se_session_transmit(s, cmd, resp);
+	if (ret != TEE_SUCCESS) {
+		EMSG("exchange apdu failed: %d", ret);
+		return ret;
+	}
+
+	if (resp->sw1 == CMD_OK_SW1 && resp->sw2 == CMD_OK_SW2) {
+		if (open_ops)
+			*channel_id = resp->base.data_buf[0];
+		ret = TEE_SUCCESS;
+	} else {
+		EMSG("operation failed, sw1:%02X, sw2:%02X",
+				resp->sw1, resp->sw2);
+		ret = TEE_ERROR_NOT_SUPPORTED;
+	}
+
+	apdu_release(to_apdu_base(cmd));
+	apdu_release(to_apdu_base(resp));
+
+	return ret;
+}
+
+TEE_Result iso7816_open_available_logical_channel(struct tee_se_session *s,
+		int *channel_id)
+{
+	return internal_manage_channel(s, true, channel_id);
+}
+
+TEE_Result iso7816_close_logical_channel(struct tee_se_session *s,
+		int channel_id)
+{
+	return internal_manage_channel(s, false, &channel_id);
+}
+
+TEE_Result iso7816_select(struct tee_se_channel *c, struct tee_se_aid *aid)
+{
+	return internal_select(c, aid, FIRST_OR_ONLY_OCCURRENCE);
+}
+
+TEE_Result iso7816_select_next(struct tee_se_channel *c)
+{
+	return internal_select(c, NULL, NEXT_OCCURRENCE);
+}
diff --git a/core/tee/se/manager.c b/core/tee/se/manager.c
new file mode 100644
index 0000000..30b35f6
--- /dev/null
+++ b/core/tee/se/manager.c
@@ -0,0 +1,153 @@
+/*
+ * Copyright (c) 2014, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <initcall.h>
+#include <trace.h>
+#include <kernel/mutex.h>
+#include <tee/se/manager.h>
+#include <tee/se/session.h>
+#include <tee/se/reader.h>
+#include <tee/se/reader/interface.h>
+
+#include <stdlib.h>
+#include <sys/queue.h>
+
+#include "reader_priv.h"
+#include "session_priv.h"
+
+TAILQ_HEAD(reader_proxy_head, tee_se_reader_proxy);
+
+struct tee_se_manager_ctx {
+	/* number of readers registered */
+	size_t reader_count;
+	/* mutex to pretect the reader proxy list */
+	struct mutex mutex;
+	/* list of registered readers */
+	struct reader_proxy_head reader_proxies;
+};
+static struct tee_se_manager_ctx se_manager_ctx;
+
+TEE_Result tee_se_manager_register_reader(struct tee_se_reader *r)
+{
+	struct tee_se_manager_ctx *ctx = &se_manager_ctx;
+	struct tee_se_reader_proxy *proxy =
+		malloc(sizeof(struct tee_se_reader_proxy));
+	if (!proxy)
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	proxy->reader = r;
+	proxy->refcnt = 0;
+	proxy->basic_channel_locked = false;
+	mutex_init(&proxy->mutex);
+
+	mutex_lock(&ctx->mutex);
+	TAILQ_INSERT_TAIL(&ctx->reader_proxies, proxy, link);
+	ctx->reader_count++;
+	mutex_unlock(&ctx->mutex);
+
+	return TEE_SUCCESS;
+}
+
+TEE_Result tee_se_manager_unregister_reader(struct tee_se_reader *r)
+{
+	struct tee_se_manager_ctx *ctx = &se_manager_ctx;
+	struct tee_se_reader_proxy *proxy;
+
+	mutex_lock(&ctx->mutex);
+	TAILQ_FOREACH(proxy, &ctx->reader_proxies, link)
+	{
+		if (proxy->reader == r)
+			TAILQ_REMOVE(&ctx->reader_proxies, proxy, link);
+		free(proxy);
+	}
+	ctx->reader_count--;
+	mutex_unlock(&ctx->mutex);
+
+	return TEE_SUCCESS;
+}
+
+size_t tee_se_manager_get_reader_count(void)
+{
+	struct tee_se_manager_ctx *ctx = &se_manager_ctx;
+
+	return ctx->reader_count;
+}
+
+TEE_Result tee_se_manager_get_readers(
+		struct tee_se_reader_proxy **proxy_list,
+		size_t *proxy_list_size)
+{
+	struct tee_se_manager_ctx *ctx = &se_manager_ctx;
+	struct tee_se_reader_proxy *proxy;
+	size_t i = 0;
+
+	if (TAILQ_EMPTY(&ctx->reader_proxies))
+		return TEE_ERROR_ITEM_NOT_FOUND;
+
+	TAILQ_FOREACH(proxy, &ctx->reader_proxies, link) {
+		if (i >= *proxy_list_size)
+			return TEE_ERROR_SHORT_BUFFER;
+
+		proxy_list[i] = proxy;
+		i++;
+	}
+	*proxy_list_size = i;
+
+	return TEE_SUCCESS;
+}
+
+bool tee_se_manager_is_reader_proxy_valid(
+		struct tee_se_reader_proxy *proxy)
+{
+	struct tee_se_manager_ctx *ctx = &se_manager_ctx;
+	struct tee_se_reader_proxy *h;
+
+	TAILQ_FOREACH(h, &ctx->reader_proxies, link) {
+		if (h == proxy)
+			return true;
+	}
+
+	return false;
+}
+
+static void context_init(struct tee_se_manager_ctx *ctx)
+{
+	TAILQ_INIT(&ctx->reader_proxies);
+	mutex_init(&ctx->mutex);
+	ctx->reader_count = 0;
+}
+
+static TEE_Result tee_se_manager_init(void)
+{
+	struct tee_se_manager_ctx *ctx = &se_manager_ctx;
+
+	context_init(ctx);
+
+	return TEE_SUCCESS;
+}
+
+service_init(tee_se_manager_init);
diff --git a/core/tee/se/reader.c b/core/tee/se/reader.c
new file mode 100644
index 0000000..3f0aa98
--- /dev/null
+++ b/core/tee/se/reader.c
@@ -0,0 +1,218 @@
+/*
+ * Copyright (c) 2014, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <assert.h>
+#include <kernel/mutex.h>
+#include <kernel/panic.h>
+#include <string.h>
+#include <tee_api_types.h>
+#include <trace.h>
+
+#include <tee/se/reader.h>
+#include <tee/se/reader/interface.h>
+
+#include "reader_priv.h"
+#include "session_priv.h"
+
+TEE_Result tee_se_reader_check_state(struct tee_se_reader_proxy *proxy)
+{
+	struct tee_se_reader *r;
+
+	if (proxy->refcnt == 0)
+		return TEE_ERROR_BAD_STATE;
+
+	r = proxy->reader;
+	if (r->ops->get_state) {
+		enum tee_se_reader_state state;
+
+		mutex_lock(&proxy->mutex);
+		state = r->ops->get_state(r);
+		mutex_unlock(&proxy->mutex);
+
+		if (state != READER_STATE_SE_INSERTED)
+			return TEE_ERROR_COMMUNICATION;
+	}
+
+	return TEE_SUCCESS;
+}
+
+TEE_Result tee_se_reader_get_name(struct tee_se_reader_proxy *proxy,
+		char **reader_name, size_t *reader_name_len)
+{
+	size_t name_len;
+
+	assert(proxy && proxy->reader);
+	name_len = strlen(proxy->reader->name);
+	*reader_name = proxy->reader->name;
+	*reader_name_len = name_len;
+
+	return TEE_SUCCESS;
+}
+
+void tee_se_reader_get_properties(struct tee_se_reader_proxy *proxy,
+		TEE_SEReaderProperties *prop)
+{
+	assert(proxy && proxy->reader);
+	*prop = proxy->reader->prop;
+}
+
+int tee_se_reader_get_refcnt(struct tee_se_reader_proxy *proxy)
+{
+	assert(proxy && proxy->reader);
+	return proxy->refcnt;
+}
+
+TEE_Result tee_se_reader_attach(struct tee_se_reader_proxy *proxy)
+{
+	TEE_Result ret;
+
+	mutex_lock(&proxy->mutex);
+	if (proxy->refcnt == 0) {
+		struct tee_se_reader *r = proxy->reader;
+
+		if (r->ops->open) {
+			ret = r->ops->open(r);
+			if (ret != TEE_SUCCESS) {
+				mutex_unlock(&proxy->mutex);
+				return ret;
+			}
+		}
+	}
+	proxy->refcnt++;
+	mutex_unlock(&proxy->mutex);
+	return TEE_SUCCESS;
+}
+
+void tee_se_reader_detach(struct tee_se_reader_proxy *proxy)
+{
+	if (proxy->refcnt <= 0)
+		panic("invalid refcnf");
+
+	mutex_lock(&proxy->mutex);
+	proxy->refcnt--;
+	if (proxy->refcnt == 0) {
+		struct tee_se_reader *r = proxy->reader;
+
+		if (r->ops->close)
+			r->ops->close(r);
+	}
+	mutex_unlock(&proxy->mutex);
+
+}
+
+TEE_Result tee_se_reader_transmit(struct tee_se_reader_proxy *proxy,
+		uint8_t *tx_buf, size_t tx_buf_len,
+		uint8_t *rx_buf, size_t *rx_buf_len)
+{
+	struct tee_se_reader *r;
+	TEE_Result ret;
+
+	assert(proxy && proxy->reader);
+	ret = tee_se_reader_check_state(proxy);
+	if (ret != TEE_SUCCESS)
+		return ret;
+
+	mutex_lock(&proxy->mutex);
+	r = proxy->reader;
+
+	assert(r->ops->transmit);
+	ret = r->ops->transmit(r, tx_buf, tx_buf_len, rx_buf, rx_buf_len);
+
+	mutex_unlock(&proxy->mutex);
+
+	return ret;
+}
+
+void tee_se_reader_lock_basic_channel(struct tee_se_reader_proxy *proxy)
+{
+	assert(proxy);
+
+	mutex_lock(&proxy->mutex);
+	proxy->basic_channel_locked = true;
+	mutex_unlock(&proxy->mutex);
+}
+
+void tee_se_reader_unlock_basic_channel(struct tee_se_reader_proxy *proxy)
+{
+	assert(proxy);
+
+	mutex_lock(&proxy->mutex);
+	proxy->basic_channel_locked = false;
+	mutex_unlock(&proxy->mutex);
+}
+
+bool tee_se_reader_is_basic_channel_locked(struct tee_se_reader_proxy *proxy)
+{
+	assert(proxy);
+	return proxy->basic_channel_locked;
+}
+
+TEE_Result tee_se_reader_get_atr(struct tee_se_reader_proxy *proxy,
+		uint8_t **atr, size_t *atr_len)
+{
+	TEE_Result ret;
+	struct tee_se_reader *r;
+
+	assert(proxy && atr && atr_len);
+	ret = tee_se_reader_check_state(proxy);
+	if (ret != TEE_SUCCESS)
+		return ret;
+
+	mutex_lock(&proxy->mutex);
+	r = proxy->reader;
+
+	assert(r->ops->get_atr);
+	ret = r->ops->get_atr(r, atr, atr_len);
+
+	mutex_unlock(&proxy->mutex);
+	return ret;
+}
+
+TEE_Result tee_se_reader_open_session(struct tee_se_reader_proxy *proxy,
+		struct tee_se_session **session)
+{
+	TEE_Result ret;
+	struct tee_se_session *s;
+
+	assert(session && !*session);
+	assert(proxy && proxy->reader);
+
+	s = tee_se_session_alloc(proxy);
+	if (!s)
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	ret = tee_se_reader_attach(proxy);
+	if (ret != TEE_SUCCESS)
+		goto err_free_session;
+
+	*session = s;
+
+	return TEE_SUCCESS;
+err_free_session:
+	tee_se_session_free(s);
+	return ret;
+}
diff --git a/core/tee/se/reader/passthru_reader/driver.c b/core/tee/se/reader/passthru_reader/driver.c
new file mode 100644
index 0000000..e3c550e
--- /dev/null
+++ b/core/tee/se/reader/passthru_reader/driver.c
@@ -0,0 +1,119 @@
+/*
+ * Copyright (c) 2014, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <platform_config.h>
+#include <io.h>
+#include <initcall.h>
+#include <tee/se/reader/interface.h>
+#include <mm/core_memprot.h>
+
+#include <trace.h>
+
+#include <stdlib.h>
+
+#include "pcsc.h"
+#include "reader.h"
+
+struct pcsc_context {
+	uint32_t mmio_base;
+	uint8_t num_readers;
+	struct pcsc_reader *readers;
+};
+static struct pcsc_context pcsc_context;
+
+register_phys_mem(MEM_AREA_IO_SEC, PCSC_BASE, 0x1000);
+
+static uint32_t pcsc_read_reg(struct pcsc_context *ctx, uint8_t offset)
+{
+	return read32(ctx->mmio_base + offset);
+}
+
+static void pcsc_write_reg(struct pcsc_context *ctx, uint8_t offset,
+		uint32_t value) __attribute__((unused));
+static void pcsc_write_reg(struct pcsc_context *ctx, uint8_t offset,
+		uint32_t value)
+{
+	write32(ctx->mmio_base + offset, value);
+}
+
+static TEE_Result populate_readers(struct pcsc_context *ctx)
+{
+	int i;
+	uint32_t reader_mmio_base = ctx->mmio_base + PCSC_REG_MAX;
+	TEE_Result ret;
+
+	ctx->readers = malloc(sizeof(struct pcsc_reader) * ctx->num_readers);
+	if (!ctx->readers)
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	for (i = 0; i < ctx->num_readers; i++) {
+		uint32_t mmio_base =
+			reader_mmio_base + (i * PCSC_REG_READER_MAX);
+		struct pcsc_reader *r = &ctx->readers[i];
+
+		init_reader(r, i, mmio_base);
+		ret = tee_se_manager_register_reader(&r->se_reader);
+		if (ret != TEE_SUCCESS)
+			goto err_rollback;
+	}
+
+	return TEE_SUCCESS;
+
+err_rollback:
+	i--;
+	while (i) {
+		tee_se_manager_unregister_reader(&ctx->readers[i].se_reader);
+		i--;
+	}
+	free(ctx->readers);
+	return ret;
+}
+
+static void context_init(struct pcsc_context *ctx)
+{
+	ctx->mmio_base = (vaddr_t)phys_to_virt(PCSC_BASE, MEM_AREA_IO_SEC);
+	if (ctx->mmio_base) {
+		ctx->num_readers = pcsc_read_reg(ctx, PCSC_REG_NUM_READERS);
+		DMSG("%d reader detected", ctx->num_readers);
+	}
+}
+
+static TEE_Result pcsc_passthru_reader_init(void)
+{
+	TEE_Result ret;
+	struct pcsc_context *ctx = &pcsc_context;
+
+	context_init(ctx);
+
+	ret = populate_readers(ctx);
+	if (ret != TEE_SUCCESS)
+		return ret;
+
+	return TEE_SUCCESS;
+}
+
+driver_init(pcsc_passthru_reader_init);
diff --git a/core/tee/se/reader/passthru_reader/pcsc.h b/core/tee/se/reader/passthru_reader/pcsc.h
new file mode 100644
index 0000000..6f0636b
--- /dev/null
+++ b/core/tee/se/reader/passthru_reader/pcsc.h
@@ -0,0 +1,83 @@
+/*
+ * Copyright (c) 2014, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef PCSC_H
+#define PCSC_H
+
+/* common control registers */
+#define PCSC_REG_NUM_READERS    0x0
+#define PCSC_REG_IRQ_STATUS     0x4
+#define     PCSC_IRQ_STATE_CHANGE   0x1
+#define PCSC_REG_MAX            0x8
+
+/* per-reader control/status registers */
+#define PCSC_REG_READER_CONTROL 0x0
+/* preferred protocol, directly mapped to pcsclite */
+#define     PCSC_READER_CTL_PROTOCOL_T0     0x0001
+#define     PCSC_READER_CTL_PROTOCOL_T1     0x0002
+#define     PCSC_READER_CTL_PROTOCOL_T15    0x0004
+#define     PCSC_READER_CTL_PROTOCOL_RAW    0x0008
+#define     PCSC_READER_CTL_PROTOCOL_MASK   0x000f
+/* shared mode, directly mapped to pcsclite */
+#define     PCSC_READER_CTL_SHARE_MASK      0x0030
+#define     PCSC_READER_CTL_SHARE_SHIFT     4
+#define     PCSC_READER_CTL_SHARE_EXCLUSIVE 0x0010
+#define     PCSC_READER_CTL_SHARE_SHARED    0x0020
+#define     PCSC_READER_CTL_SHARE_DIRECT    0x0030
+/* disposition mode, directly mapped to pcsclite */
+#define     PCSC_READER_CTL_DISPOSITION_MASK    0x0300
+#define     PCSC_READER_CTL_DISPOSITION_SHIFT   8
+#define     PCSC_READER_CTL_DISPOSITION_LEAVE_CARD  0x0000
+#define     PCSC_READER_CTL_DISPOSITION_RESET_CARD  0x0100
+#define     PCSC_READER_CTL_DISPOSITION_UNPOWER_CARD 0x0200
+#define     PCSC_READER_CTL_DISPOSITION_EJECT_CARD  0x0300
+/* reader commands */
+#define     PCSC_READER_CTL_CONNECT         0x1000
+#define     PCSC_READER_CTL_DISCONNECT      0x2000
+#define     PCSC_READER_CTL_READ_ATR        0x4000
+#define     PCSC_READER_CTL_TRANSMIT        0x8000
+#define PCSC_REG_READER_STATE   0x4
+/* reader state, directly mapped to pcsclite */
+#define     PCSC_READER_STATE_IGNORE    0x0001
+#define     PCSC_READER_STATE_CHANGED   0x0002
+#define     PCSC_READER_STATE_UNKNOWN   0x0004
+#define     PCSC_READER_STATE_UNAVAILABLE   0x0008
+#define     PCSC_READER_STATE_EMPTY     0x0010
+#define     PCSC_READER_STATE_PRESENT   0x0020
+#define     PCSC_READER_STATE_ATRMATCH  0x0040
+#define     PCSC_READER_STATE_EXCLUSIVE 0x0080
+#define     PCSC_READER_STATE_INUSE     0x0100
+#define     PCSC_READER_STATE_MUTE      0x0200
+#define     PCSC_READER_STATE_UNPOWERED 0x0400
+#define PCSC_REG_READER_TX_ADDR    0x8
+#define PCSC_REG_READER_TX_SIZE    0xc
+#define PCSC_REG_READER_RX_ADDR    0x10
+#define PCSC_REG_READER_RX_SIZE    0x14
+#define PCSC_REG_READER_ATR_LEN    0x18
+#define PCSC_REG_READER_MAX     0x1c
+
+#endif
diff --git a/core/tee/se/reader/passthru_reader/reader.c b/core/tee/se/reader/passthru_reader/reader.c
new file mode 100644
index 0000000..4cb13e1
--- /dev/null
+++ b/core/tee/se/reader/passthru_reader/reader.c
@@ -0,0 +1,248 @@
+/*
+ * Copyright (c) 2014, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <io.h>
+#include <kernel/panic.h>
+#include <mm/core_memprot.h>
+#include <stdio.h>
+#include <trace.h>
+
+#include <tee/se/util.h>
+#include <tee/se/reader/interface.h>
+
+#include "pcsc.h"
+#include "reader.h"
+
+static void pcsc_reader_dump(struct pcsc_reader *r)
+{
+	DMSG("[%s]:", r->se_reader.name);
+	if (r->state & PCSC_READER_STATE_IGNORE)
+		DMSG("  Ignore this reader");
+
+	if (r->state & PCSC_READER_STATE_UNKNOWN)
+		DMSG("  Reader unknown");
+
+	if (r->state & PCSC_READER_STATE_UNAVAILABLE)
+		DMSG("  Status unavailable");
+
+	if (r->state & PCSC_READER_STATE_EMPTY)
+		DMSG("  Card removed");
+
+	if (r->state & PCSC_READER_STATE_PRESENT)
+		DMSG("  Card inserted");
+
+	if (r->state & PCSC_READER_STATE_ATRMATCH)
+		DMSG("  ATR matches card");
+
+	if (r->state & PCSC_READER_STATE_EXCLUSIVE)
+		DMSG("  Exclusive Mode");
+
+	if (r->state & PCSC_READER_STATE_INUSE)
+		DMSG("  Shared Mode");
+
+	if (r->state & PCSC_READER_STATE_MUTE)
+		DMSG("  Unresponsive card");
+
+	if (r->state & PCSC_READER_STATE_UNPOWERED)
+		DMSG("  Reader Unpowered,");
+
+	if (r->state & PCSC_READER_STATE_PRESENT)
+		DMSG("Card Connected: [%s]",
+				r->connected ? "Yes" : "No");
+
+	if (r->connected) {
+		char dumpbuf[DUMP_BUF_MAX], *buf = dumpbuf;
+		size_t remain = DUMP_BUF_MAX;
+
+		buf = print_buf(buf, &remain, "ATR: ");
+		dump_hex(buf, &remain, r->atr, r->atr_len);
+		DMSG("%s", buf);
+	}
+}
+
+static uint32_t pcsc_reader_read_reg(struct pcsc_reader *r, uint32_t offset)
+{
+	return read32(r->mmio_base + offset);
+}
+
+static void pcsc_reader_write_reg(struct pcsc_reader *r, uint32_t offset,
+		uint32_t value)
+{
+	write32(value, r->mmio_base + offset);
+}
+
+static void pcsc_reader_get_atr(struct pcsc_reader *r)
+{
+	uint32_t atr_paddr = 0;
+	uint32_t atr_len = pcsc_reader_read_reg(r, PCSC_REG_READER_ATR_LEN);
+
+	atr_paddr = virt_to_phys((void *)r->atr);
+	pcsc_reader_write_reg(r, PCSC_REG_READER_RX_ADDR,
+			atr_paddr);
+	pcsc_reader_write_reg(r, PCSC_REG_READER_RX_SIZE,
+			atr_len);
+	pcsc_reader_write_reg(r, PCSC_REG_READER_CONTROL,
+			PCSC_READER_CTL_READ_ATR);
+	r->atr_len = atr_len;
+}
+
+static void pcsc_reader_connect(struct pcsc_reader *r)
+{
+	if (r->connected)
+		panic();
+
+	pcsc_reader_write_reg(r, PCSC_REG_READER_CONTROL,
+			PCSC_READER_CTL_CONNECT |
+			PCSC_READER_CTL_PROTOCOL_T1 |
+			PCSC_READER_CTL_SHARE_SHARED);
+	r->connected = true;
+	pcsc_reader_get_atr(r);
+}
+
+static void pcsc_reader_disconnect(struct pcsc_reader *r)
+{
+	if (!r->connected)
+		panic();
+
+	pcsc_reader_write_reg(r, PCSC_REG_READER_CONTROL,
+			PCSC_READER_CTL_DISCONNECT |
+			PCSC_READER_CTL_DISPOSITION_RESET_CARD);
+	r->connected = false;
+	r->atr_len = 0;
+}
+
+static TEE_Result pcsc_reader_transmit(struct pcsc_reader *r, uint8_t *tx_buf,
+		size_t tx_len, uint8_t *rx_buf, size_t *rx_len)
+{
+	uint32_t tx_buf_paddr = 0, rx_buf_paddr = 0;
+
+	if (!r->connected)
+		panic();
+
+	tx_buf_paddr = virt_to_phys((void *)tx_buf);
+	rx_buf_paddr = virt_to_phys((void *)rx_buf);
+
+	pcsc_reader_write_reg(r, PCSC_REG_READER_TX_ADDR,
+			tx_buf_paddr);
+	pcsc_reader_write_reg(r, PCSC_REG_READER_TX_SIZE,
+			tx_len);
+	pcsc_reader_write_reg(r, PCSC_REG_READER_RX_ADDR,
+			rx_buf_paddr);
+	pcsc_reader_write_reg(r, PCSC_REG_READER_RX_SIZE,
+			*rx_len);
+	pcsc_reader_write_reg(r, PCSC_REG_READER_CONTROL,
+			PCSC_READER_CTL_TRANSMIT);
+
+	*rx_len = pcsc_reader_read_reg(r, PCSC_REG_READER_RX_SIZE);
+	return TEE_SUCCESS;
+}
+
+static TEE_Result pcsc_passthru_reader_open(struct tee_se_reader *se_reader)
+{
+	struct pcsc_reader *r = se_reader->private_data;
+
+	if (!se_reader->prop.sePresent) {
+		EMSG("SE is not present");
+		return TEE_ERROR_COMMUNICATION;
+	}
+
+	pcsc_reader_connect(r);
+
+	pcsc_reader_dump(r);
+
+	return TEE_SUCCESS;
+}
+
+static void pcsc_passthru_reader_close(struct tee_se_reader *se_reader)
+{
+	struct pcsc_reader *r = se_reader->private_data;
+
+	pcsc_reader_disconnect(r);
+
+	pcsc_reader_dump(r);
+}
+
+static TEE_Result pcsc_passthru_reader_transmit(struct tee_se_reader *se_reader,
+		uint8_t *tx_buf, size_t tx_len, uint8_t *rx_buf, size_t *rx_len)
+{
+	struct pcsc_reader *r = se_reader->private_data;
+
+	return pcsc_reader_transmit(r, tx_buf, tx_len, rx_buf, rx_len);
+}
+
+static enum tee_se_reader_state pcsc_passthru_reader_get_state(
+		struct tee_se_reader *se_reader)
+{
+	struct pcsc_reader *r = se_reader->private_data;
+
+	if (r->state & PCSC_READER_STATE_PRESENT)
+		return READER_STATE_SE_INSERTED;
+	else
+		return READER_STATE_SE_EJECTED;
+}
+
+static TEE_Result pcsc_passthru_reader_get_atr(
+		struct tee_se_reader *se_reader, uint8_t **atr,
+		size_t *atr_len)
+{
+	struct pcsc_reader *r = se_reader->private_data;
+
+	if (r->atr_len > 0) {
+		*atr = r->atr;
+		*atr_len = r->atr_len;
+		return TEE_SUCCESS;
+	} else
+		return TEE_ERROR_COMMUNICATION;
+}
+
+static struct tee_se_reader_ops pcsc_passthru_reader_ops = {
+	.open = pcsc_passthru_reader_open,
+	.close = pcsc_passthru_reader_close,
+	.get_state = pcsc_passthru_reader_get_state,
+	.get_atr = pcsc_passthru_reader_get_atr,
+	.transmit = pcsc_passthru_reader_transmit,
+};
+
+void init_reader(struct pcsc_reader *r, uint8_t index, uint32_t mmio_base)
+{
+	r->index = index;
+	r->mmio_base = mmio_base;
+	r->atr_len = 0;
+	r->state = pcsc_reader_read_reg(r, PCSC_REG_READER_STATE);
+
+	snprintf(r->se_reader.name, TEE_SE_READER_NAME_MAX,
+			"tee_reader_pcsc#%d", index);
+	r->se_reader.ops = &pcsc_passthru_reader_ops;
+	r->se_reader.prop.teeOnly = true;
+	r->se_reader.prop.selectResponseEnable = true;
+	if (r->state & PCSC_READER_STATE_PRESENT)
+		r->se_reader.prop.sePresent = true;
+	else
+		r->se_reader.prop.sePresent = false;
+	r->se_reader.private_data = r;
+}
+
diff --git a/core/tee/se/reader/passthru_reader/reader.h b/core/tee/se/reader/passthru_reader/reader.h
new file mode 100644
index 0000000..cb9541c
--- /dev/null
+++ b/core/tee/se/reader/passthru_reader/reader.h
@@ -0,0 +1,45 @@
+/*
+ * Copyright (c) 2014, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef READER_H
+#define READER_H
+
+#define MAX_ATR_SIZE	23
+
+struct pcsc_reader {
+	bool connected;
+	uint8_t index;
+	uint32_t state;
+	uint32_t mmio_base;
+	uint8_t atr[MAX_ATR_SIZE];
+	uint8_t atr_len;
+	struct tee_se_reader se_reader;
+};
+
+void init_reader(struct pcsc_reader *r, uint8_t index, uint32_t mmio_base);
+
+#endif
diff --git a/core/tee/se/reader/passthru_reader/sub.mk b/core/tee/se/reader/passthru_reader/sub.mk
new file mode 100644
index 0000000..f5b1e08
--- /dev/null
+++ b/core/tee/se/reader/passthru_reader/sub.mk
@@ -0,0 +1 @@
+srcs-y	+= driver.c reader.c
diff --git a/core/tee/se/reader/sub.mk b/core/tee/se/reader/sub.mk
new file mode 100644
index 0000000..7899bcc
--- /dev/null
+++ b/core/tee/se/reader/sub.mk
@@ -0,0 +1 @@
+subdirs-${CFG_PCSC_PASSTHRU_READER_DRV} = passthru_reader
diff --git a/core/tee/se/reader_priv.h b/core/tee/se/reader_priv.h
new file mode 100644
index 0000000..00e139e
--- /dev/null
+++ b/core/tee/se/reader_priv.h
@@ -0,0 +1,49 @@
+/*
+ * Copyright (c) 2014, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef TEE_SE_READER_PRIV_H
+#define TEE_SE_READER_PRIV_H
+
+/*
+ * Reader Proxy is used to serialize access from multiple seesions,
+ * and maintain reference counter. All access to the reader should
+ * go through Reader Proxy
+ */
+struct tee_se_reader_proxy {
+	struct tee_se_reader *reader;
+	int refcnt;
+	bool basic_channel_locked;
+	struct mutex mutex;
+
+	TAILQ_ENTRY(tee_se_reader_proxy) link;
+};
+
+TEE_Result tee_se_reader_check_state(struct tee_se_reader_proxy *proxy);
+
+int tee_se_reader_get_refcnt(struct tee_se_reader_proxy *proxy);
+
+#endif
diff --git a/core/tee/se/service.c b/core/tee/se/service.c
new file mode 100644
index 0000000..ae063ed
--- /dev/null
+++ b/core/tee/se/service.c
@@ -0,0 +1,209 @@
+/*
+ * Copyright (c) 2014, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <assert.h>
+#include <tee_api_types.h>
+#include <trace.h>
+
+#include <kernel/tee_ta_manager.h>
+#include <kernel/user_ta.h>
+#include <tee/se/service.h>
+#include <tee/se/session.h>
+#include <tee/se/reader.h>
+
+#include "service_priv.h"
+#include "reader_priv.h"
+#include "session_priv.h"
+
+TEE_Result tee_se_service_open(
+		struct tee_se_service **service)
+{
+	TEE_Result ret;
+	struct tee_se_service *h;
+	struct tee_ta_session *sess;
+	struct user_ta_ctx *utc;
+
+	ret = tee_ta_get_current_session(&sess);
+	if (ret != TEE_SUCCESS)
+		return ret;
+	utc = to_user_ta_ctx(sess->ctx);
+
+	assert(service);
+	if (utc->se_service != NULL)
+		return TEE_ERROR_ACCESS_CONFLICT;
+
+	h = malloc(sizeof(struct tee_se_service));
+	if (!h)
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	TAILQ_INIT(&h->opened_sessions);
+	TAILQ_INIT(&h->closed_sessions);
+	mutex_init(&h->mutex);
+	*service = h;
+
+	utc->se_service = h;
+
+	return TEE_SUCCESS;
+}
+
+TEE_Result tee_se_service_add_session(
+		struct tee_se_service *service,
+		struct tee_se_session *session)
+{
+	assert(service && session);
+
+	mutex_lock(&service->mutex);
+	TAILQ_INSERT_TAIL(&service->opened_sessions, session, link);
+	mutex_unlock(&service->mutex);
+
+	return TEE_SUCCESS;
+}
+
+TEE_Result tee_se_service_is_session_closed(
+		struct tee_se_service *service,
+		struct tee_se_session *session)
+{
+	struct tee_se_session *s;
+
+	TAILQ_FOREACH(s, &service->closed_sessions, link) {
+		if (s == session)
+			return TEE_SUCCESS;
+	}
+
+	return tee_se_reader_check_state(session->reader_proxy);
+}
+
+void tee_se_service_close_session(
+		struct tee_se_service *service,
+		struct tee_se_session *session)
+{
+	assert(service && session);
+
+	tee_se_session_close(session);
+
+	mutex_lock(&service->mutex);
+
+	TAILQ_REMOVE(&service->opened_sessions,
+			session, link);
+	TAILQ_INSERT_TAIL(&service->closed_sessions,
+			session, link);
+
+	mutex_unlock(&service->mutex);
+}
+
+void tee_se_service_close_sessions_by_reader(
+		struct tee_se_service *service,
+		struct tee_se_reader_proxy *proxy)
+{
+	struct tee_se_session *s;
+
+	assert(service && proxy);
+
+	TAILQ_FOREACH(s, &service->opened_sessions, link) {
+		if (s->reader_proxy == proxy)
+			tee_se_service_close_session(service, s);
+	}
+}
+
+TEE_Result tee_se_service_close(
+		struct tee_se_service *service __unused)
+{
+	TEE_Result ret;
+	struct tee_se_service *h;
+	struct tee_se_session *s;
+	struct tee_ta_session *sess;
+	struct user_ta_ctx *utc;
+
+	ret = tee_ta_get_current_session(&sess);
+	if (ret != TEE_SUCCESS)
+		return ret;
+
+	utc = to_user_ta_ctx(sess->ctx);
+	assert(utc->se_service);
+	h = utc->se_service;
+
+	/* clean up all sessions */
+	mutex_lock(&h->mutex);
+	TAILQ_FOREACH(s, &h->opened_sessions, link) {
+		TAILQ_REMOVE(&h->opened_sessions, s, link);
+		tee_se_session_close(s);
+	}
+
+	TAILQ_FOREACH(s, &h->closed_sessions, link)
+		TAILQ_REMOVE(&h->closed_sessions, s, link);
+
+	mutex_unlock(&h->mutex);
+
+	free(h);
+
+	return TEE_SUCCESS;
+}
+
+bool tee_se_service_is_valid(struct tee_se_service *service)
+{
+	TEE_Result ret;
+	struct tee_ta_session *sess;
+
+	ret = tee_ta_get_current_session(&sess);
+	if (ret != TEE_SUCCESS)
+		return false;
+
+	if (to_user_ta_ctx(sess->ctx)->se_service == service)
+		return true;
+	else
+		return false;
+}
+
+bool tee_se_service_is_session_valid(
+		struct tee_se_service *service,
+		struct tee_se_session *session_service)
+{
+	struct tee_se_session *sh;
+
+	TAILQ_FOREACH(sh, &service->opened_sessions, link) {
+		if (sh == session_service)
+			return true;
+	}
+	TAILQ_FOREACH(sh, &service->closed_sessions, link) {
+		if (sh == session_service)
+			return true;
+	}
+	return false;
+}
+
+bool tee_se_service_is_channel_valid(struct tee_se_service *service,
+		struct tee_se_channel *channel)
+{
+	struct tee_se_session *s;
+
+	TAILQ_FOREACH(s, &service->opened_sessions, link) {
+		if (tee_se_session_is_channel_exist(s, channel))
+			return true;
+	}
+
+	return false;
+}
diff --git a/core/tee/se/service_priv.h b/core/tee/se/service_priv.h
new file mode 100644
index 0000000..9ad155a
--- /dev/null
+++ b/core/tee/se/service_priv.h
@@ -0,0 +1,42 @@
+/*
+ * Copyright (c) 2014, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef TEE_SE_SERVICE_PRIV_H
+#define TEE_SE_SERVICE_PRIV_H
+
+TAILQ_HEAD(se_session_head, tee_se_session);
+
+struct tee_se_service {
+	/* list of sessions opened on the service */
+	struct se_session_head opened_sessions;
+	/* list of sessions closed on the service */
+	struct se_session_head closed_sessions;
+	/* mutex to pretect the session lists */
+	struct mutex mutex;
+};
+
+#endif
diff --git a/core/tee/se/session.c b/core/tee/se/session.c
new file mode 100644
index 0000000..ed28aa4
--- /dev/null
+++ b/core/tee/se/session.c
@@ -0,0 +1,194 @@
+/*
+ * Copyright (c) 2014, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <assert.h>
+#include <kernel/mutex.h>
+#include <stdlib.h>
+#include <sys/queue.h>
+#include <trace.h>
+
+#include <tee/se/reader.h>
+#include <tee/se/session.h>
+#include <tee/se/channel.h>
+#include <tee/se/iso7816.h>
+
+#include "session_priv.h"
+#include "channel_priv.h"
+
+struct tee_se_session *tee_se_session_alloc(
+		struct tee_se_reader_proxy *proxy)
+{
+	struct tee_se_session *s;
+
+	assert(proxy);
+	s = malloc(sizeof(struct tee_se_session));
+	if (s) {
+		TAILQ_INIT(&s->channels);
+		s->reader_proxy = proxy;
+	}
+	return s;
+}
+
+void tee_se_session_free(struct tee_se_session *s)
+{
+	free(s);
+}
+
+bool tee_se_session_is_channel_exist(struct tee_se_session *s,
+		struct tee_se_channel *c)
+{
+	struct tee_se_channel *c1;
+
+	TAILQ_FOREACH(c1, &s->channels, link) {
+		if (c1 == c)
+			return true;
+	}
+	return false;
+}
+
+TEE_Result tee_se_session_get_atr(struct tee_se_session *s,
+		uint8_t **atr, size_t *atr_len)
+{
+	assert(s && atr && atr_len);
+
+	return tee_se_reader_get_atr(s->reader_proxy, atr, atr_len);
+}
+
+TEE_Result tee_se_session_open_basic_channel(struct tee_se_session *s,
+		struct tee_se_aid *aid, struct tee_se_channel **channel)
+{
+	struct tee_se_channel *c;
+	TEE_Result ret;
+
+	assert(s && channel && !*channel);
+
+	if (tee_se_reader_is_basic_channel_locked(s->reader_proxy)) {
+		*channel = NULL;
+		return TEE_ERROR_NOT_SUPPORTED;
+	}
+
+	c = tee_se_channel_alloc(s, 0);
+	if (!c)
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	if (aid) {
+		ret = iso7816_select(c, aid);
+		if (ret != TEE_SUCCESS)
+			goto err_free_channel;
+	}
+
+	tee_se_reader_lock_basic_channel(s->reader_proxy);
+	*channel = c;
+	TAILQ_INSERT_TAIL(&s->channels, c, link);
+
+	return TEE_SUCCESS;
+
+err_free_channel:
+	tee_se_channel_free(c);
+	return ret;
+}
+
+TEE_Result tee_se_session_open_logical_channel(struct tee_se_session *s,
+		struct tee_se_aid *aid, struct tee_se_channel **channel)
+{
+	int channel_id;
+	struct tee_se_channel *c;
+	TEE_Result ret;
+
+	assert(s && channel && !*channel);
+
+	ret = iso7816_open_available_logical_channel(s, &channel_id);
+	if (ret != TEE_SUCCESS)
+		return ret;
+
+	c = tee_se_channel_alloc(s, channel_id);
+	if (!c)
+		goto err_close_channel;
+
+	if (aid != NULL) {
+		ret = iso7816_select(c, aid);
+		if (ret != TEE_SUCCESS)
+			goto err_free_channel;
+	}
+
+	*channel = c;
+	TAILQ_INSERT_TAIL(&s->channels, c, link);
+
+	return TEE_SUCCESS;
+
+err_free_channel:
+	tee_se_channel_free(c);
+err_close_channel:
+	iso7816_close_logical_channel(s, channel_id);
+
+	return ret;
+}
+
+void tee_se_session_close_channel(struct tee_se_session *s,
+		struct tee_se_channel *c)
+{
+	int channel_id;
+
+	assert(s && c);
+	channel_id = tee_se_channel_get_id(c);
+	if (channel_id > 0) {
+		iso7816_close_logical_channel(s, channel_id);
+	} else {
+		tee_se_reader_unlock_basic_channel(s->reader_proxy);
+	}
+
+	TAILQ_REMOVE(&s->channels, c, link);
+	tee_se_channel_free(c);
+}
+
+TEE_Result tee_se_session_transmit(struct tee_se_session *s,
+		struct cmd_apdu *c, struct resp_apdu *r)
+{
+	struct tee_se_reader_proxy *h = s->reader_proxy;
+
+	/*
+	 * This call might block the caller.
+	 * The reader proxy will make sure only 1 session
+	 * is transmitting. Others should wait until the
+	 * activating transation finished.
+	 */
+	return iso7816_exchange_apdu(h, c, r);
+}
+
+void tee_se_session_close(struct tee_se_session *s)
+{
+	struct tee_se_channel *c;
+
+	assert(s);
+
+	TAILQ_FOREACH(c, &s->channels, link)
+		tee_se_session_close_channel(s, c);
+
+	tee_se_reader_detach(s->reader_proxy);
+
+	tee_se_session_free(s);
+}
diff --git a/core/tee/se/session_priv.h b/core/tee/se/session_priv.h
new file mode 100644
index 0000000..81b28b4
--- /dev/null
+++ b/core/tee/se/session_priv.h
@@ -0,0 +1,46 @@
+/*
+ * Copyright (c) 2014, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef TEE_SE_SESSION_PRIV_H
+#define TEE_SE_SESSION_PRIV_H
+
+TAILQ_HEAD(channel_list, tee_se_channel);
+
+struct tee_se_session {
+	struct tee_se_reader_proxy *reader_proxy;
+
+	/* list of channels opened on the session*/
+	struct channel_list channels;
+
+	TAILQ_ENTRY(tee_se_session) link;
+};
+
+struct tee_se_session *tee_se_session_alloc(struct tee_se_reader_proxy *proxy);
+
+void tee_se_session_free(struct tee_se_session *s);
+
+#endif
diff --git a/core/tee/se/sub.mk b/core/tee/se/sub.mk
new file mode 100644
index 0000000..f5ebdd8
--- /dev/null
+++ b/core/tee/se/sub.mk
@@ -0,0 +1,3 @@
+srcs-y += service.c manager.c reader.c iso7816.c session.c channel.c aid.c apdu.c util.c svc.c
+
+subdirs-y += reader
diff --git a/core/tee/se/svc.c b/core/tee/se/svc.c
new file mode 100644
index 0000000..2859989
--- /dev/null
+++ b/core/tee/se/svc.c
@@ -0,0 +1,516 @@
+/*
+ * Copyright (c) 2014, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+#include <tee_api_types.h>
+#include <kernel/tee_ta_manager.h>
+#include <kernel/user_ta.h>
+#include <tee/tee_svc.h>
+#include <tee/se/svc.h>
+#include <trace.h>
+#include <utee_defines.h>
+
+TEE_Result syscall_se_service_open(uint32_t *service_handle)
+{
+	struct tee_ta_session *sess;
+	struct tee_se_service *kservice;
+	TEE_Result ret;
+
+	ret = tee_ta_get_current_session(&sess);
+	if (ret != TEE_SUCCESS)
+		return ret;
+
+	ret = tee_se_service_open(&kservice);
+	if (ret != TEE_SUCCESS)
+		return ret;
+
+	return tee_svc_copy_kaddr_to_uref(service_handle, kservice);
+}
+
+TEE_Result syscall_se_service_close(unsigned long service_handle)
+{
+	struct tee_se_service *h = tee_svc_uref_to_kaddr(service_handle);
+
+	if (!tee_se_service_is_valid(h))
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	return tee_se_service_close(h);
+}
+
+TEE_Result syscall_se_service_get_readers(unsigned long service_handle,
+			uint32_t *reader_handles, uint64_t *len)
+{
+	TEE_Result ret;
+	size_t i;
+	size_t tmp_klen;
+	uint64_t klen;
+	struct tee_se_service *h = tee_svc_uref_to_kaddr(service_handle);
+	struct tee_ta_session *sess;
+	struct tee_se_reader_proxy **kreaders;
+	size_t kreaders_size;
+
+	if (!tee_se_service_is_valid(h))
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	ret = tee_ta_get_current_session(&sess);
+	if (ret != TEE_SUCCESS)
+		return ret;
+
+	ret = tee_svc_copy_from_user(&klen, len, sizeof(klen));
+	if (ret != TEE_SUCCESS)
+		return ret;
+
+	if (klen < tee_se_manager_get_reader_count())
+		return TEE_ERROR_SHORT_BUFFER;
+
+	kreaders_size = klen * sizeof(struct tee_se_reader_proxy *);
+	kreaders = malloc(kreaders_size);
+	if (kreaders == NULL)
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	tmp_klen = klen;
+	ret = tee_se_manager_get_readers(kreaders, &tmp_klen);
+	if (ret != TEE_SUCCESS)
+		goto err_free_kreaders;
+	klen = tmp_klen;
+
+	for (i = 0; i < klen; i++) {
+		ret = tee_svc_copy_kaddr_to_uref(&reader_handles[i],
+						 kreaders[i]);
+		if (ret != TEE_SUCCESS)
+			goto err_free_kreaders;
+	}
+
+	ret = tee_svc_copy_to_user(len, &klen, sizeof(*len));
+
+err_free_kreaders:
+	free(kreaders);
+
+	return ret;
+}
+
+TEE_Result syscall_se_reader_get_prop(unsigned long reader_handle, uint32_t *p)
+{
+	TEE_Result ret;
+	TEE_SEReaderProperties kprop;
+	uint32_t kp = 0;
+	struct tee_se_reader_proxy *r = tee_svc_uref_to_kaddr(reader_handle);
+	struct tee_ta_session *sess;
+
+	if (!tee_se_manager_is_reader_proxy_valid(r))
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	ret = tee_ta_get_current_session(&sess);
+	if (ret != TEE_SUCCESS)
+		return ret;
+
+	tee_se_reader_get_properties(r, &kprop);
+	if (kprop.sePresent)
+		kp |= UTEE_SE_READER_PRESENT;
+	if (kprop.teeOnly)
+		kp |= UTEE_SE_READER_TEE_ONLY;
+	if (kprop.selectResponseEnable)
+		kp |= UTEE_SE_READER_SELECT_RESPONE_ENABLE;
+	ret = tee_svc_copy_to_user(p, &kp, sizeof(kp));
+	if (ret != TEE_SUCCESS)
+		return ret;
+
+	return TEE_SUCCESS;
+}
+
+TEE_Result syscall_se_reader_get_name(unsigned long reader_handle,
+			char *name, uint64_t *name_len)
+{
+	TEE_Result ret;
+	struct tee_se_reader_proxy *r = tee_svc_uref_to_kaddr(reader_handle);
+	struct tee_ta_session *sess;
+	char *kname;
+	size_t kname_len;
+	uint64_t uname_len;
+
+	if (!tee_se_manager_is_reader_proxy_valid(r))
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	ret = tee_ta_get_current_session(&sess);
+	if (ret != TEE_SUCCESS)
+		return ret;
+
+	ret = tee_svc_copy_from_user(&uname_len, name_len, sizeof(uname_len));
+	if (ret != TEE_SUCCESS)
+		return ret;
+
+	kname_len = uname_len;
+	tee_se_reader_get_name(r, &kname, &kname_len);
+
+	if (uname_len < kname_len)
+		return TEE_ERROR_SHORT_BUFFER;
+
+	ret = tee_svc_copy_to_user(name, kname, kname_len);
+	if (ret != TEE_SUCCESS)
+		return ret;
+
+	uname_len = kname_len;
+	ret = tee_svc_copy_to_user(name_len, &uname_len, sizeof(*name_len));
+	if (ret != TEE_SUCCESS)
+		return ret;
+
+	return TEE_SUCCESS;
+}
+
+TEE_Result syscall_se_reader_open_session(unsigned long reader_handle,
+			uint32_t *session_handle)
+{
+	TEE_Result ret;
+	struct tee_se_reader_proxy *r = tee_svc_uref_to_kaddr(reader_handle);
+	struct tee_ta_session *sess;
+	struct tee_se_service *service;
+	struct tee_se_session *ksession = NULL;
+
+	if (!tee_se_manager_is_reader_proxy_valid(r))
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	ret = tee_ta_get_current_session(&sess);
+	if (ret != TEE_SUCCESS)
+		return ret;
+
+	ret = tee_se_reader_open_session(r, &ksession);
+	if (ret != TEE_SUCCESS)
+		return ret;
+
+	service = to_user_ta_ctx(sess->ctx)->se_service;
+	ret = tee_se_service_add_session(service, ksession);
+
+	ret = tee_svc_copy_kaddr_to_uref(session_handle, ksession);
+	if (ret != TEE_SUCCESS)
+		return ret;
+
+	return TEE_SUCCESS;
+}
+
+TEE_Result syscall_se_reader_close_sessions(unsigned long reader_handle)
+{
+	TEE_Result ret;
+	struct tee_se_reader_proxy *r = tee_svc_uref_to_kaddr(reader_handle);
+	struct tee_se_service *service;
+	struct tee_ta_session *sess;
+
+	if (!tee_se_manager_is_reader_proxy_valid(r))
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	ret = tee_ta_get_current_session(&sess);
+	if (ret != TEE_SUCCESS)
+		return ret;
+
+	service = to_user_ta_ctx(sess->ctx)->se_service;
+	tee_se_service_close_sessions_by_reader(service, r);
+
+	return TEE_SUCCESS;
+}
+
+TEE_Result syscall_se_session_is_closed(unsigned long session_handle)
+{
+	TEE_Result ret;
+	struct tee_se_session *s = tee_svc_uref_to_kaddr(session_handle);
+	struct tee_ta_session *sess;
+	struct tee_se_service *service;
+
+	ret = tee_ta_get_current_session(&sess);
+	if (ret != TEE_SUCCESS)
+		return ret;
+
+	service = to_user_ta_ctx(sess->ctx)->se_service;
+
+	if (!tee_se_service_is_session_valid(service, s))
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	return tee_se_service_is_session_closed(service, s);
+}
+
+TEE_Result syscall_se_session_get_atr(unsigned long session_handle,
+			void *atr, uint64_t *atr_len)
+{
+	TEE_Result ret;
+	struct tee_se_session *s = tee_svc_uref_to_kaddr(session_handle);
+	struct tee_ta_session *sess;
+	struct tee_se_service *service;
+	size_t katr_len;
+	uint64_t uatr_len;
+	uint8_t *katr;
+
+	ret = tee_ta_get_current_session(&sess);
+	if (ret != TEE_SUCCESS)
+		return ret;
+
+	service = to_user_ta_ctx(sess->ctx)->se_service;
+	if (!tee_se_service_is_session_valid(service, s))
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	ret = tee_svc_copy_from_user(&uatr_len, atr_len, sizeof(uatr_len));
+	if (ret != TEE_SUCCESS)
+		return ret;
+
+	katr_len = uatr_len;
+	ret = tee_se_session_get_atr(s, &katr, &katr_len);
+	if (ret != TEE_SUCCESS)
+		return ret;
+
+	if (uatr_len < katr_len)
+		return TEE_ERROR_SHORT_BUFFER;
+
+	ret = tee_svc_copy_to_user(atr, katr, katr_len);
+	if (ret != TEE_SUCCESS)
+		return ret;
+
+	uatr_len = katr_len;
+	ret = tee_svc_copy_to_user(atr_len, &uatr_len, sizeof(*atr_len));
+	if (ret != TEE_SUCCESS)
+		return ret;
+
+	return TEE_SUCCESS;
+}
+
+TEE_Result syscall_se_session_open_channel(unsigned long session_handle,
+			unsigned long is_logical, const void *aid_buf,
+			size_t aid_buf_len, uint32_t *channel_handle)
+{
+	TEE_Result ret;
+	struct tee_se_session *s = tee_svc_uref_to_kaddr(session_handle);
+	struct tee_ta_session *sess;
+	struct tee_se_service *service;
+	struct tee_se_aid *se_aid = NULL;
+	struct tee_se_channel *kc = NULL;
+
+	ret = tee_ta_get_current_session(&sess);
+	if (ret != TEE_SUCCESS)
+		return ret;
+
+	service = to_user_ta_ctx(sess->ctx)->se_service;
+	if (!tee_se_service_is_session_valid(service, s))
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	if (aid_buf) {
+		ret = tee_se_aid_create_from_buffer((void *)aid_buf,
+						    aid_buf_len, &se_aid);
+		if (ret != TEE_SUCCESS)
+			return ret;
+	}
+
+	if (is_logical)
+		ret = tee_se_session_open_logical_channel(s, se_aid, &kc);
+	else
+		ret = tee_se_session_open_basic_channel(s, se_aid, &kc);
+	if (ret != TEE_SUCCESS)
+		goto error_free_aid;
+
+	ret = tee_svc_copy_kaddr_to_uref(channel_handle, kc);
+	if (ret != TEE_SUCCESS)
+		goto error_free_aid;
+
+	return TEE_SUCCESS;
+
+error_free_aid:
+	if (se_aid)
+		tee_se_aid_release(se_aid);
+	return TEE_SUCCESS;
+}
+
+TEE_Result syscall_se_session_close(unsigned long session_handle)
+{
+	TEE_Result ret;
+	struct tee_se_session *s = tee_svc_uref_to_kaddr(session_handle);
+	struct tee_ta_session *sess;
+	struct tee_se_service *service;
+
+	ret = tee_ta_get_current_session(&sess);
+	if (ret != TEE_SUCCESS)
+		return ret;
+
+	service = to_user_ta_ctx(sess->ctx)->se_service;
+	if (!tee_se_service_is_session_valid(service, s))
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	tee_se_service_close_session(service, s);
+
+	return TEE_SUCCESS;
+}
+
+TEE_Result syscall_se_channel_select_next(unsigned long channel_handle)
+{
+	TEE_Result ret;
+	struct tee_se_channel *c = tee_svc_uref_to_kaddr(channel_handle);
+	struct tee_ta_session *sess;
+	struct tee_se_service *service;
+
+	ret = tee_ta_get_current_session(&sess);
+	if (ret != TEE_SUCCESS)
+		return ret;
+
+	service = to_user_ta_ctx(sess->ctx)->se_service;
+	if (!tee_se_service_is_channel_valid(service, c))
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	tee_se_channel_select_next(c);
+
+	return TEE_SUCCESS;
+}
+
+TEE_Result syscall_se_channel_get_select_resp(unsigned long channel_handle,
+			void *resp, uint64_t *resp_len)
+{
+	TEE_Result ret;
+	struct tee_se_channel *c = tee_svc_uref_to_kaddr(channel_handle);
+	struct tee_ta_session *sess;
+	struct tee_se_service *service;
+	struct resp_apdu *resp_apdu;
+	size_t kresp_len;
+	uint64_t uresp_len;
+
+	ret = tee_ta_get_current_session(&sess);
+	if (ret != TEE_SUCCESS)
+		return ret;
+
+	service = to_user_ta_ctx(sess->ctx)->se_service;
+	if (!tee_se_service_is_channel_valid(service, c))
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	ret = tee_svc_copy_from_user(&uresp_len, resp_len, sizeof(size_t));
+	if (ret != TEE_SUCCESS)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	ret = tee_se_channel_get_select_response(c, &resp_apdu);
+	if (ret != TEE_SUCCESS)
+		return ret;
+
+	kresp_len = apdu_get_length(to_apdu_base(resp_apdu));
+	if (uresp_len < kresp_len)
+		return TEE_ERROR_SHORT_BUFFER;
+
+	ret = tee_svc_copy_to_user(resp,
+			apdu_get_data(to_apdu_base(resp_apdu)), kresp_len);
+	if (ret != TEE_SUCCESS)
+		return ret;
+
+	uresp_len = kresp_len;
+	ret = tee_svc_copy_to_user(resp_len, &uresp_len, sizeof(*resp_len));
+	if (ret != TEE_SUCCESS)
+		return ret;
+
+	return TEE_SUCCESS;
+}
+
+TEE_Result syscall_se_channel_transmit(unsigned long channel_handle,
+			void *cmd, unsigned long cmd_len, void *resp,
+			uint64_t *resp_len)
+{
+	TEE_Result ret;
+	struct tee_se_channel *c = tee_svc_uref_to_kaddr(channel_handle);
+	struct tee_ta_session *sess;
+	struct tee_se_service *service;
+	struct cmd_apdu *cmd_apdu;
+	struct resp_apdu *resp_apdu;
+	void *kcmd_buf;
+	uint64_t kresp_len;
+
+	ret = tee_ta_get_current_session(&sess);
+	if (ret != TEE_SUCCESS)
+		return ret;
+
+	service = to_user_ta_ctx(sess->ctx)->se_service;
+	if (!tee_se_service_is_channel_valid(service, c))
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	ret = tee_svc_copy_from_user(&kresp_len, resp_len, sizeof(kresp_len));
+	if (ret != TEE_SUCCESS)
+		return ret;
+
+	kcmd_buf = malloc(cmd_len);
+	if (kcmd_buf == NULL)
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	ret = tee_svc_copy_from_user(kcmd_buf, cmd, cmd_len);
+	if (ret != TEE_SUCCESS)
+		goto err_free_cmd_buf;
+
+	cmd_apdu =
+		alloc_cmd_apdu_from_buf(kcmd_buf, cmd_len);
+	if (cmd_apdu == NULL)
+		goto err_free_cmd_buf;
+
+	kresp_len -= 2; /* reserve space for SW1 and SW2 */
+	resp_apdu = alloc_resp_apdu(kresp_len);
+	if (resp_apdu == NULL)
+		goto err_free_cmd_apdu;
+
+	ret = tee_se_channel_transmit(c, cmd_apdu, resp_apdu);
+	if (ret != TEE_SUCCESS)
+		goto err_free_resp_apdu;
+
+	kresp_len = apdu_get_length(to_apdu_base(resp_apdu));
+	ret = tee_svc_copy_to_user(resp_len, &kresp_len, sizeof(*resp_len));
+	if (ret != TEE_SUCCESS)
+		goto err_free_resp_apdu;
+
+	ret = tee_svc_copy_to_user(resp, resp_apdu_get_data(resp_apdu),
+				   kresp_len);
+	if (ret != TEE_SUCCESS)
+		goto err_free_resp_apdu;
+
+	apdu_release(to_apdu_base(resp_apdu));
+	apdu_release(to_apdu_base(cmd_apdu));
+	free(kcmd_buf);
+
+	return TEE_SUCCESS;
+
+err_free_resp_apdu:
+	apdu_release(to_apdu_base(resp_apdu));
+err_free_cmd_apdu:
+	apdu_release(to_apdu_base(cmd_apdu));
+err_free_cmd_buf:
+	free(kcmd_buf);
+	return ret;
+}
+
+TEE_Result syscall_se_channel_close(unsigned long channel_handle)
+{
+	TEE_Result ret;
+	struct tee_se_channel *c = tee_svc_uref_to_kaddr(channel_handle);
+	struct tee_ta_session *sess;
+	struct tee_se_session *s;
+	struct tee_se_service *service;
+
+	ret = tee_ta_get_current_session(&sess);
+	if (ret != TEE_SUCCESS)
+		return ret;
+
+	service = to_user_ta_ctx(sess->ctx)->se_service;
+	if (!tee_se_service_is_channel_valid(service, c))
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	s = tee_se_channel_get_session(c);
+
+	tee_se_session_close_channel(s, c);
+
+	return TEE_SUCCESS;
+}
diff --git a/core/tee/se/util.c b/core/tee/se/util.c
new file mode 100644
index 0000000..d118f26
--- /dev/null
+++ b/core/tee/se/util.c
@@ -0,0 +1,78 @@
+/*
+ * Copyright (c) 2014, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <tee_api_types.h>
+#include <trace.h>
+#include <tee/se/util.h>
+
+#include <stdio.h>
+
+char *print_buf(char *buf, size_t *remain_size, const char *fmt, ...)
+{
+	va_list ap;
+	size_t len;
+
+	va_start(ap, fmt);
+	len = vsnprintf(buf, *remain_size, fmt, ap);
+	buf += len;
+	*remain_size -= len;
+	va_end(ap);
+	return buf;
+}
+
+void dump_hex(char *buf, size_t *remain_size, uint8_t *input_buf,
+		size_t input_size)
+{
+	size_t i;
+
+	for (i = 0; i < input_size; i++)
+		buf = print_buf(buf, remain_size, "%02X ", input_buf[i]);
+}
+
+void print_hex(uint8_t *input_buf, size_t input_size)
+{
+	char buf[DUMP_BUF_MAX];
+	size_t remain = sizeof(buf);
+
+	dump_hex(buf, &remain, input_buf, input_size);
+	DMSG("%s", buf);
+}
+
+uint8_t *hex_decode(const char *in, size_t len, uint8_t *out)
+{
+	size_t i, t, hn, ln;
+
+	for (t = 0, i = 0; i < len; i += 2, ++t) {
+		hn = in[i] > '9' ?
+			(in[i] | 32) - 'a' + 10 : in[i] - '0';
+		ln = in[i + 1] > '9' ?
+			(in[i + 1] | 32) - 'a' + 10 : in[i + 1] - '0';
+
+		out[t] = (hn << 4) | ln;
+	}
+	return out;
+}
diff --git a/core/tee/sub.mk b/core/tee/sub.mk
new file mode 100644
index 0000000..32f0f98
--- /dev/null
+++ b/core/tee/sub.mk
@@ -0,0 +1,47 @@
+CFG_CRYPTO ?= y
+
+ifeq (y,$(CFG_CRYPTO))
+
+# HMAC-based Extract-and-Expand Key Derivation Function
+# http://tools.ietf.org/html/rfc5869
+# This is an OP-TEE extension, not part of the GlobalPlatform Internal API v1.0
+CFG_CRYPTO_HKDF ?= y
+
+# NIST SP800-56A Concatenation Key Derivation Function
+# This is an OP-TEE extension
+CFG_CRYPTO_CONCAT_KDF ?= y
+
+# PKCS #5 v2.0 / RFC 2898 key derivation function 2
+# This is an OP-TEE extension
+CFG_CRYPTO_PBKDF2 ?= y
+
+endif
+
+srcs-y += tee_cryp_utl.c
+srcs-$(CFG_CRYPTO_HKDF) += tee_cryp_hkdf.c
+srcs-$(CFG_CRYPTO_CONCAT_KDF) += tee_cryp_concat_kdf.c
+srcs-$(CFG_CRYPTO_PBKDF2) += tee_cryp_pbkdf2.c
+
+ifeq ($(CFG_WITH_USER_TA),y)
+
+srcs-y += tee_svc.c
+cppflags-tee_svc.c-y += -DTEE_IMPL_VERSION=$(TEE_IMPL_VERSION)
+srcs-y += tee_svc_cryp.c
+srcs-y += tee_svc_storage.c
+srcs-$(CFG_RPMB_FS) += tee_rpmb_fs.c
+srcs-$(CFG_REE_FS) += tee_ree_fs.c
+srcs-$(CFG_SQL_FS) += tee_sql_fs.c
+srcs-$(call cfg-one-enabled,CFG_REE_FS CFG_SQL_FS) += tee_fs_rpc.c
+srcs-$(call cfg-one-enabled,CFG_REE_FS CFG_SQL_FS CFG_RPMB_FS) += \
+	tee_fs_rpc_cache.c
+srcs-y += tee_fs_key_manager.c
+srcs-y += tee_obj.c
+srcs-y += tee_pobj.c
+srcs-y += tee_time_generic.c
+
+endif #CFG_WITH_USER_TA,y
+
+srcs-y += uuid.c
+
+subdirs-$(CFG_SE_API) += se
+
diff --git a/core/tee/tee_cryp_concat_kdf.c b/core/tee/tee_cryp_concat_kdf.c
new file mode 100644
index 0000000..b0c7c3d
--- /dev/null
+++ b/core/tee/tee_cryp_concat_kdf.c
@@ -0,0 +1,105 @@
+/*
+ * Copyright (c) 2014, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <tee/tee_cryp_concat_kdf.h>
+#include <tee/tee_cryp_provider.h>
+#include <tee/tee_cryp_utl.h>
+#include <utee_defines.h>
+#include <stdlib.h>
+#include <string.h>
+
+TEE_Result tee_cryp_concat_kdf(uint32_t hash_id, const uint8_t *shared_secret,
+			       size_t shared_secret_len,
+			       const uint8_t *other_info,
+			       size_t other_info_len, uint8_t *derived_key,
+			       size_t derived_key_len)
+{
+	TEE_Result res;
+	size_t ctx_size, hash_len, i, n, sz;
+	void *ctx = NULL;
+	uint8_t tmp[TEE_MAX_HASH_SIZE];
+	uint32_t be_count;
+	uint8_t *out = derived_key;
+	uint32_t hash_algo = TEE_ALG_HASH_ALGO(hash_id);
+	const struct hash_ops *hash = &crypto_ops.hash;
+
+	if (!hash->get_ctx_size || !hash->init || !hash->update ||
+	    !hash->final) {
+		res = TEE_ERROR_NOT_IMPLEMENTED;
+		goto out;
+	}
+
+	res = hash->get_ctx_size(hash_algo, &ctx_size);
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	ctx = malloc(ctx_size);
+	if (!ctx) {
+		res = TEE_ERROR_OUT_OF_MEMORY;
+		goto out;
+	}
+
+	res = tee_hash_get_digest_size(hash_algo, &hash_len);
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	n = derived_key_len / hash_len;
+	sz = hash_len;
+	for (i = 1; i <= n + 1; i++) {
+		be_count = TEE_U32_TO_BIG_ENDIAN(i);
+
+		res = hash->init(ctx, hash_algo);
+		if (res != TEE_SUCCESS)
+			goto out;
+		res = hash->update(ctx, hash_algo, (uint8_t *)&be_count,
+				   sizeof(be_count));
+		if (res != TEE_SUCCESS)
+			goto out;
+		res = hash->update(ctx, hash_algo, shared_secret,
+				   shared_secret_len);
+		if (res != TEE_SUCCESS)
+			goto out;
+		if (other_info && other_info_len) {
+			res = hash->update(ctx, hash_algo, other_info,
+					   other_info_len);
+			if (res != TEE_SUCCESS)
+				goto out;
+		}
+		res = hash->final(ctx, hash_algo, tmp, sizeof(tmp));
+		if (res != TEE_SUCCESS)
+			goto out;
+
+		if (i == n + 1)
+			sz = derived_key_len % hash_len;
+		memcpy(out, tmp, sz);
+		out += sz;
+	}
+	res = TEE_SUCCESS;
+out:
+	free(ctx);
+	return res;
+}
diff --git a/core/tee/tee_cryp_hkdf.c b/core/tee/tee_cryp_hkdf.c
new file mode 100644
index 0000000..d5287d9
--- /dev/null
+++ b/core/tee/tee_cryp_hkdf.c
@@ -0,0 +1,208 @@
+/*
+ * Copyright (c) 2014, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <tee/tee_cryp_hkdf.h>
+#include <tee/tee_cryp_provider.h>
+#include <tee/tee_cryp_utl.h>
+#include <stdlib.h>
+#include <string.h>
+#include <utee_defines.h>
+
+
+static const uint8_t zero_salt[TEE_MAX_HASH_SIZE];
+
+static TEE_Result hkdf_extract(uint32_t hash_id, const uint8_t *ikm,
+			       size_t ikm_len, const uint8_t *salt,
+			       size_t salt_len, uint8_t *prk, size_t *prk_len)
+{
+	TEE_Result res;
+	size_t ctx_size;
+	void *ctx = NULL;
+	uint32_t hash_algo = TEE_ALG_HASH_ALGO(hash_id);
+	uint32_t hmac_algo = (TEE_OPERATION_MAC << 28) | hash_id;
+	const struct mac_ops *m = &crypto_ops.mac;
+
+	if (!m->get_ctx_size || !m->init || !m->update) {
+		res = TEE_ERROR_NOT_IMPLEMENTED;
+		goto out;
+	}
+
+	if (!salt || !salt_len) {
+		/*
+		 * RFC 5869 section 2.2:
+		 * If not provided, [the salt] is set to a string of HashLen
+		 * zeros
+		 */
+		salt = zero_salt;
+		res = tee_hash_get_digest_size(hash_algo, &salt_len);
+		if (res != TEE_SUCCESS)
+			goto out;
+	}
+
+	res = m->get_ctx_size(hmac_algo, &ctx_size);
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	ctx = malloc(ctx_size);
+	if (!ctx) {
+		res = TEE_ERROR_OUT_OF_MEMORY;
+		goto out;
+	}
+
+	/*
+	 * RFC 5869 section 2.1: "Note that in the extract step, 'IKM' is used
+	 * as the HMAC input, not as the HMAC key."
+	 * Therefore, salt is the HMAC key in the formula from section 2.2:
+	 * "PRK = HMAC-Hash(salt, IKM)"
+	 */
+	res = m->init(ctx, hmac_algo, salt, salt_len);
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	res = m->update(ctx, hmac_algo, ikm, ikm_len);
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	res = m->final(ctx, hmac_algo, prk, *prk_len);
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	res = tee_hash_get_digest_size(hash_algo, prk_len);
+out:
+	free(ctx);
+	return res;
+}
+
+static TEE_Result hkdf_expand(uint32_t hash_id, const uint8_t *prk,
+			      size_t prk_len, const uint8_t *info,
+			      size_t info_len, uint8_t *okm, size_t okm_len)
+{
+	uint8_t tn[TEE_MAX_HASH_SIZE];
+	size_t tn_len, hash_len, i, n, where, ctx_size;
+	TEE_Result res = TEE_SUCCESS;
+	void *ctx = NULL;
+	const struct mac_ops *m = &crypto_ops.mac;
+	uint32_t hash_algo = TEE_ALG_HASH_ALGO(hash_id);
+	uint32_t hmac_algo = TEE_ALG_HMAC_ALGO(hash_id);
+
+	if (!m->get_ctx_size || !m->init || !m->update || !m->final) {
+		res = TEE_ERROR_NOT_IMPLEMENTED;
+		goto out;
+	}
+
+	res = tee_hash_get_digest_size(hash_algo, &hash_len);
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	if (!okm || prk_len < hash_len) {
+		res = TEE_ERROR_BAD_STATE;
+		goto out;
+	}
+
+	if (!info)
+		info_len = 0;
+
+	res = m->get_ctx_size(hmac_algo, &ctx_size);
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	ctx = malloc(ctx_size);
+	if (!ctx) {
+		res = TEE_ERROR_OUT_OF_MEMORY;
+		goto out;
+	}
+
+	/* N = ceil(L/HashLen) */
+	n = okm_len / hash_len;
+	if ((okm_len % hash_len) != 0)
+		n++;
+
+	if (n > 255) {
+		res = TEE_ERROR_BAD_PARAMETERS;
+		goto out;
+	}
+
+
+	/*
+	 * RFC 5869 section 2.3
+	 *   T = T(1) | T(2) | T(3) | ... | T(N)
+	 *   OKM = first L octets of T
+	 *   T(0) = empty string (zero length)
+	 *   T(1) = HMAC-Hash(PRK, T(0) | info | 0x01)
+	 *   T(2) = HMAC-Hash(PRK, T(1) | info | 0x02)
+	 *   T(3) = HMAC-Hash(PRK, T(2) | info | 0x03)
+	 *   ...
+	 */
+	tn_len = 0;
+	where = 0;
+	for (i = 1; i <= n; i++) {
+		uint8_t c = i;
+
+		res = m->init(ctx, hmac_algo, prk, prk_len);
+		if (res != TEE_SUCCESS)
+			goto out;
+		res = m->update(ctx, hmac_algo, tn, tn_len);
+		if (res != TEE_SUCCESS)
+			goto out;
+		res = m->update(ctx, hmac_algo, info, info_len);
+		if (res != TEE_SUCCESS)
+			goto out;
+		res = m->update(ctx, hmac_algo, &c, 1);
+		if (res != TEE_SUCCESS)
+			goto out;
+		res = m->final(ctx, hmac_algo, tn, sizeof(tn));
+		if (res != TEE_SUCCESS)
+			goto out;
+
+		memcpy(okm + where, tn, (i < n) ? hash_len : (okm_len - where));
+		where += hash_len;
+		tn_len = hash_len;
+	}
+
+out:
+	free(ctx);
+	return res;
+}
+
+TEE_Result tee_cryp_hkdf(uint32_t hash_id, const uint8_t *ikm, size_t ikm_len,
+			 const uint8_t *salt, size_t salt_len,
+			 const uint8_t *info, size_t info_len, uint8_t *okm,
+			 size_t okm_len)
+{
+	TEE_Result res;
+	uint8_t prk[TEE_MAX_HASH_SIZE];
+	size_t prk_len = sizeof(prk);
+
+	res = hkdf_extract(hash_id, ikm, ikm_len, salt, salt_len, prk,
+			   &prk_len);
+	if (res != TEE_SUCCESS)
+		return res;
+	res = hkdf_expand(hash_id, prk, prk_len, info, info_len, okm,
+			  okm_len);
+
+	return res;
+}
diff --git a/core/tee/tee_cryp_pbkdf2.c b/core/tee/tee_cryp_pbkdf2.c
new file mode 100644
index 0000000..5e6abc1
--- /dev/null
+++ b/core/tee/tee_cryp_pbkdf2.c
@@ -0,0 +1,147 @@
+/*
+ * Copyright (c) 2014, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <tee/tee_cryp_pbkdf2.h>
+#include <tee/tee_cryp_provider.h>
+#include <tee/tee_cryp_utl.h>
+#include <utee_defines.h>
+#include <stdlib.h>
+#include <string.h>
+
+struct hmac_parms {
+	uint32_t algo;
+	size_t hash_len;
+	void *ctx;
+};
+
+struct pbkdf2_parms {
+	const uint8_t *password;
+	size_t password_len;
+	const uint8_t *salt;
+	size_t salt_len;
+	uint32_t iteration_count;
+};
+
+static TEE_Result pbkdf2_f(uint8_t *out, size_t len, uint32_t idx,
+			   struct hmac_parms *h, struct pbkdf2_parms *p)
+{
+	TEE_Result res;
+	uint8_t u[TEE_MAX_HASH_SIZE];
+	uint32_t be_index;
+	size_t i, j;
+	const struct mac_ops *mac = &crypto_ops.mac;
+
+	memset(out, 0, len);
+	for (i = 1; i <= p->iteration_count; i++) {
+		res = mac->init(h->ctx, h->algo, p->password, p->password_len);
+		if (res != TEE_SUCCESS)
+			return res;
+
+		if (i == 1) {
+			if (p->salt && p->salt_len) {
+				res = mac->update(h->ctx, h->algo, p->salt,
+						  p->salt_len);
+				if (res != TEE_SUCCESS)
+					return res;
+			}
+
+			be_index = TEE_U32_TO_BIG_ENDIAN(idx);
+
+			res = mac->update(h->ctx, h->algo,
+					  (uint8_t *)&be_index,
+					  sizeof(be_index));
+			if (res != TEE_SUCCESS)
+				return res;
+		} else {
+			res = mac->update(h->ctx, h->algo, u, h->hash_len);
+			if (res != TEE_SUCCESS)
+				return res;
+		}
+
+		res = mac->final(h->ctx, h->algo, u, sizeof(u));
+		if (res != TEE_SUCCESS)
+			return res;
+
+		for (j = 0; j < len; j++)
+			out[j] ^= u[j];
+	}
+	return TEE_SUCCESS;
+}
+
+TEE_Result tee_cryp_pbkdf2(uint32_t hash_id, const uint8_t *password,
+			   size_t password_len, const uint8_t *salt,
+			   size_t salt_len, uint32_t iteration_count,
+			   uint8_t *derived_key, size_t derived_key_len)
+{
+	TEE_Result res;
+	size_t ctx_size, i, l, r;
+	uint8_t *out = derived_key;
+	struct pbkdf2_parms pbkdf2_parms;
+	struct hmac_parms hmac_parms = {0, };
+	const struct mac_ops *mac = &crypto_ops.mac;
+
+	if (!mac->get_ctx_size || !mac->init || !mac->update ||
+	    !mac->final)
+		return TEE_ERROR_NOT_IMPLEMENTED;
+
+	hmac_parms.algo = TEE_ALG_HMAC_ALGO(hash_id);
+
+	res = tee_mac_get_digest_size(hmac_parms.algo, &hmac_parms.hash_len);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = mac->get_ctx_size(hmac_parms.algo, &ctx_size);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	hmac_parms.ctx = malloc(ctx_size);
+	if (!hmac_parms.ctx)
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	pbkdf2_parms.password = password;
+	pbkdf2_parms.password_len = password_len;
+	pbkdf2_parms.salt = salt;
+	pbkdf2_parms.salt_len = salt_len;
+	pbkdf2_parms.iteration_count = iteration_count;
+
+	l = derived_key_len / hmac_parms.hash_len;
+	r = derived_key_len % hmac_parms.hash_len;
+
+	for (i = 1; i <= l; i++) {
+		res = pbkdf2_f(out, hmac_parms.hash_len, i, &hmac_parms,
+			       &pbkdf2_parms);
+		if (res != TEE_SUCCESS)
+			goto out;
+		out += hmac_parms.hash_len;
+	}
+	if (r)
+		res = pbkdf2_f(out, r, i, &hmac_parms, &pbkdf2_parms);
+
+out:
+	free(hmac_parms.ctx);
+	return res;
+}
diff --git a/core/tee/tee_cryp_utl.c b/core/tee/tee_cryp_utl.c
new file mode 100644
index 0000000..fa01161
--- /dev/null
+++ b/core/tee/tee_cryp_utl.c
@@ -0,0 +1,403 @@
+/*
+ * Copyright (c) 2014, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <stdlib.h>
+#include <string.h>
+#include <string_ext.h>
+#include <utee_defines.h>
+#include <tee/tee_cryp_utl.h>
+#include <tee/tee_cryp_provider.h>
+#include <kernel/tee_time.h>
+#include <rng_support.h>
+#include <initcall.h>
+
+#if !defined(CFG_WITH_SOFTWARE_PRNG)
+TEE_Result get_rng_array(void *buffer, int len)
+{
+	char *buf_char = buffer;
+	int i;
+
+
+	if (buf_char == NULL)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	for (i = 0; i < len; i++)
+		buf_char[i] = hw_get_random_byte();
+
+	return TEE_SUCCESS;
+}
+#endif
+
+TEE_Result tee_hash_get_digest_size(uint32_t algo, size_t *size)
+{
+	switch (algo) {
+	case TEE_ALG_MD5:
+	case TEE_ALG_HMAC_MD5:
+		*size = TEE_MD5_HASH_SIZE;
+		break;
+	case TEE_ALG_SHA1:
+	case TEE_ALG_HMAC_SHA1:
+	case TEE_ALG_DSA_SHA1:
+		*size = TEE_SHA1_HASH_SIZE;
+		break;
+	case TEE_ALG_SHA224:
+	case TEE_ALG_HMAC_SHA224:
+	case TEE_ALG_DSA_SHA224:
+		*size = TEE_SHA224_HASH_SIZE;
+		break;
+	case TEE_ALG_SHA256:
+	case TEE_ALG_HMAC_SHA256:
+	case TEE_ALG_DSA_SHA256:
+		*size = TEE_SHA256_HASH_SIZE;
+		break;
+	case TEE_ALG_SHA384:
+	case TEE_ALG_HMAC_SHA384:
+		*size = TEE_SHA384_HASH_SIZE;
+		break;
+	case TEE_ALG_SHA512:
+	case TEE_ALG_HMAC_SHA512:
+		*size = TEE_SHA512_HASH_SIZE;
+		break;
+	default:
+		return TEE_ERROR_NOT_SUPPORTED;
+	}
+
+	return TEE_SUCCESS;
+}
+
+TEE_Result tee_hash_createdigest(uint32_t algo, const uint8_t *data,
+				 size_t datalen, uint8_t *digest,
+				 size_t digestlen)
+{
+	TEE_Result res = TEE_ERROR_BAD_STATE;
+	void *ctx = NULL;
+	size_t ctxsize;
+
+	if (crypto_ops.hash.get_ctx_size == NULL ||
+	    crypto_ops.hash.init == NULL ||
+	    crypto_ops.hash.update == NULL ||
+	    crypto_ops.hash.final == NULL)
+		return TEE_ERROR_NOT_IMPLEMENTED;
+
+	if (crypto_ops.hash.get_ctx_size(algo, &ctxsize) != TEE_SUCCESS) {
+		res = TEE_ERROR_NOT_SUPPORTED;
+		goto out;
+	}
+
+	ctx = malloc(ctxsize);
+	if (ctx == NULL) {
+		res = TEE_ERROR_OUT_OF_MEMORY;
+		goto out;
+	}
+
+	if (crypto_ops.hash.init(ctx, algo) != TEE_SUCCESS)
+		goto out;
+
+	if (datalen != 0) {
+		if (crypto_ops.hash.update(ctx, algo, data, datalen)
+		    != TEE_SUCCESS)
+			goto out;
+	}
+
+	if (crypto_ops.hash.final(ctx, algo, digest, digestlen) != TEE_SUCCESS)
+		goto out;
+
+	res = TEE_SUCCESS;
+
+out:
+	if (ctx)
+		free(ctx);
+
+	return res;
+}
+
+TEE_Result tee_mac_get_digest_size(uint32_t algo, size_t *size)
+{
+	switch (algo) {
+	case TEE_ALG_HMAC_MD5:
+	case TEE_ALG_HMAC_SHA224:
+	case TEE_ALG_HMAC_SHA1:
+	case TEE_ALG_HMAC_SHA256:
+	case TEE_ALG_HMAC_SHA384:
+	case TEE_ALG_HMAC_SHA512:
+		return tee_hash_get_digest_size(algo, size);
+	case TEE_ALG_AES_CBC_MAC_NOPAD:
+	case TEE_ALG_AES_CBC_MAC_PKCS5:
+	case TEE_ALG_AES_CMAC:
+		*size = TEE_AES_BLOCK_SIZE;
+		return TEE_SUCCESS;
+	case TEE_ALG_DES_CBC_MAC_NOPAD:
+	case TEE_ALG_DES_CBC_MAC_PKCS5:
+	case TEE_ALG_DES3_CBC_MAC_NOPAD:
+	case TEE_ALG_DES3_CBC_MAC_PKCS5:
+		*size = TEE_DES_BLOCK_SIZE;
+		return TEE_SUCCESS;
+	default:
+		return TEE_ERROR_NOT_SUPPORTED;
+	}
+}
+
+TEE_Result tee_cipher_get_block_size(uint32_t algo, size_t *size)
+{
+	switch (algo) {
+	case TEE_ALG_AES_CBC_MAC_NOPAD:
+	case TEE_ALG_AES_CBC_MAC_PKCS5:
+	case TEE_ALG_AES_CMAC:
+	case TEE_ALG_AES_ECB_NOPAD:
+	case TEE_ALG_AES_CBC_NOPAD:
+	case TEE_ALG_AES_CTR:
+	case TEE_ALG_AES_CTS:
+	case TEE_ALG_AES_XTS:
+	case TEE_ALG_AES_CCM:
+	case TEE_ALG_AES_GCM:
+		*size = 16;
+		break;
+
+	case TEE_ALG_DES_CBC_MAC_NOPAD:
+	case TEE_ALG_DES_CBC_MAC_PKCS5:
+	case TEE_ALG_DES_ECB_NOPAD:
+	case TEE_ALG_DES_CBC_NOPAD:
+	case TEE_ALG_DES3_CBC_MAC_NOPAD:
+	case TEE_ALG_DES3_CBC_MAC_PKCS5:
+	case TEE_ALG_DES3_ECB_NOPAD:
+	case TEE_ALG_DES3_CBC_NOPAD:
+		*size = 8;
+		break;
+
+	default:
+		return TEE_ERROR_NOT_SUPPORTED;
+	}
+
+	return TEE_SUCCESS;
+}
+
+TEE_Result tee_do_cipher_update(void *ctx, uint32_t algo,
+				TEE_OperationMode mode, bool last_block,
+				const uint8_t *data, size_t len, uint8_t *dst)
+{
+	TEE_Result res;
+	size_t block_size;
+
+	if (mode != TEE_MODE_ENCRYPT && mode != TEE_MODE_DECRYPT)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	if (crypto_ops.cipher.update == NULL)
+		return TEE_ERROR_NOT_IMPLEMENTED;
+	/*
+	 * Check that the block contains the correct number of data, apart
+	 * for the last block in some XTS / CTR / XTS mode
+	 */
+	res = tee_cipher_get_block_size(algo, &block_size);
+	if (res != TEE_SUCCESS)
+		return res;
+	if ((len % block_size) != 0) {
+		if (!last_block)
+			return TEE_ERROR_BAD_PARAMETERS;
+
+		switch (algo) {
+		case TEE_ALG_AES_ECB_NOPAD:
+		case TEE_ALG_DES_ECB_NOPAD:
+		case TEE_ALG_DES3_ECB_NOPAD:
+		case TEE_ALG_AES_CBC_NOPAD:
+		case TEE_ALG_DES_CBC_NOPAD:
+		case TEE_ALG_DES3_CBC_NOPAD:
+			return TEE_ERROR_BAD_PARAMETERS;
+
+		case TEE_ALG_AES_CTR:
+		case TEE_ALG_AES_XTS:
+		case TEE_ALG_AES_CTS:
+			/*
+			 * These modes doesn't require padding for the last
+			 * block.
+			 *
+			 * This isn't entirely true, both XTS and CTS can only
+			 * encrypt minimum one block and also they need at least
+			 * one complete block in the last update to finish the
+			 * encryption. The algorithms are supposed to detect
+			 * that, we're only making sure that all data fed up to
+			 * that point consists of complete blocks.
+			 */
+			break;
+
+		default:
+			return TEE_ERROR_NOT_SUPPORTED;
+		}
+	}
+
+	return crypto_ops.cipher.update(ctx, algo, mode, last_block, data, len,
+					dst);
+}
+
+/*
+ * From http://en.wikipedia.org/wiki/Ciphertext_stealing
+ * CBC ciphertext stealing encryption using a standard
+ * CBC interface:
+ *	1. Pad the last partial plaintext block with 0.
+ *	2. Encrypt the whole padded plaintext using the
+ *	   standard CBC mode.
+ *	3. Swap the last two ciphertext blocks.
+ *	4. Truncate the ciphertext to the length of the
+ *	   original plaintext.
+ *
+ * CBC ciphertext stealing decryption using a standard
+ * CBC interface
+ *	1. Dn = Decrypt (K, Cn-1). Decrypt the second to last
+ *	   ciphertext block.
+ *	2. Cn = Cn || Tail (Dn, B-M). Pad the ciphertext to the
+ *	   nearest multiple of the block size using the last
+ *	   B-M bits of block cipher decryption of the
+ *	   second-to-last ciphertext block.
+ *	3. Swap the last two ciphertext blocks.
+ *	4. Decrypt the (modified) ciphertext using the standard
+ *	   CBC mode.
+ *	5. Truncate the plaintext to the length of the original
+ *	   ciphertext.
+ */
+TEE_Result tee_aes_cbc_cts_update(void *cbc_ctx, void *ecb_ctx,
+				  TEE_OperationMode mode, bool last_block,
+				  const uint8_t *data, size_t len,
+				  uint8_t *dst)
+{
+	TEE_Result res;
+	int nb_blocks, len_last_block, block_size = 16;
+	uint8_t tmp_block[64], tmp2_block[64];
+
+	if (!last_block)
+		return tee_do_cipher_update(cbc_ctx, TEE_ALG_AES_CBC_NOPAD,
+					     mode, last_block, data, len, dst);
+
+	/* Compute the last block length and check constraints */
+	nb_blocks = ((len + block_size - 1) / block_size);
+	if (nb_blocks < 2)
+		return TEE_ERROR_BAD_STATE;
+	len_last_block = len % block_size;
+	if (len_last_block == 0)
+		len_last_block = block_size;
+
+	if (mode == TEE_MODE_ENCRYPT) {
+		memcpy(tmp_block,
+		       data + ((nb_blocks - 1) * block_size),
+		       len_last_block);
+		memset(tmp_block + len_last_block,
+		       0,
+		       block_size - len_last_block);
+
+		res = tee_do_cipher_update(cbc_ctx, TEE_ALG_AES_CBC_NOPAD,
+					   mode, 0, data,
+					   (nb_blocks - 1) * block_size, dst);
+		if (res != TEE_SUCCESS)
+			return res;
+
+		memcpy(dst + (nb_blocks - 1) * block_size,
+		       dst + (nb_blocks - 2) * block_size,
+		       len_last_block);
+
+		res = tee_do_cipher_update(cbc_ctx, TEE_ALG_AES_CBC_NOPAD,
+					   mode, 0, tmp_block, block_size,
+					   dst + (nb_blocks - 2) * block_size);
+		if (res != TEE_SUCCESS)
+			return res;
+	} else {
+		/* 1. Decrypt the second to last ciphertext block */
+		res = tee_do_cipher_update(ecb_ctx, TEE_ALG_AES_ECB_NOPAD,
+					   mode, 0,
+					   data + (nb_blocks - 2) * block_size,
+					   block_size, tmp2_block);
+		if (res != TEE_SUCCESS)
+			return res;
+
+		/* 2. Cn = Cn || Tail (Dn, B-M) */
+		memcpy(tmp_block, data + ((nb_blocks - 1) * block_size),
+		       len_last_block);
+		memcpy(tmp_block + len_last_block, tmp2_block + len_last_block,
+		       block_size - len_last_block);
+
+		/* 3. Swap the last two ciphertext blocks */
+		/* done by passing the correct buffers in step 4. */
+
+		/* 4. Decrypt the (modified) ciphertext */
+		if (nb_blocks > 2) {
+			res = tee_do_cipher_update(cbc_ctx,
+						   TEE_ALG_AES_CBC_NOPAD, mode,
+						   0, data,
+						   (nb_blocks - 2) *
+						   block_size, dst);
+			if (res != TEE_SUCCESS)
+				return res;
+		}
+
+		res = tee_do_cipher_update(cbc_ctx, TEE_ALG_AES_CBC_NOPAD,
+					   mode, 0, tmp_block, block_size,
+					   dst +
+					   ((nb_blocks - 2) * block_size));
+		if (res != TEE_SUCCESS)
+			return res;
+
+		res = tee_do_cipher_update(cbc_ctx, TEE_ALG_AES_CBC_NOPAD,
+					   mode, 0, data +
+					   ((nb_blocks - 2) * block_size),
+					   block_size, tmp_block);
+		if (res != TEE_SUCCESS)
+			return res;
+
+		/* 5. Truncate the plaintext */
+		memcpy(dst + (nb_blocks - 1) * block_size, tmp_block,
+		       len_last_block);
+	}
+	return TEE_SUCCESS;
+}
+
+TEE_Result tee_prng_add_entropy(const uint8_t *in, size_t len)
+{
+	if (crypto_ops.prng.add_entropy)
+		return crypto_ops.prng.add_entropy(in, len);
+
+	return TEE_SUCCESS;
+}
+
+/*
+ * Override this in your platform code to feed the PRNG platform-specific
+ * jitter entropy. This implementation does not efficiently deliver entropy
+ * and is here for backwards-compatibility.
+ */
+__weak void plat_prng_add_jitter_entropy(void)
+{
+	TEE_Time current;
+
+	if (tee_time_get_sys_time(&current) == TEE_SUCCESS)
+		tee_prng_add_entropy((uint8_t *)&current, sizeof(current));
+}
+
+static TEE_Result tee_cryp_init(void)
+{
+	if (crypto_ops.init)
+		return crypto_ops.init();
+
+	return TEE_SUCCESS;
+}
+
+service_init(tee_cryp_init);
diff --git a/core/tee/tee_fs_key_manager.c b/core/tee/tee_fs_key_manager.c
new file mode 100644
index 0000000..c827cef
--- /dev/null
+++ b/core/tee/tee_fs_key_manager.c
@@ -0,0 +1,553 @@
+/*
+ * Copyright (c) 2015, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+
+/*
+ * Acronyms:
+ *
+ * FEK - File Encryption Key
+ * SSK - Secure Storage Key
+ * TSK - Trusted app Storage Key
+ * IV  - Initial vector
+ * HUK - Hardware Unique Key
+ * RNG - Random Number Generator
+ */
+
+#include <initcall.h>
+#include <stdlib.h>
+#include <string.h>
+#include <kernel/panic.h>
+#include <kernel/tee_common_otp.h>
+#include <kernel/tee_ta_manager.h>
+#include <tee/tee_cryp_utl.h>
+#include <tee/tee_cryp_provider.h>
+#include <tee/tee_fs_key_manager.h>
+#include <compiler.h>
+#include <trace.h>
+#include <util.h>
+
+struct tee_fs_ssk {
+	bool is_init;
+	uint8_t key[TEE_FS_KM_SSK_SIZE];
+};
+
+struct aad {
+	const uint8_t *encrypted_key;
+	const uint8_t *iv;
+};
+
+struct km_header {
+	struct aad aad;
+	uint8_t *tag;
+};
+
+static struct tee_fs_ssk tee_fs_ssk;
+static uint8_t string_for_ssk_gen[] = "ONLY_FOR_tee_fs_ssk";
+
+
+static TEE_Result do_hmac(uint8_t *out_key, uint32_t out_key_size,
+			  const uint8_t *in_key, uint32_t in_key_size,
+			  const uint8_t *message, uint32_t message_size)
+{
+	TEE_Result res = TEE_ERROR_GENERIC;
+	uint8_t *ctx = NULL;
+	size_t hash_ctx_size = 0;
+
+	if (!out_key || !in_key || !message)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	res = crypto_ops.mac.get_ctx_size(TEE_FS_KM_HMAC_ALG, &hash_ctx_size);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	ctx = malloc(hash_ctx_size);
+	if (!ctx)
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	res = crypto_ops.mac.init(ctx, TEE_FS_KM_HMAC_ALG, in_key, in_key_size);
+	if (res != TEE_SUCCESS)
+		goto exit;
+
+	res = crypto_ops.mac.update(ctx, TEE_FS_KM_HMAC_ALG,
+			message, message_size);
+	if (res != TEE_SUCCESS)
+		goto exit;
+
+	res = crypto_ops.mac.final(ctx, TEE_FS_KM_HMAC_ALG, out_key,
+				   out_key_size);
+	if (res != TEE_SUCCESS)
+		goto exit;
+
+	res = TEE_SUCCESS;
+
+exit:
+	free(ctx);
+	return res;
+}
+
+static TEE_Result fek_crypt(TEE_OperationMode mode,
+		uint8_t *key, int size)
+{
+	TEE_Result res;
+	uint8_t *ctx = NULL;
+	size_t ctx_size;
+	uint8_t tsk[TEE_FS_KM_TSK_SIZE];
+	uint8_t dst_key[TEE_FS_KM_FEK_SIZE];
+	struct tee_ta_session *sess;
+
+	if (!key)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	if (size != TEE_FS_KM_FEK_SIZE)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	if (tee_fs_ssk.is_init == 0)
+		return TEE_ERROR_GENERIC;
+
+	res = tee_ta_get_current_session(&sess);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = do_hmac(tsk, sizeof(tsk), tee_fs_ssk.key, TEE_FS_KM_SSK_SIZE,
+		      (uint8_t *)&sess->ctx->uuid, sizeof(TEE_UUID));
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = crypto_ops.cipher.get_ctx_size(TEE_FS_KM_ENC_FEK_ALG, &ctx_size);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	ctx = malloc(ctx_size);
+	if (!ctx)
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	res = crypto_ops.cipher.init(ctx, TEE_FS_KM_ENC_FEK_ALG, mode, tsk,
+				     sizeof(tsk), NULL, 0, NULL, 0);
+	if (res != TEE_SUCCESS)
+		goto exit;
+
+	res = crypto_ops.cipher.update(ctx, TEE_FS_KM_ENC_FEK_ALG,
+			mode, true, key, size, dst_key);
+	if (res != TEE_SUCCESS)
+		goto exit;
+
+	crypto_ops.cipher.final(ctx, TEE_FS_KM_ENC_FEK_ALG);
+
+	memcpy(key, dst_key, sizeof(dst_key));
+
+exit:
+	free(ctx);
+
+	return res;
+}
+
+static TEE_Result generate_fek(uint8_t *key, uint8_t len)
+{
+	return crypto_ops.prng.read(key, len);
+}
+
+static TEE_Result generate_iv(uint8_t *iv, uint8_t len)
+{
+	return crypto_ops.prng.read(iv, len);
+}
+
+static TEE_Result tee_fs_init_key_manager(void)
+{
+	int res = TEE_SUCCESS;
+	struct tee_hw_unique_key huk;
+	uint8_t chip_id[TEE_FS_KM_CHIP_ID_LENGTH];
+	uint8_t message[sizeof(chip_id) + sizeof(string_for_ssk_gen)];
+
+	/* Secure Storage Key Generation:
+	 *
+	 *     SSK = HMAC(HUK, message)
+	 *     message := concatenate(chip_id, static string)
+	 * */
+	tee_otp_get_hw_unique_key(&huk);
+	tee_otp_get_die_id(chip_id, sizeof(chip_id));
+
+	memcpy(message, chip_id, sizeof(chip_id));
+	memcpy(message + sizeof(chip_id), string_for_ssk_gen,
+			sizeof(string_for_ssk_gen));
+
+	res = do_hmac(tee_fs_ssk.key, sizeof(tee_fs_ssk.key),
+			huk.data, sizeof(huk.data),
+			message, sizeof(message));
+
+	if (res == TEE_SUCCESS)
+		tee_fs_ssk.is_init = 1;
+
+	return res;
+}
+
+static TEE_Result do_auth_enc(TEE_OperationMode mode,
+		struct km_header *hdr,
+		uint8_t *fek, int fek_len,
+		const uint8_t *data_in, size_t in_size,
+		uint8_t *data_out, size_t *out_size)
+{
+	TEE_Result res = TEE_SUCCESS;
+	uint8_t *ctx = NULL;
+	size_t ctx_size;
+	size_t tag_len = TEE_FS_KM_MAX_TAG_LEN;
+
+	if ((mode != TEE_MODE_ENCRYPT) && (mode != TEE_MODE_DECRYPT))
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	if (*out_size < in_size) {
+		EMSG("output buffer(%zd) < input buffer(%zd)",
+				*out_size, in_size);
+		return TEE_ERROR_SHORT_BUFFER;
+	}
+
+	res = crypto_ops.authenc.get_ctx_size(TEE_FS_KM_AUTH_ENC_ALG,
+			&ctx_size);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	ctx = malloc(ctx_size);
+	if (!ctx) {
+		EMSG("request memory size %zu failed", ctx_size);
+		return TEE_ERROR_OUT_OF_MEMORY;
+	}
+
+	res = crypto_ops.authenc.init(ctx, TEE_FS_KM_AUTH_ENC_ALG,
+			mode, fek, fek_len, hdr->aad.iv,
+			TEE_FS_KM_IV_LEN, TEE_FS_KM_MAX_TAG_LEN,
+			sizeof(struct aad), in_size);
+	if (res != TEE_SUCCESS)
+		goto exit;
+
+	res = crypto_ops.authenc.update_aad(ctx, TEE_FS_KM_AUTH_ENC_ALG,
+			mode, (uint8_t *)hdr->aad.encrypted_key,
+			TEE_FS_KM_FEK_SIZE);
+	if (res != TEE_SUCCESS)
+		goto exit;
+
+	res = crypto_ops.authenc.update_aad(ctx, TEE_FS_KM_AUTH_ENC_ALG,
+			mode, (uint8_t *)hdr->aad.iv,
+			TEE_FS_KM_IV_LEN);
+	if (res != TEE_SUCCESS)
+		goto exit;
+
+	if (mode == TEE_MODE_ENCRYPT) {
+		res = crypto_ops.authenc.enc_final(ctx, TEE_FS_KM_AUTH_ENC_ALG,
+				data_in, in_size, data_out, out_size,
+				hdr->tag, &tag_len);
+	} else {
+		res = crypto_ops.authenc.dec_final(ctx, TEE_FS_KM_AUTH_ENC_ALG,
+				data_in, in_size, data_out, out_size,
+				hdr->tag, tag_len);
+	}
+
+	if (res != TEE_SUCCESS)
+		goto exit;
+
+	crypto_ops.authenc.final(ctx, TEE_FS_KM_AUTH_ENC_ALG);
+
+exit:
+	free(ctx);
+	return res;
+}
+
+size_t tee_fs_get_header_size(enum tee_fs_file_type type)
+{
+	size_t header_size = 0;
+
+	switch (type) {
+	case META_FILE:
+		header_size = sizeof(struct meta_header);
+		break;
+	case BLOCK_FILE:
+		header_size = sizeof(struct block_header);
+		break;
+	default:
+		panic("Unknown file type");
+	}
+
+	return header_size;
+}
+
+TEE_Result tee_fs_generate_fek(uint8_t *buf, int buf_size)
+{
+	TEE_Result res;
+
+	if (buf_size != TEE_FS_KM_FEK_SIZE)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	res = generate_fek(buf, TEE_FS_KM_FEK_SIZE);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	return fek_crypt(TEE_MODE_ENCRYPT, buf,
+			TEE_FS_KM_FEK_SIZE);
+}
+
+TEE_Result tee_fs_encrypt_file(enum tee_fs_file_type file_type,
+		const uint8_t *data_in, size_t data_in_size,
+		uint8_t *data_out, size_t *data_out_size,
+		const uint8_t *encrypted_fek)
+{
+	TEE_Result res = TEE_SUCCESS;
+	struct km_header hdr;
+	uint8_t iv[TEE_FS_KM_IV_LEN];
+	uint8_t tag[TEE_FS_KM_MAX_TAG_LEN];
+	uint8_t fek[TEE_FS_KM_FEK_SIZE];
+	uint8_t *ciphertext;
+	size_t cipher_size;
+	size_t header_size = tee_fs_get_header_size(file_type);
+
+	/*
+	 * Meta File Format: |Header|Chipertext|
+	 * Header Format:    |AAD|Tag|
+	 * AAD Format:       |Encrypted_FEK|IV|
+	 *
+	 * Block File Format: |Header|Ciphertext|
+	 * Header Format:     |IV|Tag|
+	 *
+	 * TSK = HMAC(SSK, TA_UUID)
+	 * FEK = AES_DECRYPT(TSK, Encrypted_FEK)
+	 * Chipertext = AES_GCM_ENCRYPT(FEK, IV, Meta_Info, AAD)
+	 */
+
+	if (*data_out_size != (header_size + data_in_size))
+		return TEE_ERROR_SHORT_BUFFER;
+
+	res = generate_iv(iv, TEE_FS_KM_IV_LEN);
+	if (res != TEE_SUCCESS)
+		goto fail;
+
+	memcpy(fek, encrypted_fek, TEE_FS_KM_FEK_SIZE);
+	res = fek_crypt(TEE_MODE_DECRYPT, fek, TEE_FS_KM_FEK_SIZE);
+	if (res != TEE_SUCCESS)
+		goto fail;
+
+	ciphertext = data_out + header_size;
+	cipher_size = data_in_size;
+
+	hdr.aad.iv = iv;
+	hdr.aad.encrypted_key = encrypted_fek;
+	hdr.tag = tag;
+
+	res = do_auth_enc(TEE_MODE_ENCRYPT, &hdr,
+			fek, TEE_FS_KM_FEK_SIZE,
+			data_in, data_in_size,
+			ciphertext, &cipher_size);
+
+	if (res == TEE_SUCCESS) {
+		if (file_type == META_FILE) {
+			memcpy(data_out, encrypted_fek, TEE_FS_KM_FEK_SIZE);
+			data_out += TEE_FS_KM_FEK_SIZE;
+		}
+
+		memcpy(data_out, iv, TEE_FS_KM_IV_LEN);
+		data_out += TEE_FS_KM_IV_LEN;
+		memcpy(data_out, tag, TEE_FS_KM_MAX_TAG_LEN);
+
+		*data_out_size = header_size + cipher_size;
+	}
+
+fail:
+	return res;
+}
+
+TEE_Result tee_fs_decrypt_file(enum tee_fs_file_type file_type,
+		const uint8_t *data_in, size_t data_in_size,
+		uint8_t *plaintext, size_t *plaintext_size,
+		uint8_t *encrypted_fek)
+{
+	TEE_Result res = TEE_SUCCESS;
+	struct km_header km_hdr;
+	size_t file_hdr_size = tee_fs_get_header_size(file_type);
+	const uint8_t *cipher = data_in + file_hdr_size;
+	int cipher_size = data_in_size - file_hdr_size;
+	uint8_t fek[TEE_FS_KM_FEK_SIZE];
+
+	if (file_type == META_FILE) {
+		struct meta_header *hdr = (struct meta_header *)data_in;
+
+		km_hdr.aad.encrypted_key = hdr->encrypted_key;
+		km_hdr.aad.iv = hdr->common.iv;
+		km_hdr.tag = hdr->common.tag;
+
+		/* return encrypted FEK to tee_fs which is used for block
+		 * encryption/decryption */
+		memcpy(encrypted_fek, hdr->encrypted_key, TEE_FS_KM_FEK_SIZE);
+	} else {
+		struct block_header *hdr = (struct block_header *)data_in;
+
+		km_hdr.aad.encrypted_key = encrypted_fek;
+		km_hdr.aad.iv = hdr->common.iv;
+		km_hdr.tag = hdr->common.tag;
+	}
+
+	memcpy(fek, km_hdr.aad.encrypted_key, TEE_FS_KM_FEK_SIZE);
+	res = fek_crypt(TEE_MODE_DECRYPT, fek, TEE_FS_KM_FEK_SIZE);
+	if (res != TEE_SUCCESS) {
+		EMSG("Failed to decrypt FEK, res=0x%x", res);
+		return res;
+	}
+
+	return do_auth_enc(TEE_MODE_DECRYPT, &km_hdr, fek, TEE_FS_KM_FEK_SIZE,
+			cipher, cipher_size, plaintext, plaintext_size);
+}
+
+static TEE_Result sha256(uint8_t *out, size_t out_size, const uint8_t *in,
+			 size_t in_size)
+{
+	TEE_Result res;
+	uint8_t *ctx = NULL;
+	size_t ctx_size;
+	uint32_t algo = TEE_ALG_SHA256;
+
+	res = crypto_ops.hash.get_ctx_size(algo, &ctx_size);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	ctx = malloc(ctx_size);
+	if (!ctx)
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	res = crypto_ops.hash.init(ctx, algo);
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	res = crypto_ops.hash.update(ctx, algo, in, in_size);
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	res = crypto_ops.hash.final(ctx, algo, out, out_size);
+
+out:
+	free(ctx);
+	return res;
+}
+
+static TEE_Result aes_ecb(uint8_t out[TEE_AES_BLOCK_SIZE],
+			  const uint8_t in[TEE_AES_BLOCK_SIZE],
+			  const uint8_t *key, size_t key_size)
+{
+	TEE_Result res;
+	uint8_t *ctx = NULL;
+	size_t ctx_size;
+	uint32_t algo = TEE_ALG_AES_ECB_NOPAD;
+
+	res = crypto_ops.cipher.get_ctx_size(algo, &ctx_size);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	ctx = malloc(ctx_size);
+	if (!ctx)
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	res = crypto_ops.cipher.init(ctx, algo, TEE_MODE_ENCRYPT, key,
+				     key_size, NULL, 0, NULL, 0);
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	res = crypto_ops.cipher.update(ctx, algo, TEE_MODE_ENCRYPT, true, in,
+				       TEE_AES_BLOCK_SIZE, out);
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	crypto_ops.cipher.final(ctx, algo);
+	res = TEE_SUCCESS;
+
+out:
+	free(ctx);
+	return res;
+}
+
+static TEE_Result essiv(uint8_t iv[TEE_AES_BLOCK_SIZE],
+			const uint8_t fek[TEE_FS_KM_FEK_SIZE],
+			uint16_t blk_idx)
+{
+	TEE_Result res;
+	uint8_t sha[TEE_SHA256_HASH_SIZE];
+	uint8_t pad_blkid[TEE_AES_BLOCK_SIZE] = { 0, };
+
+	res = sha256(sha, sizeof(sha), fek, TEE_FS_KM_FEK_SIZE);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	pad_blkid[0] = (blk_idx & 0xFF);
+	pad_blkid[1] = (blk_idx & 0xFF00) >> 8;
+
+	return aes_ecb(iv, pad_blkid, sha, 16);
+}
+
+/*
+ * Encryption/decryption of RPMB FS file data. This is AES CBC with ESSIV.
+ */
+TEE_Result tee_fs_crypt_block(uint8_t *out, const uint8_t *in, size_t size,
+			      uint16_t blk_idx, const uint8_t *encrypted_fek,
+			      TEE_OperationMode mode)
+{
+	TEE_Result res;
+	uint8_t fek[TEE_FS_KM_FEK_SIZE];
+	uint8_t iv[TEE_AES_BLOCK_SIZE];
+	uint8_t *ctx;
+	size_t ctx_size;
+	uint32_t algo = TEE_ALG_AES_CBC_NOPAD;
+
+	DMSG("%scrypt block #%u", (mode == TEE_MODE_ENCRYPT) ? "En" : "De",
+	     blk_idx);
+
+	/* Decrypt FEK */
+	memcpy(fek, encrypted_fek, TEE_FS_KM_FEK_SIZE);
+	res = fek_crypt(TEE_MODE_DECRYPT, fek, TEE_FS_KM_FEK_SIZE);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	/* Compute initialization vector for this block */
+	res = essiv(iv, fek, blk_idx);
+
+	/* Run AES CBC */
+	res = crypto_ops.cipher.get_ctx_size(algo, &ctx_size);
+	if (res != TEE_SUCCESS)
+		return res;
+	ctx = malloc(ctx_size);
+	if (!ctx)
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	res = crypto_ops.cipher.init(ctx, algo, mode, fek, sizeof(fek), NULL,
+				     0, iv, TEE_AES_BLOCK_SIZE);
+	if (res != TEE_SUCCESS)
+		goto exit;
+	res = crypto_ops.cipher.update(ctx, algo, mode, true, in, size, out);
+	if (res != TEE_SUCCESS)
+		goto exit;
+
+	crypto_ops.cipher.final(ctx, algo);
+
+exit:
+	free(ctx);
+	return res;
+}
+
+service_init_late(tee_fs_init_key_manager);
+
diff --git a/core/tee/tee_fs_rpc.c b/core/tee/tee_fs_rpc.c
new file mode 100644
index 0000000..5e1078a
--- /dev/null
+++ b/core/tee/tee_fs_rpc.c
@@ -0,0 +1,375 @@
+/*
+ * Copyright (c) 2016, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <assert.h>
+#include <kernel/thread.h>
+#include <mm/core_memprot.h>
+#include <optee_msg_supplicant.h>
+#include <stdlib.h>
+#include <string.h>
+#include <string_ext.h>
+#include <tee/tee_fs.h>
+#include <tee/tee_fs_rpc.h>
+#include <trace.h>
+#include <util.h>
+
+struct tee_fs_dir {
+	int nw_dir;
+	struct tee_fs_dirent d;
+};
+
+static TEE_Result operation_commit(struct tee_fs_rpc_operation *op)
+{
+	return thread_rpc_cmd(op->id, op->num_params, op->params);
+}
+
+static TEE_Result operation_open(uint32_t id, unsigned int cmd,
+				 const char *fname, int *fd)
+{
+	struct tee_fs_rpc_operation op = { .id = id, .num_params = 3 };
+	TEE_Result res;
+	void *va;
+	paddr_t pa;
+	uint64_t cookie;
+	size_t fname_size = strlen(fname) + 1;
+
+	va = tee_fs_rpc_cache_alloc(fname_size, &pa, &cookie);
+	if (!va)
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	op.params[0].attr = OPTEE_MSG_ATTR_TYPE_VALUE_INPUT;
+	op.params[0].u.value.a = cmd;
+
+	op.params[1].attr = OPTEE_MSG_ATTR_TYPE_TMEM_INPUT;
+	op.params[1].u.tmem.buf_ptr = pa;
+	op.params[1].u.tmem.size = fname_size;
+	op.params[1].u.tmem.shm_ref = cookie;
+	strlcpy(va, fname, fname_size);
+
+	op.params[2].attr = OPTEE_MSG_ATTR_TYPE_VALUE_OUTPUT;
+
+	res = operation_commit(&op);
+	if (res == TEE_SUCCESS)
+		*fd = op.params[2].u.value.a;
+
+	return res;
+}
+
+TEE_Result tee_fs_rpc_open(uint32_t id, const char *fname, int *fd)
+{
+	return operation_open(id, OPTEE_MRF_OPEN, fname, fd);
+}
+
+TEE_Result tee_fs_rpc_create(uint32_t id, const char *fname, int *fd)
+{
+	return operation_open(id, OPTEE_MRF_CREATE, fname, fd);
+}
+
+TEE_Result tee_fs_rpc_close(uint32_t id, int fd)
+{
+	struct tee_fs_rpc_operation op = { .id = id, .num_params = 1 };
+
+	op.params[0].attr = OPTEE_MSG_ATTR_TYPE_VALUE_INPUT;
+	op.params[0].u.value.a = OPTEE_MRF_CLOSE;
+	op.params[0].u.value.b = fd;
+
+	return operation_commit(&op);
+}
+
+TEE_Result tee_fs_rpc_read_init(struct tee_fs_rpc_operation *op,
+				uint32_t id, int fd, tee_fs_off_t offset,
+				size_t data_len, void **out_data)
+{
+	uint8_t *va;
+	paddr_t pa;
+	uint64_t cookie;
+
+	if (offset < 0)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	va = tee_fs_rpc_cache_alloc(data_len, &pa, &cookie);
+	if (!va)
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	memset(op, 0, sizeof(*op));
+	op->id = id;
+	op->num_params = 2;
+
+	op->params[0].attr = OPTEE_MSG_ATTR_TYPE_VALUE_INPUT;
+	op->params[0].u.value.a = OPTEE_MRF_READ;
+	op->params[0].u.value.b = fd;
+	op->params[0].u.value.c = offset;
+
+	op->params[1].attr = OPTEE_MSG_ATTR_TYPE_TMEM_OUTPUT;
+	op->params[1].u.tmem.buf_ptr = pa;
+	op->params[1].u.tmem.size = data_len;
+	op->params[1].u.tmem.shm_ref = cookie;
+
+	*out_data = va;
+
+	return TEE_SUCCESS;
+}
+
+TEE_Result tee_fs_rpc_read_final(struct tee_fs_rpc_operation *op,
+				 size_t *data_len)
+{
+	TEE_Result res = operation_commit(op);
+
+	if (res == TEE_SUCCESS)
+		*data_len = op->params[1].u.tmem.size;
+	return res;
+}
+
+TEE_Result tee_fs_rpc_write_init(struct tee_fs_rpc_operation *op,
+				 uint32_t id, int fd, tee_fs_off_t offset,
+				 size_t data_len, void **data)
+{
+	uint8_t *va;
+	paddr_t pa;
+	uint64_t cookie;
+
+	if (offset < 0)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	va = tee_fs_rpc_cache_alloc(data_len, &pa, &cookie);
+	if (!va)
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	memset(op, 0, sizeof(*op));
+	op->id = id;
+	op->num_params = 2;
+
+
+	op->params[0].attr = OPTEE_MSG_ATTR_TYPE_VALUE_INPUT;
+	op->params[0].u.value.a = OPTEE_MRF_WRITE;
+	op->params[0].u.value.b = fd;
+	op->params[0].u.value.c = offset;
+
+	op->params[1].attr = OPTEE_MSG_ATTR_TYPE_TMEM_INPUT;
+	op->params[1].u.tmem.buf_ptr = pa;
+	op->params[1].u.tmem.size = data_len;
+	op->params[1].u.tmem.shm_ref = cookie;
+
+	*data = va;
+
+	return TEE_SUCCESS;
+}
+
+TEE_Result tee_fs_rpc_write_final(struct tee_fs_rpc_operation *op)
+{
+	return operation_commit(op);
+}
+
+TEE_Result tee_fs_rpc_truncate(uint32_t id, int fd, size_t len)
+{
+	struct tee_fs_rpc_operation op = { .id = id, .num_params = 1 };
+
+	op.params[0].attr = OPTEE_MSG_ATTR_TYPE_VALUE_INPUT;
+	op.params[0].u.value.a = OPTEE_MRF_TRUNCATE;
+	op.params[0].u.value.b = fd;
+	op.params[0].u.value.c = len;
+
+	return operation_commit(&op);
+}
+
+TEE_Result tee_fs_rpc_remove(uint32_t id, const char *fname)
+{
+	struct tee_fs_rpc_operation op = { .id = id, .num_params = 2 };
+	void *va;
+	paddr_t pa;
+	uint64_t cookie;
+	size_t name_len = strlen(fname) + 1;
+
+	va = tee_fs_rpc_cache_alloc(name_len, &pa, &cookie);
+	if (!va)
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	op.params[0].attr = OPTEE_MSG_ATTR_TYPE_VALUE_INPUT;
+	op.params[0].u.value.a = OPTEE_MRF_REMOVE;
+
+	op.params[1].attr = OPTEE_MSG_ATTR_TYPE_TMEM_INPUT;
+	op.params[1].u.tmem.buf_ptr = pa;
+	op.params[1].u.tmem.size = name_len;
+	op.params[1].u.tmem.shm_ref = cookie;
+	strlcpy(va, fname, name_len);
+
+	return operation_commit(&op);
+}
+
+TEE_Result tee_fs_rpc_rename(uint32_t id, const char *old_fname,
+			     const char *new_fname, bool overwrite)
+{
+	struct tee_fs_rpc_operation op = { .id = id, .num_params = 3 };
+	char *va;
+	paddr_t pa;
+	uint64_t cookie;
+	size_t old_fname_size = strlen(old_fname) + 1;
+	size_t new_fname_size = strlen(new_fname) + 1;
+
+	va = tee_fs_rpc_cache_alloc(old_fname_size + new_fname_size,
+				    &pa, &cookie);
+	if (!va)
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	op.params[0].attr = OPTEE_MSG_ATTR_TYPE_VALUE_INPUT;
+	op.params[0].u.value.a = OPTEE_MRF_RENAME;
+	op.params[0].u.value.b = overwrite;
+
+	op.params[1].attr = OPTEE_MSG_ATTR_TYPE_TMEM_INPUT;
+	op.params[1].u.tmem.buf_ptr = pa;
+	op.params[1].u.tmem.size = old_fname_size;
+	op.params[1].u.tmem.shm_ref = cookie;
+	strlcpy(va, old_fname, old_fname_size);
+
+	op.params[2].attr = OPTEE_MSG_ATTR_TYPE_TMEM_INPUT;
+	op.params[2].u.tmem.buf_ptr = pa + old_fname_size;
+	op.params[2].u.tmem.size = new_fname_size;
+	op.params[2].u.tmem.shm_ref = cookie;
+	strlcpy(va + old_fname_size, new_fname, new_fname_size);
+
+	return operation_commit(&op);
+}
+
+TEE_Result tee_fs_rpc_opendir(uint32_t id, const char *name,
+			      struct tee_fs_dir **d)
+{
+	TEE_Result res;
+	struct tee_fs_rpc_operation op = { .id = id, .num_params = 3 };
+	void *va;
+	paddr_t pa;
+	uint64_t cookie;
+	size_t name_len = strlen(name) + 1;
+	struct tee_fs_dir *dir = calloc(1, sizeof(*dir));
+
+	if (!dir)
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	va = tee_fs_rpc_cache_alloc(name_len, &pa, &cookie);
+	if (!va) {
+		res = TEE_ERROR_OUT_OF_MEMORY;
+		goto err_exit;
+	}
+
+	op.params[0].attr = OPTEE_MSG_ATTR_TYPE_VALUE_INPUT;
+	op.params[0].u.value.a = OPTEE_MRF_OPENDIR;
+
+	op.params[1].attr = OPTEE_MSG_ATTR_TYPE_TMEM_INPUT;
+	op.params[1].u.tmem.buf_ptr = pa;
+	op.params[1].u.tmem.size = name_len;
+	op.params[1].u.tmem.shm_ref = cookie;
+	strlcpy(va, name, name_len);
+
+	op.params[2].attr = OPTEE_MSG_ATTR_TYPE_VALUE_OUTPUT;
+
+	res = operation_commit(&op);
+
+	if (res != TEE_SUCCESS)
+		goto err_exit;
+
+	dir->nw_dir = op.params[2].u.value.a;
+	*d = dir;
+
+	return TEE_SUCCESS;
+err_exit:
+	free(dir);
+
+	return res;
+}
+
+TEE_Result tee_fs_rpc_closedir(uint32_t id, struct tee_fs_dir *d)
+{
+	struct tee_fs_rpc_operation op = { .id = id, .num_params = 1 };
+
+	op.params[0].attr = OPTEE_MSG_ATTR_TYPE_VALUE_INPUT;
+	op.params[0].u.value.a = OPTEE_MRF_CLOSEDIR;
+	op.params[0].u.value.b = d->nw_dir;
+
+	if (d)
+		free(d->d.d_name);
+	free(d);
+	return operation_commit(&op);
+}
+
+TEE_Result tee_fs_rpc_readdir(uint32_t id, struct tee_fs_dir *d,
+			      struct tee_fs_dirent **ent)
+{
+	TEE_Result res;
+	struct tee_fs_rpc_operation op = { .id = id, .num_params = 2 };
+	void *va;
+	paddr_t pa;
+	uint64_t cookie;
+	const size_t max_name_len = TEE_FS_NAME_MAX + 1;
+
+	if (!d)
+		return TEE_ERROR_ITEM_NOT_FOUND;
+
+	va = tee_fs_rpc_cache_alloc(max_name_len, &pa, &cookie);
+	if (!va)
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	op.params[0].attr = OPTEE_MSG_ATTR_TYPE_VALUE_INPUT;
+	op.params[0].u.value.a = OPTEE_MRF_READDIR;
+	op.params[0].u.value.b = d->nw_dir;
+
+	op.params[1].attr = OPTEE_MSG_ATTR_TYPE_TMEM_OUTPUT;
+	op.params[1].u.tmem.buf_ptr = pa;
+	op.params[1].u.tmem.size = max_name_len;
+	op.params[1].u.tmem.shm_ref = cookie;
+
+	res = operation_commit(&op);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	free(d->d.d_name);
+	d->d.d_name = strndup(va, max_name_len);
+	if (!d->d.d_name)
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	*ent = &d->d;
+	return TEE_SUCCESS;
+}
+
+TEE_Result tee_fs_rpc_begin_transaction(uint32_t id)
+{
+	struct tee_fs_rpc_operation op = { .id = id, .num_params = 1 };
+
+	op.params[0].attr = OPTEE_MSG_ATTR_TYPE_VALUE_INPUT;
+	op.params[0].u.value.a = OPTEE_MRF_BEGIN_TRANSACTION;
+
+	return operation_commit(&op);
+}
+
+TEE_Result tee_fs_rpc_end_transaction(uint32_t id, bool rollback)
+{
+	struct tee_fs_rpc_operation op = { .id = id, .num_params = 1 };
+
+	op.params[0].attr = OPTEE_MSG_ATTR_TYPE_VALUE_INPUT;
+	op.params[0].u.value.a = OPTEE_MRF_END_TRANSACTION;
+	op.params[0].u.value.b = rollback;
+
+	return operation_commit(&op);
+}
diff --git a/core/tee/tee_fs_rpc_cache.c b/core/tee/tee_fs_rpc_cache.c
new file mode 100644
index 0000000..aef2eaa
--- /dev/null
+++ b/core/tee/tee_fs_rpc_cache.c
@@ -0,0 +1,85 @@
+/*
+ * Copyright (c) 2016, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <kernel/thread.h>
+#include <mm/core_memprot.h>
+#include <mm/core_mmu.h>
+#include <tee/tee_fs_rpc.h>
+
+void tee_fs_rpc_cache_clear(struct thread_specific_data *tsd)
+{
+	if (tsd->rpc_fs_payload) {
+		thread_rpc_free_payload(tsd->rpc_fs_payload_cookie);
+		tsd->rpc_fs_payload = NULL;
+		tsd->rpc_fs_payload_cookie = 0;
+		tsd->rpc_fs_payload_size = 0;
+	}
+}
+
+void *tee_fs_rpc_cache_alloc(size_t size, paddr_t *pa, uint64_t *cookie)
+{
+	struct thread_specific_data *tsd = thread_get_tsd();
+	size_t sz = size;
+	uint64_t c = 0;
+	paddr_t p;
+	void *va;
+
+	if (!size)
+		return NULL;
+
+	/*
+	 * Always allocate in page chunks as normal world allocates payload
+	 * memory as complete pages.
+	 */
+	sz = ROUNDUP(size, SMALL_PAGE_SIZE);
+
+	if (sz > tsd->rpc_fs_payload_size) {
+		tee_fs_rpc_cache_clear(tsd);
+
+		thread_rpc_alloc_payload(sz, &p, &c);
+		if (!p)
+			return NULL;
+		if (!ALIGNMENT_IS_OK(p, uint64_t))
+			goto err;
+
+		va = phys_to_virt(p, MEM_AREA_NSEC_SHM);
+		if (!va)
+			goto err;
+
+		tsd->rpc_fs_payload = va;
+		tsd->rpc_fs_payload_pa = p;
+		tsd->rpc_fs_payload_cookie = c;
+		tsd->rpc_fs_payload_size = sz;
+	}
+
+	*pa = tsd->rpc_fs_payload_pa;
+	*cookie = tsd->rpc_fs_payload_cookie;
+	return tsd->rpc_fs_payload;
+err:
+	thread_rpc_free_payload(c);
+	return NULL;
+}
diff --git a/core/tee/tee_obj.c b/core/tee/tee_obj.c
new file mode 100644
index 0000000..78cf937
--- /dev/null
+++ b/core/tee/tee_obj.c
@@ -0,0 +1,123 @@
+/*
+ * Copyright (c) 2014, STMicroelectronics International N.V.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <tee/tee_obj.h>
+
+#include <stdlib.h>
+#include <tee_api_defines.h>
+#include <mm/tee_mmu.h>
+#include <tee/tee_fs.h>
+#include <tee/tee_fs_defs.h>
+#include <tee/tee_pobj.h>
+#include <trace.h>
+#include <tee/tee_svc_storage.h>
+#include <tee/tee_svc_cryp.h>
+
+void tee_obj_add(struct user_ta_ctx *utc, struct tee_obj *o)
+{
+	TAILQ_INSERT_TAIL(&utc->objects, o, link);
+}
+
+TEE_Result tee_obj_get(struct user_ta_ctx *utc, uint32_t obj_id,
+		       struct tee_obj **obj)
+{
+	struct tee_obj *o;
+
+	TAILQ_FOREACH(o, &utc->objects, link) {
+		if (obj_id == (vaddr_t)o) {
+			*obj = o;
+			return TEE_SUCCESS;
+		}
+	}
+	return TEE_ERROR_BAD_PARAMETERS;
+}
+
+void tee_obj_close(struct user_ta_ctx *utc, struct tee_obj *o)
+{
+	TAILQ_REMOVE(&utc->objects, o, link);
+
+	if ((o->info.handleFlags & TEE_HANDLE_FLAG_PERSISTENT)) {
+		o->pobj->fops->close(&o->fh);
+		tee_pobj_release(o->pobj);
+	}
+
+	tee_obj_free(o);
+}
+
+void tee_obj_close_all(struct user_ta_ctx *utc)
+{
+	struct tee_obj_head *objects = &utc->objects;
+
+	while (!TAILQ_EMPTY(objects))
+		tee_obj_close(utc, TAILQ_FIRST(objects));
+}
+
+TEE_Result tee_obj_verify(struct tee_ta_session *sess, struct tee_obj *o)
+{
+	TEE_Result res;
+	char *file = NULL;
+	const struct tee_file_operations *fops = o->pobj->fops;
+	struct tee_file_handle *fh = NULL;
+
+	if (!fops)
+		return TEE_ERROR_STORAGE_NOT_AVAILABLE;
+
+	file = tee_svc_storage_create_filename(sess,
+					       o->pobj->obj_id,
+					       o->pobj->obj_id_len,
+					       false);
+	if (file == NULL) {
+		res = TEE_ERROR_OUT_OF_MEMORY;
+		goto exit;
+	}
+
+	res = fops->open(file, &fh);
+	if (res == TEE_ERROR_CORRUPT_OBJECT) {
+		EMSG("Object corrupt\n");
+		tee_obj_close(to_user_ta_ctx(sess->ctx), o);
+		fops->remove(file);
+	}
+
+	free(file);
+	fops->close(&fh);
+exit:
+	return res;
+}
+
+struct tee_obj *tee_obj_alloc(void)
+{
+	return calloc(1, sizeof(struct tee_obj));
+}
+
+void tee_obj_free(struct tee_obj *o)
+{
+	if (o) {
+		tee_obj_attr_free(o);
+		free(o->attr);
+		free(o);
+	}
+}
diff --git a/core/tee/tee_pobj.c b/core/tee/tee_pobj.c
new file mode 100644
index 0000000..a7aee31
--- /dev/null
+++ b/core/tee/tee_pobj.c
@@ -0,0 +1,190 @@
+/*
+ * Copyright (c) 2014, STMicroelectronics International N.V.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <kernel/mutex.h>
+#include <stdlib.h>
+#include <string.h>
+#include <tee/tee_pobj.h>
+#include <trace.h>
+
+static TAILQ_HEAD(tee_pobjs, tee_pobj) tee_pobjs =
+		TAILQ_HEAD_INITIALIZER(tee_pobjs);
+static struct mutex pobjs_mutex = MUTEX_INITIALIZER;
+
+static TEE_Result tee_pobj_check_access(uint32_t oflags, uint32_t nflags)
+{
+	/* meta is exclusive */
+	if ((oflags & TEE_DATA_FLAG_ACCESS_WRITE_META) ||
+	    (nflags & TEE_DATA_FLAG_ACCESS_WRITE_META))
+		return TEE_ERROR_ACCESS_CONFLICT;
+
+	/*
+	 * Excerpt of TEE Internal Core API Specification v1.1:
+	 * If more than one handle is opened on the same  object, and if any
+	 * of these object handles was opened with the flag
+	 * TEE_DATA_FLAG_ACCESS_READ, then all the object handles MUST have been
+	 * opened with the flag TEE_DATA_FLAG_SHARE_READ
+	 */
+	if (((oflags & TEE_DATA_FLAG_ACCESS_READ) ||
+	     (nflags & TEE_DATA_FLAG_ACCESS_READ)) &&
+	    !((nflags & TEE_DATA_FLAG_SHARE_READ) &&
+	      (oflags & TEE_DATA_FLAG_SHARE_READ)))
+		return TEE_ERROR_ACCESS_CONFLICT;
+
+	/*
+	 * Excerpt of TEE Internal Core API Specification v1.1:
+	 * An object can be opened with only share flags, which locks the access
+	 * to an object against a given mode.
+	 * An object can be opened with no flag set, which completely locks all
+	 * subsequent attempts to access the object
+	 */
+	if ((nflags & TEE_DATA_FLAG_SHARE_READ) !=
+	    (oflags & TEE_DATA_FLAG_SHARE_READ))
+		return TEE_ERROR_ACCESS_CONFLICT;
+
+	/* Same on WRITE access */
+	if (((oflags & TEE_DATA_FLAG_ACCESS_WRITE) ||
+	     (nflags & TEE_DATA_FLAG_ACCESS_WRITE)) &&
+	    !((nflags & TEE_DATA_FLAG_SHARE_WRITE) &&
+	      (oflags & TEE_DATA_FLAG_SHARE_WRITE)))
+		return TEE_ERROR_ACCESS_CONFLICT;
+	if ((nflags & TEE_DATA_FLAG_SHARE_WRITE) !=
+	    (oflags & TEE_DATA_FLAG_SHARE_WRITE))
+		return TEE_ERROR_ACCESS_CONFLICT;
+
+	return TEE_SUCCESS;
+}
+
+TEE_Result tee_pobj_get(TEE_UUID *uuid, void *obj_id, uint32_t obj_id_len,
+			uint32_t flags, const struct tee_file_operations *fops,
+			struct tee_pobj **obj)
+{
+	struct tee_pobj *o;
+	TEE_Result res;
+
+	*obj = NULL;
+
+	mutex_lock(&pobjs_mutex);
+	/* Check if file is open */
+	TAILQ_FOREACH(o, &tee_pobjs, link) {
+		if ((obj_id_len == o->obj_id_len) &&
+		    (memcmp(obj_id, o->obj_id, obj_id_len) == 0) &&
+		    (memcmp(uuid, &o->uuid, sizeof(TEE_UUID)) == 0) &&
+		    (fops == o->fops)) {
+			*obj = o;
+		}
+	}
+
+	if (*obj) {
+		res = tee_pobj_check_access((*obj)->flags, flags);
+		if (res != TEE_SUCCESS)
+			*obj = NULL;
+		else
+			(*obj)->refcnt++;
+		goto out;
+	}
+
+	/* new file */
+	o = calloc(sizeof(struct tee_pobj), 1);
+	if (!o) {
+		res = TEE_ERROR_OUT_OF_MEMORY;
+		goto out;
+	}
+
+	o->refcnt = 1;
+	memcpy(&o->uuid, uuid, sizeof(TEE_UUID));
+	o->flags = flags;
+	o->fops = fops;
+
+	o->obj_id = malloc(obj_id_len);
+	if (o->obj_id == NULL) {
+		free(o);
+		res = TEE_ERROR_OUT_OF_MEMORY;
+		goto out;
+	}
+	memcpy(o->obj_id, obj_id, obj_id_len);
+	o->obj_id_len = obj_id_len;
+
+	TAILQ_INSERT_TAIL(&tee_pobjs, o, link);
+	*obj = o;
+
+	res = TEE_SUCCESS;
+out:
+	mutex_unlock(&pobjs_mutex);
+	return res;
+}
+
+TEE_Result tee_pobj_release(struct tee_pobj *obj)
+{
+	if (obj == NULL)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	mutex_lock(&pobjs_mutex);
+	obj->refcnt--;
+	if (obj->refcnt == 0) {
+		TAILQ_REMOVE(&tee_pobjs, obj, link);
+		free(obj->obj_id);
+		free(obj);
+	}
+	mutex_unlock(&pobjs_mutex);
+
+	return TEE_SUCCESS;
+}
+
+TEE_Result tee_pobj_rename(struct tee_pobj *obj, void *obj_id,
+			   uint32_t obj_id_len)
+{
+	TEE_Result res = TEE_SUCCESS;
+	void *new_obj_id = NULL;
+
+	if (obj == NULL || obj_id == NULL)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	mutex_lock(&pobjs_mutex);
+	if (obj->refcnt != 1) {
+		res = TEE_ERROR_BAD_STATE;
+		goto exit;
+	}
+
+	new_obj_id = malloc(obj_id_len);
+	if (new_obj_id == NULL) {
+		res = TEE_ERROR_OUT_OF_MEMORY;
+		goto exit;
+	}
+	memcpy(new_obj_id, obj_id, obj_id_len);
+
+	/* update internal data */
+	free(obj->obj_id);
+	obj->obj_id = new_obj_id;
+	obj->obj_id_len = obj_id_len;
+	new_obj_id = NULL;
+
+exit:
+	mutex_unlock(&pobjs_mutex);
+	free(new_obj_id);
+	return res;
+}
diff --git a/core/tee/tee_ree_fs.c b/core/tee/tee_ree_fs.c
new file mode 100644
index 0000000..7a82acb
--- /dev/null
+++ b/core/tee/tee_ree_fs.c
@@ -0,0 +1,792 @@
+/*
+ * Copyright (c) 2015, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <assert.h>
+#include <kernel/thread.h>
+#include <kernel/mutex.h>
+#include <kernel/panic.h>
+#include <mm/core_memprot.h>
+#include <optee_msg_supplicant.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <string_ext.h>
+#include <sys/queue.h>
+#include <tee/tee_cryp_provider.h>
+#include <tee/tee_fs.h>
+#include <tee/tee_fs_defs.h>
+#include <tee/tee_fs_rpc.h>
+#include <tee/tee_fs_key_manager.h>
+#include <trace.h>
+#include <utee_defines.h>
+#include <util.h>
+
+/*
+ * This file implements the tee_file_operations structure for a secure
+ * filesystem based on single file in normal world.
+ *
+ * All fields in the REE file are duplicated with two versions 0 and 1. The
+ * active meta-data block is selected by the lowest bit in the
+ * meta-counter.  The active file block is selected by corresponding bit
+ * number in struct tee_fs_file_info.backup_version_table.
+ *
+ * The atomicity of each operation is ensured by updating meta-counter when
+ * everything in the secondary blocks (both meta-data and file-data blocks)
+ * are successfully written.  The main purpose of the code below is to
+ * perform block encryption and authentication of the file data, and
+ * properly handle seeking through the file. One file (in the sense of
+ * struct tee_file_operations) maps to one file in the REE filesystem, and
+ * has the following structure:
+ *
+ * [ 4 bytes meta-counter]
+ * [ meta-data version 0][ meta-data version 1 ]
+ * [ Block 0 version 0 ][ Block 0 version 1 ]
+ * [ Block 1 version 0 ][ Block 1 version 1 ]
+ * ...
+ * [ Block n version 0 ][ Block n version 1 ]
+ *
+ * One meta-data block is built up as:
+ * [ struct meta_header | struct tee_fs_get_header_size ]
+ *
+ * One data block is built up as:
+ * [ struct block_header | BLOCK_FILE_SIZE bytes ]
+ *
+ * struct meta_header and struct block_header are defined in
+ * tee_fs_key_manager.h.
+ *
+ */
+
+#define BLOCK_SHIFT	12
+
+#define BLOCK_SIZE	(1 << BLOCK_SHIFT)
+
+#define MAX_FILE_SIZE	(BLOCK_SIZE * NUM_BLOCKS_PER_FILE)
+
+struct tee_fs_fd {
+	uint32_t meta_counter;
+	struct tee_fs_file_meta meta;
+	tee_fs_off_t pos;
+	uint32_t flags;
+	bool is_new_file;
+	int fd;
+};
+
+static inline int pos_to_block_num(int position)
+{
+	return position >> BLOCK_SHIFT;
+}
+
+static inline int get_last_block_num(size_t size)
+{
+	return pos_to_block_num(size - 1);
+}
+
+static bool get_backup_version_of_block(struct tee_fs_file_meta *meta,
+					size_t block_num)
+{
+	uint32_t index = (block_num / 32);
+	uint32_t block_mask = 1 << (block_num % 32);
+
+	return !!(meta->info.backup_version_table[index] & block_mask);
+}
+
+static inline void toggle_backup_version_of_block(
+		struct tee_fs_file_meta *meta,
+		size_t block_num)
+{
+	uint32_t index = (block_num / 32);
+	uint32_t block_mask = 1 << (block_num % 32);
+
+	meta->info.backup_version_table[index] ^= block_mask;
+}
+
+struct block_operations {
+
+	/*
+	 * Read a block from REE File System which is corresponding
+	 * to the given block_num.
+	 */
+	struct block *(*read)(struct tee_fs_fd *fdp, int block_num);
+
+	/*
+	 * Write the given block to REE File System
+	 */
+	int (*write)(struct tee_fs_fd *fdp, struct block *b,
+			struct tee_fs_file_meta *new_meta);
+};
+
+static struct mutex ree_fs_mutex = MUTEX_INITIALIZER;
+
+static TEE_Result ree_fs_opendir_rpc(const char *name, struct tee_fs_dir **d)
+
+{
+	return tee_fs_rpc_opendir(OPTEE_MSG_RPC_CMD_FS, name, d);
+}
+
+static void ree_fs_closedir_rpc(struct tee_fs_dir *d)
+{
+	if (d)
+		tee_fs_rpc_closedir(OPTEE_MSG_RPC_CMD_FS, d);
+}
+
+static TEE_Result ree_fs_readdir_rpc(struct tee_fs_dir *d,
+				     struct tee_fs_dirent **ent)
+{
+	return tee_fs_rpc_readdir(OPTEE_MSG_RPC_CMD_FS, d, ent);
+}
+
+static size_t meta_size(void)
+{
+	return tee_fs_get_header_size(META_FILE) +
+	       sizeof(struct tee_fs_file_meta);
+}
+
+static size_t meta_pos_raw(struct tee_fs_fd *fdp, bool active)
+{
+	size_t offs = sizeof(uint32_t);
+
+	if ((fdp->meta_counter & 1) == active)
+		offs += meta_size();
+	return offs;
+}
+
+static size_t block_size_raw(void)
+{
+	return tee_fs_get_header_size(BLOCK_FILE) + BLOCK_SIZE;
+}
+
+static size_t block_pos_raw(struct tee_fs_file_meta *meta, size_t block_num,
+			    bool active)
+{
+	size_t n = block_num * 2;
+
+	if (active == get_backup_version_of_block(meta, block_num))
+		n++;
+
+	return sizeof(uint32_t) + meta_size() * 2 + n * block_size_raw();
+}
+
+/*
+ * encrypted_fek: as input for META_FILE and BLOCK_FILE
+ */
+static TEE_Result encrypt_and_write_file(struct tee_fs_fd *fdp,
+		enum tee_fs_file_type file_type, size_t offs,
+		void *data_in, size_t data_in_size,
+		uint8_t *encrypted_fek)
+{
+	TEE_Result res;
+	struct tee_fs_rpc_operation op;
+	void *ciphertext;
+	size_t header_size = tee_fs_get_header_size(file_type);
+	size_t ciphertext_size = header_size + data_in_size;
+
+
+	res = tee_fs_rpc_write_init(&op, OPTEE_MSG_RPC_CMD_FS, fdp->fd,
+				    offs, ciphertext_size, &ciphertext);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = tee_fs_encrypt_file(file_type, data_in, data_in_size,
+				  ciphertext, &ciphertext_size, encrypted_fek);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	return tee_fs_rpc_write_final(&op);
+}
+
+/*
+ * encrypted_fek: as output for META_FILE
+ *                as input for BLOCK_FILE
+ */
+static TEE_Result read_and_decrypt_file(struct tee_fs_fd *fdp,
+		enum tee_fs_file_type file_type, size_t offs,
+		void *data_out, size_t *data_out_size,
+		uint8_t *encrypted_fek)
+{
+	TEE_Result res;
+	struct tee_fs_rpc_operation op;
+	size_t bytes;
+	void *ciphertext;
+
+	bytes = *data_out_size + tee_fs_get_header_size(file_type);
+	res = tee_fs_rpc_read_init(&op, OPTEE_MSG_RPC_CMD_FS, fdp->fd, offs,
+				   bytes, &ciphertext);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = tee_fs_rpc_read_final(&op, &bytes);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	if (!bytes) {
+		*data_out_size = 0;
+		return TEE_SUCCESS;
+	}
+
+	res = tee_fs_decrypt_file(file_type, ciphertext, bytes, data_out,
+				  data_out_size, encrypted_fek);
+	if (res != TEE_SUCCESS)
+		return TEE_ERROR_CORRUPT_OBJECT;
+	return TEE_SUCCESS;
+}
+
+static TEE_Result write_meta_file(struct tee_fs_fd *fdp,
+		struct tee_fs_file_meta *meta)
+{
+	size_t offs = meta_pos_raw(fdp, false);
+
+	return encrypt_and_write_file(fdp, META_FILE, offs,
+			(void *)&meta->info, sizeof(meta->info),
+			meta->encrypted_fek);
+}
+
+static TEE_Result write_meta_counter(struct tee_fs_fd *fdp)
+{
+	TEE_Result res;
+	struct tee_fs_rpc_operation op;
+	size_t bytes = sizeof(uint32_t);
+	void *data;
+
+	res = tee_fs_rpc_write_init(&op, OPTEE_MSG_RPC_CMD_FS, fdp->fd, 0,
+				    bytes, &data);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	memcpy(data, &fdp->meta_counter, bytes);
+
+	return tee_fs_rpc_write_final(&op);
+}
+
+static TEE_Result create_meta(struct tee_fs_fd *fdp, const char *fname)
+{
+	TEE_Result res;
+
+	memset(fdp->meta.info.backup_version_table, 0xff,
+		sizeof(fdp->meta.info.backup_version_table));
+	fdp->meta.info.length = 0;
+
+	res = tee_fs_generate_fek(fdp->meta.encrypted_fek, TEE_FS_KM_FEK_SIZE);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = tee_fs_rpc_create(OPTEE_MSG_RPC_CMD_FS, fname, &fdp->fd);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	fdp->meta.counter = fdp->meta_counter;
+
+	res = write_meta_file(fdp, &fdp->meta);
+	if (res != TEE_SUCCESS)
+		return res;
+	return write_meta_counter(fdp);
+}
+
+static TEE_Result commit_meta_file(struct tee_fs_fd *fdp,
+				   struct tee_fs_file_meta *new_meta)
+{
+	TEE_Result res;
+
+	new_meta->counter = fdp->meta_counter + 1;
+
+	res = write_meta_file(fdp, new_meta);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	/*
+	 * From now on the new meta is successfully committed,
+	 * change tee_fs_fd accordingly
+	 */
+	fdp->meta = *new_meta;
+	fdp->meta_counter = fdp->meta.counter;
+
+	return write_meta_counter(fdp);
+}
+
+static TEE_Result read_meta_file(struct tee_fs_fd *fdp,
+		struct tee_fs_file_meta *meta)
+{
+	size_t meta_info_size = sizeof(struct tee_fs_file_info);
+	size_t offs = meta_pos_raw(fdp, true);
+
+	return read_and_decrypt_file(fdp, META_FILE, offs,
+				     &meta->info, &meta_info_size,
+				     meta->encrypted_fek);
+}
+
+static TEE_Result read_meta_counter(struct tee_fs_fd *fdp)
+{
+	TEE_Result res;
+	struct tee_fs_rpc_operation op;
+	void *data;
+	size_t bytes = sizeof(uint32_t);
+
+	res = tee_fs_rpc_read_init(&op, OPTEE_MSG_RPC_CMD_FS, fdp->fd, 0,
+				   bytes, &data);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = tee_fs_rpc_read_final(&op, &bytes);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	if (bytes != sizeof(uint32_t))
+		return TEE_ERROR_CORRUPT_OBJECT;
+
+	memcpy(&fdp->meta_counter, data, bytes);
+
+	return TEE_SUCCESS;
+}
+
+static TEE_Result read_meta(struct tee_fs_fd *fdp, const char *fname)
+{
+	TEE_Result res;
+
+	res = tee_fs_rpc_open(OPTEE_MSG_RPC_CMD_FS, fname, &fdp->fd);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = read_meta_counter(fdp);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	return read_meta_file(fdp, &fdp->meta);
+}
+
+static TEE_Result read_block(struct tee_fs_fd *fdp, int bnum, uint8_t *data)
+{
+	TEE_Result res;
+	size_t ct_size = block_size_raw();
+	size_t out_size = BLOCK_SIZE;
+	ssize_t pos = block_pos_raw(&fdp->meta, bnum, true);
+	size_t bytes;
+	void *ct;
+	struct tee_fs_rpc_operation op;
+
+	res = tee_fs_rpc_read_init(&op, OPTEE_MSG_RPC_CMD_FS, fdp->fd, pos,
+				   ct_size, &ct);
+	if (res != TEE_SUCCESS)
+		return res;
+	res = tee_fs_rpc_read_final(&op, &bytes);
+	if (res != TEE_SUCCESS)
+		return res;
+	if (!bytes) {
+		memset(data, 0, BLOCK_SIZE);
+		return TEE_SUCCESS; /* Block does not exist */
+	}
+
+	return tee_fs_decrypt_file(BLOCK_FILE, ct, bytes, data,
+				   &out_size, fdp->meta.encrypted_fek);
+}
+
+static TEE_Result write_block(struct tee_fs_fd *fdp, size_t bnum, uint8_t *data,
+			      struct tee_fs_file_meta *new_meta)
+{
+	TEE_Result res;
+	size_t offs = block_pos_raw(new_meta, bnum, false);
+
+	res = encrypt_and_write_file(fdp, BLOCK_FILE, offs, data,
+				     BLOCK_SIZE, new_meta->encrypted_fek);
+	if (res == TEE_SUCCESS)
+		toggle_backup_version_of_block(new_meta, bnum);
+	return res;
+}
+
+static TEE_Result out_of_place_write(struct tee_fs_fd *fdp, const void *buf,
+		size_t len, struct tee_fs_file_meta *new_meta)
+{
+	TEE_Result res;
+	int start_block_num = pos_to_block_num(fdp->pos);
+	int end_block_num = pos_to_block_num(fdp->pos + len - 1);
+	size_t remain_bytes = len;
+	uint8_t *data_ptr = (uint8_t *)buf;
+	uint8_t *block;
+	int orig_pos = fdp->pos;
+
+	block = malloc(BLOCK_SIZE);
+	if (!block)
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	while (start_block_num <= end_block_num) {
+		int offset = fdp->pos % BLOCK_SIZE;
+		size_t size_to_write = MIN(remain_bytes, (size_t)BLOCK_SIZE);
+
+		if (size_to_write + offset > BLOCK_SIZE)
+			size_to_write = BLOCK_SIZE - offset;
+
+		res = read_block(fdp, start_block_num, block);
+		if (res == TEE_ERROR_ITEM_NOT_FOUND)
+			memset(block, 0, BLOCK_SIZE);
+		else if (res != TEE_SUCCESS)
+			goto exit;
+
+		if (data_ptr)
+			memcpy(block + offset, data_ptr, size_to_write);
+		else
+			memset(block + offset, 0, size_to_write);
+
+		res = write_block(fdp, start_block_num, block, new_meta);
+		if (res != TEE_SUCCESS)
+			goto exit;
+
+		if (data_ptr)
+			data_ptr += size_to_write;
+		remain_bytes -= size_to_write;
+		start_block_num++;
+		fdp->pos += size_to_write;
+	}
+
+	if (fdp->pos > (tee_fs_off_t)new_meta->info.length)
+		new_meta->info.length = fdp->pos;
+
+exit:
+	free(block);
+	if (res != TEE_SUCCESS)
+		fdp->pos = orig_pos;
+	return res;
+}
+
+static TEE_Result open_internal(const char *file, bool create,
+				struct tee_file_handle **fh)
+{
+	TEE_Result res;
+	size_t len;
+	struct tee_fs_fd *fdp = NULL;
+
+	if (!file)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	len = strlen(file) + 1;
+	if (len > TEE_FS_NAME_MAX)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	fdp = calloc(1, sizeof(struct tee_fs_fd));
+	if (!fdp)
+		return TEE_ERROR_OUT_OF_MEMORY;
+	fdp->fd = -1;
+
+	mutex_lock(&ree_fs_mutex);
+
+	if (create)
+		res = create_meta(fdp, file);
+	else
+		res = read_meta(fdp, file);
+
+	if (res == TEE_SUCCESS) {
+		*fh = (struct tee_file_handle *)fdp;
+	} else {
+		if (fdp->fd != -1)
+			tee_fs_rpc_close(OPTEE_MSG_RPC_CMD_FS, fdp->fd);
+		if (create)
+			tee_fs_rpc_remove(OPTEE_MSG_RPC_CMD_FS, file);
+		free(fdp);
+	}
+
+	mutex_unlock(&ree_fs_mutex);
+	return res;
+}
+
+static TEE_Result ree_fs_open(const char *file, struct tee_file_handle **fh)
+{
+	return open_internal(file, false, fh);
+}
+
+static TEE_Result ree_fs_create(const char *file, struct tee_file_handle **fh)
+{
+	return open_internal(file, true, fh);
+}
+
+static void ree_fs_close(struct tee_file_handle **fh)
+{
+	struct tee_fs_fd *fdp = (struct tee_fs_fd *)*fh;
+
+	if (fdp) {
+		tee_fs_rpc_close(OPTEE_MSG_RPC_CMD_FS, fdp->fd);
+		free(fdp);
+		*fh = NULL;
+	}
+}
+
+static TEE_Result ree_fs_seek(struct tee_file_handle *fh, int32_t offset,
+			      TEE_Whence whence, int32_t *new_offs)
+{
+	TEE_Result res;
+	tee_fs_off_t new_pos;
+	size_t filelen;
+	struct tee_fs_fd *fdp = (struct tee_fs_fd *)fh;
+
+	mutex_lock(&ree_fs_mutex);
+
+	DMSG("offset=%d, whence=%d", (int)offset, whence);
+
+	filelen = fdp->meta.info.length;
+
+	switch (whence) {
+	case TEE_DATA_SEEK_SET:
+		new_pos = offset;
+		break;
+
+	case TEE_DATA_SEEK_CUR:
+		new_pos = fdp->pos + offset;
+		break;
+
+	case TEE_DATA_SEEK_END:
+		new_pos = filelen + offset;
+		break;
+
+	default:
+		res = TEE_ERROR_BAD_PARAMETERS;
+		goto exit;
+	}
+
+	if (new_pos < 0)
+		new_pos = 0;
+
+	if (new_pos > TEE_DATA_MAX_POSITION) {
+		EMSG("Position is beyond TEE_DATA_MAX_POSITION");
+		res = TEE_ERROR_BAD_PARAMETERS;
+		goto exit;
+	}
+
+	fdp->pos = new_pos;
+	if (new_offs)
+		*new_offs = new_pos;
+	res = TEE_SUCCESS;
+exit:
+	mutex_unlock(&ree_fs_mutex);
+	return res;
+}
+
+/*
+ * To ensure atomic truncate operation, we can:
+ *
+ *  - update file length to new length
+ *  - commit new meta
+ *
+ * To ensure atomic extend operation, we can:
+ *
+ *  - update file length to new length
+ *  - allocate and fill zero data to new blocks
+ *  - commit new meta
+ *
+ * Any failure before committing new meta is considered as
+ * update failed, and the file content will not be updated
+ */
+static TEE_Result ree_fs_ftruncate_internal(struct tee_fs_fd *fdp,
+					    tee_fs_off_t new_file_len)
+{
+	TEE_Result res;
+	size_t old_file_len = fdp->meta.info.length;
+	struct tee_fs_file_meta new_meta;
+
+	if (new_file_len > MAX_FILE_SIZE)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	new_meta = fdp->meta;
+	new_meta.info.length = new_file_len;
+
+	if ((size_t)new_file_len > old_file_len) {
+		size_t ext_len = new_file_len - old_file_len;
+		int orig_pos = fdp->pos;
+
+		fdp->pos = old_file_len;
+		res = out_of_place_write(fdp, NULL, ext_len, &new_meta);
+		fdp->pos = orig_pos;
+		if (res != TEE_SUCCESS)
+			return res;
+	}
+
+	return commit_meta_file(fdp, &new_meta);
+}
+
+static TEE_Result ree_fs_read(struct tee_file_handle *fh, void *buf,
+			      size_t *len)
+{
+	TEE_Result res;
+	int start_block_num;
+	int end_block_num;
+	size_t remain_bytes;
+	uint8_t *data_ptr = buf;
+	uint8_t *block = NULL;
+	struct tee_fs_fd *fdp = (struct tee_fs_fd *)fh;
+
+	mutex_lock(&ree_fs_mutex);
+
+	remain_bytes = *len;
+	if ((fdp->pos + remain_bytes) < remain_bytes ||
+	    fdp->pos > (tee_fs_off_t)fdp->meta.info.length)
+		remain_bytes = 0;
+	else if (fdp->pos + (tee_fs_off_t)remain_bytes >
+		(tee_fs_off_t)fdp->meta.info.length)
+		remain_bytes = fdp->meta.info.length - fdp->pos;
+
+	*len = remain_bytes;
+
+	if (!remain_bytes) {
+		res = TEE_SUCCESS;
+		goto exit;
+	}
+
+	start_block_num = pos_to_block_num(fdp->pos);
+	end_block_num = pos_to_block_num(fdp->pos + remain_bytes - 1);
+
+	block = malloc(BLOCK_SIZE);
+	if (!block) {
+		res = TEE_ERROR_OUT_OF_MEMORY;
+		goto exit;
+	}
+
+	while (start_block_num <= end_block_num) {
+		tee_fs_off_t offset = fdp->pos % BLOCK_SIZE;
+		size_t size_to_read = MIN(remain_bytes, (size_t)BLOCK_SIZE);
+
+		if (size_to_read + offset > BLOCK_SIZE)
+			size_to_read = BLOCK_SIZE - offset;
+
+		res = read_block(fdp, start_block_num, block);
+		if (res != TEE_SUCCESS) {
+			if (res == TEE_ERROR_MAC_INVALID)
+				res = TEE_ERROR_CORRUPT_OBJECT;
+			goto exit;
+		}
+
+		memcpy(data_ptr, block + offset, size_to_read);
+
+		data_ptr += size_to_read;
+		remain_bytes -= size_to_read;
+		fdp->pos += size_to_read;
+
+		start_block_num++;
+	}
+	res = TEE_SUCCESS;
+exit:
+	mutex_unlock(&ree_fs_mutex);
+	free(block);
+	return res;
+}
+
+/*
+ * To ensure atomicity of write operation, we need to
+ * do the following steps:
+ * (The sequence of operations is very important)
+ *
+ *  - Create a new backup version of meta file as a copy
+ *    of current meta file.
+ *  - For each blocks to write:
+ *    - Create new backup version for current block.
+ *    - Write data to new backup version.
+ *    - Update the new meta file accordingly.
+ *  - Write the new meta file.
+ *
+ * (Any failure in above steps is considered as update failed,
+ *  and the file content will not be updated)
+ */
+static TEE_Result ree_fs_write(struct tee_file_handle *fh, const void *buf,
+			       size_t len)
+{
+	TEE_Result res;
+	struct tee_fs_file_meta new_meta;
+	struct tee_fs_fd *fdp = (struct tee_fs_fd *)fh;
+	size_t file_size;
+
+	if (!len)
+		return TEE_SUCCESS;
+
+	mutex_lock(&ree_fs_mutex);
+
+	file_size = fdp->meta.info.length;
+
+	if ((fdp->pos + len) > MAX_FILE_SIZE || (fdp->pos + len) < len) {
+		res = TEE_ERROR_BAD_PARAMETERS;
+		goto exit;
+	}
+
+	if (file_size < (size_t)fdp->pos) {
+		res = ree_fs_ftruncate_internal(fdp, fdp->pos);
+		if (res != TEE_SUCCESS)
+			goto exit;
+	}
+
+	new_meta = fdp->meta;
+	res = out_of_place_write(fdp, buf, len, &new_meta);
+	if (res != TEE_SUCCESS)
+		goto exit;
+
+	res = commit_meta_file(fdp, &new_meta);
+exit:
+	mutex_unlock(&ree_fs_mutex);
+	return res;
+}
+
+static TEE_Result ree_fs_rename(const char *old, const char *new,
+				bool overwrite)
+{
+	TEE_Result res;
+
+	mutex_lock(&ree_fs_mutex);
+	res = tee_fs_rpc_rename(OPTEE_MSG_RPC_CMD_FS, old, new, overwrite);
+	mutex_unlock(&ree_fs_mutex);
+
+	return res;
+}
+
+static TEE_Result ree_fs_remove(const char *file)
+{
+	TEE_Result res;
+
+	mutex_lock(&ree_fs_mutex);
+	res = tee_fs_rpc_remove(OPTEE_MSG_RPC_CMD_FS, file);
+	mutex_unlock(&ree_fs_mutex);
+
+	return res;
+}
+
+static TEE_Result ree_fs_truncate(struct tee_file_handle *fh, size_t len)
+{
+	TEE_Result res;
+	struct tee_fs_fd *fdp = (struct tee_fs_fd *)fh;
+
+	mutex_lock(&ree_fs_mutex);
+	res = ree_fs_ftruncate_internal(fdp, len);
+	mutex_unlock(&ree_fs_mutex);
+
+	return res;
+}
+
+const struct tee_file_operations ree_fs_ops = {
+	.open = ree_fs_open,
+	.create = ree_fs_create,
+	.close = ree_fs_close,
+	.read = ree_fs_read,
+	.write = ree_fs_write,
+	.seek = ree_fs_seek,
+	.truncate = ree_fs_truncate,
+	.rename = ree_fs_rename,
+	.remove = ree_fs_remove,
+	.opendir = ree_fs_opendir_rpc,
+	.closedir = ree_fs_closedir_rpc,
+	.readdir = ree_fs_readdir_rpc,
+};
diff --git a/core/tee/tee_rpmb_fs.c b/core/tee/tee_rpmb_fs.c
new file mode 100644
index 0000000..229bce7
--- /dev/null
+++ b/core/tee/tee_rpmb_fs.c
@@ -0,0 +1,2617 @@
+/*
+ * Copyright (c) 2014, STMicroelectronics International N.V.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <assert.h>
+#include <kernel/tee_common.h>
+#include <kernel/mutex.h>
+#include <kernel/panic.h>
+#include <kernel/tee_common_otp.h>
+#include <kernel/thread.h>
+#include <mm/core_memprot.h>
+#include <mm/tee_mm.h>
+#include <optee_msg_supplicant.h>
+#include <stdlib.h>
+#include <string.h>
+#include <string_ext.h>
+#include <sys/queue.h>
+#include <tee/tee_cryp_provider.h>
+#include <tee/tee_fs_defs.h>
+#include <tee/tee_fs.h>
+#include <tee/tee_fs_key_manager.h>
+#include <trace.h>
+#include <util.h>
+
+#define RPMB_STORAGE_START_ADDRESS      0
+#define RPMB_FS_FAT_START_ADDRESS       512
+#define RPMB_BLOCK_SIZE_SHIFT           8
+
+#define RPMB_FS_MAGIC                   0x52504D42
+#define FS_VERSION                      2
+#define N_ENTRIES                       8
+
+#define FILE_IS_ACTIVE                  (1u << 0)
+#define FILE_IS_LAST_ENTRY              (1u << 1)
+
+#define TEE_RPMB_FS_FILENAME_LENGTH 224
+
+/**
+ * FS parameters: Information often used by internal functions.
+ * fat_start_address will be set by rpmb_fs_setup().
+ * rpmb_fs_parameters can be read by any other function.
+ */
+struct rpmb_fs_parameters {
+	uint32_t fat_start_address;
+	uint32_t max_rpmb_address;
+};
+
+/**
+ * File entry for a single file in a RPMB_FS partition.
+ */
+struct rpmb_fat_entry {
+	uint32_t start_address;
+	uint32_t data_size;
+	uint32_t flags;
+	uint32_t write_counter;
+	uint8_t fek[TEE_FS_KM_FEK_SIZE];
+	char filename[TEE_RPMB_FS_FILENAME_LENGTH];
+};
+
+/**
+ * FAT entry context with reference to a FAT entry and its
+ * location in RPMB.
+ */
+struct rpmb_file_handle {
+	struct rpmb_fat_entry fat_entry;
+	char filename[TEE_RPMB_FS_FILENAME_LENGTH];
+	/* Address for current entry in RPMB */
+	uint32_t rpmb_fat_address;
+	/* Current position */
+	uint32_t pos;
+};
+
+/**
+ * RPMB_FS partition data
+ */
+struct rpmb_fs_partition {
+	uint32_t rpmb_fs_magic;
+	uint32_t fs_version;
+	uint32_t write_counter;
+	uint32_t fat_start_address;
+	/* Do not use reserved[] for other purpose than partition data. */
+	uint8_t reserved[112];
+};
+
+/**
+ * A node in a list of directory entries. entry->name is a
+ * pointer to name here.
+ */
+struct tee_rpmb_fs_dirent {
+	struct tee_fs_dirent entry;
+	char name[TEE_RPMB_FS_FILENAME_LENGTH];
+	/* */
+	SIMPLEQ_ENTRY(tee_rpmb_fs_dirent) link;
+};
+
+/**
+ * The RPMB directory representation. It contains a queue of
+ * RPMB directory entries: 'next'.
+ * The current pointer points to the last directory entry
+ * returned by readdir().
+ */
+struct tee_fs_dir {
+	struct tee_rpmb_fs_dirent *current;
+	/* */
+	SIMPLEQ_HEAD(next_head, tee_rpmb_fs_dirent) next;
+};
+
+static struct rpmb_fs_parameters *fs_par;
+
+/*
+ * Lower interface to RPMB device
+ */
+
+#define RPMB_DATA_OFFSET            (RPMB_STUFF_DATA_SIZE + RPMB_KEY_MAC_SIZE)
+#define RPMB_MAC_PROTECT_DATA_SIZE  (RPMB_DATA_FRAME_SIZE - RPMB_DATA_OFFSET)
+
+#define RPMB_MSG_TYPE_REQ_AUTH_KEY_PROGRAM          0x0001
+#define RPMB_MSG_TYPE_REQ_WRITE_COUNTER_VAL_READ    0x0002
+#define RPMB_MSG_TYPE_REQ_AUTH_DATA_WRITE           0x0003
+#define RPMB_MSG_TYPE_REQ_AUTH_DATA_READ            0x0004
+#define RPMB_MSG_TYPE_REQ_RESULT_READ               0x0005
+#define RPMB_MSG_TYPE_RESP_AUTH_KEY_PROGRAM         0x0100
+#define RPMB_MSG_TYPE_RESP_WRITE_COUNTER_VAL_READ   0x0200
+#define RPMB_MSG_TYPE_RESP_AUTH_DATA_WRITE          0x0300
+#define RPMB_MSG_TYPE_RESP_AUTH_DATA_READ           0x0400
+
+#define RPMB_STUFF_DATA_SIZE                        196
+#define RPMB_KEY_MAC_SIZE                           32
+#define RPMB_DATA_SIZE                              256
+#define RPMB_NONCE_SIZE                             16
+#define RPMB_DATA_FRAME_SIZE                        512
+
+#define RPMB_RESULT_OK                              0x00
+#define RPMB_RESULT_GENERAL_FAILURE                 0x01
+#define RPMB_RESULT_AUTH_FAILURE                    0x02
+#define RPMB_RESULT_COUNTER_FAILURE                 0x03
+#define RPMB_RESULT_ADDRESS_FAILURE                 0x04
+#define RPMB_RESULT_WRITE_FAILURE                   0x05
+#define RPMB_RESULT_READ_FAILURE                    0x06
+#define RPMB_RESULT_AUTH_KEY_NOT_PROGRAMMED         0x07
+#define RPMB_RESULT_MASK                            0x3F
+#define RPMB_RESULT_WR_CNT_EXPIRED                  0x80
+
+/* RPMB internal commands */
+#define RPMB_CMD_DATA_REQ      0x00
+#define RPMB_CMD_GET_DEV_INFO  0x01
+
+#define RPMB_SIZE_SINGLE (128 * 1024)
+
+/* Error codes for get_dev_info request/response. */
+#define RPMB_CMD_GET_DEV_INFO_RET_OK     0x00
+#define RPMB_CMD_GET_DEV_INFO_RET_ERROR  0x01
+
+struct rpmb_data_frame {
+	uint8_t stuff_bytes[RPMB_STUFF_DATA_SIZE];
+	uint8_t key_mac[RPMB_KEY_MAC_SIZE];
+	uint8_t data[RPMB_DATA_SIZE];
+	uint8_t nonce[RPMB_NONCE_SIZE];
+	uint8_t write_counter[4];
+	uint8_t address[2];
+	uint8_t block_count[2];
+	uint8_t op_result[2];
+	uint8_t msg_type[2];
+};
+
+struct rpmb_req {
+	uint16_t cmd;
+	uint16_t dev_id;
+	uint16_t block_count;
+	/* variable length of data */
+	/* uint8_t data[]; REMOVED! */
+};
+
+#define TEE_RPMB_REQ_DATA(req) \
+		((void *)((struct rpmb_req *)(req) + 1))
+
+struct rpmb_raw_data {
+	uint16_t msg_type;
+	uint16_t *op_result;
+	uint16_t *block_count;
+	uint16_t *blk_idx;
+	uint32_t *write_counter;
+	uint8_t *nonce;
+	uint8_t *key_mac;
+	uint8_t *data;
+	/* data length to read or write */
+	uint32_t len;
+	/* Byte address offset in the first block involved */
+	uint8_t byte_offset;
+};
+
+#define RPMB_EMMC_CID_SIZE 16
+struct rpmb_dev_info {
+	uint8_t cid[RPMB_EMMC_CID_SIZE];
+	/* EXT CSD-slice 168 "RPMB Size" */
+	uint8_t rpmb_size_mult;
+	/* EXT CSD-slice 222 "Reliable Write Sector Count" */
+	uint8_t rel_wr_sec_c;
+	/* Check the ret code and accept the data only if it is OK. */
+	uint8_t ret_code;
+};
+
+/*
+ * Struct for rpmb context data.
+ *
+ * @key              RPMB key.
+ * @cid              eMMC card ID.
+ * @hash_ctx_size    Hash context size
+ * @wr_cnt           Current write counter.
+ * @max_blk_idx      The highest block index supported by current device.
+ * @rel_wr_blkcnt    Max number of data blocks for each reliable write.
+ * @dev_id           Device ID of the eMMC device.
+ * @wr_cnt_synced    Flag indicating if write counter is synced to RPMB.
+ * @key_derived      Flag indicating if key has been generated.
+ * @key_verified     Flag indicating the key generated is verified ok.
+ * @dev_info_synced  Flag indicating if dev info has been retrieved from RPMB.
+ */
+struct tee_rpmb_ctx {
+	uint8_t key[RPMB_KEY_MAC_SIZE];
+	uint8_t cid[RPMB_EMMC_CID_SIZE];
+	size_t hash_ctx_size;
+	uint32_t wr_cnt;
+	uint16_t max_blk_idx;
+	uint16_t rel_wr_blkcnt;
+	uint16_t dev_id;
+	bool wr_cnt_synced;
+	bool key_derived;
+	bool key_verified;
+	bool dev_info_synced;
+};
+
+static struct tee_rpmb_ctx *rpmb_ctx;
+
+/*
+ * Mutex to serialize the operations exported by this file.
+ * It protects rpmb_ctx and prevents overlapping operations on eMMC devices with
+ * different IDs.
+ */
+static struct mutex rpmb_mutex = MUTEX_INITIALIZER;
+
+#ifdef CFG_RPMB_TESTKEY
+
+static const uint8_t rpmb_test_key[RPMB_KEY_MAC_SIZE] = {
+	0xD3, 0xEB, 0x3E, 0xC3, 0x6E, 0x33, 0x4C, 0x9F,
+	0x98, 0x8C, 0xE2, 0xC0, 0xB8, 0x59, 0x54, 0x61,
+	0x0D, 0x2B, 0xCF, 0x86, 0x64, 0x84, 0x4D, 0xF2,
+	0xAB, 0x56, 0xE6, 0xC6, 0x1B, 0xB7, 0x01, 0xE4
+};
+
+static TEE_Result tee_rpmb_key_gen(uint16_t dev_id __unused,
+				   uint8_t *key, uint32_t len)
+{
+	TEE_Result res = TEE_SUCCESS;
+
+	if (!key || RPMB_KEY_MAC_SIZE != len) {
+		res = TEE_ERROR_BAD_PARAMETERS;
+		goto out;
+	}
+
+	DMSG("RPMB: Using test key");
+	memcpy(key, rpmb_test_key, RPMB_KEY_MAC_SIZE);
+
+out:
+	return res;
+}
+
+#else /* !CFG_RPMB_TESTKEY */
+
+/*
+ * NOTE: We need a common API to get hw unique key and it
+ * should return error when the hw unique is not a valid
+ * one as stated below.
+ * We need to make sure the hw unique we get is valid by:
+ * 1. In case of HUK is used, checking if OTP is hidden (in
+ *    which case only zeros will be returned) or not;
+ * 2. In case of SSK is used, checking if SSK in OTP is
+ *    write_locked (which means a valid key is provisioned)
+ *    or not.
+ *
+ * Maybe tee_get_hw_unique_key() should be exposed as
+ * generic API for getting hw unique key!
+ * We should change the API tee_otp_get_hw_unique_key()
+ * to return error code!
+ */
+static TEE_Result tee_get_hw_unique_key(struct tee_hw_unique_key *hwkey)
+{
+	if (!hwkey)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	tee_otp_get_hw_unique_key(hwkey);
+
+	return TEE_SUCCESS;
+}
+
+static TEE_Result tee_rpmb_key_gen(uint16_t dev_id __unused,
+				   uint8_t *key, uint32_t len)
+{
+	TEE_Result res;
+	struct tee_hw_unique_key hwkey;
+	uint8_t *ctx = NULL;
+
+	if (!key || RPMB_KEY_MAC_SIZE != len) {
+		res = TEE_ERROR_BAD_PARAMETERS;
+		goto out;
+	}
+
+	IMSG("RPMB: Using generated key");
+	res = tee_get_hw_unique_key(&hwkey);
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	ctx = malloc(rpmb_ctx->hash_ctx_size);
+	if (!ctx) {
+		res = TEE_ERROR_OUT_OF_MEMORY;
+		goto out;
+	}
+
+	res = crypto_ops.mac.init(ctx, TEE_ALG_HMAC_SHA256, hwkey.data,
+				  HW_UNIQUE_KEY_LENGTH);
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	res = crypto_ops.mac.update(ctx, TEE_ALG_HMAC_SHA256,
+				    (uint8_t *)rpmb_ctx->cid,
+				    RPMB_EMMC_CID_SIZE);
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	res = crypto_ops.mac.final(ctx, TEE_ALG_HMAC_SHA256, key, len);
+
+out:
+	free(ctx);
+	return res;
+}
+
+#endif /* !CFG_RPMB_TESTKEY */
+
+static void u32_to_bytes(uint32_t u32, uint8_t *bytes)
+{
+	*bytes = (uint8_t) (u32 >> 24);
+	*(bytes + 1) = (uint8_t) (u32 >> 16);
+	*(bytes + 2) = (uint8_t) (u32 >> 8);
+	*(bytes + 3) = (uint8_t) u32;
+}
+
+static void bytes_to_u32(uint8_t *bytes, uint32_t *u32)
+{
+	*u32 = (uint32_t) ((*(bytes) << 24) +
+			   (*(bytes + 1) << 16) +
+			   (*(bytes + 2) << 8) + (*(bytes + 3)));
+}
+
+static void u16_to_bytes(uint16_t u16, uint8_t *bytes)
+{
+	*bytes = (uint8_t) (u16 >> 8);
+	*(bytes + 1) = (uint8_t) u16;
+}
+
+static void bytes_to_u16(uint8_t *bytes, uint16_t *u16)
+{
+	*u16 = (uint16_t) ((*bytes << 8) + *(bytes + 1));
+}
+
+static TEE_Result tee_rpmb_mac_calc(uint8_t *mac, uint32_t macsize,
+				    uint8_t *key, uint32_t keysize,
+				    struct rpmb_data_frame *datafrms,
+				    uint16_t blkcnt)
+{
+	TEE_Result res = TEE_ERROR_GENERIC;
+	int i;
+	uint8_t *ctx = NULL;
+
+	if (!mac || !key || !datafrms)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	ctx = malloc(rpmb_ctx->hash_ctx_size);
+	if (!ctx)
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	res = crypto_ops.mac.init(ctx, TEE_ALG_HMAC_SHA256, key, keysize);
+	if (res != TEE_SUCCESS)
+		goto func_exit;
+
+	for (i = 0; i < blkcnt; i++) {
+		res = crypto_ops.mac.update(ctx, TEE_ALG_HMAC_SHA256,
+					  datafrms[i].data,
+					  RPMB_MAC_PROTECT_DATA_SIZE);
+		if (res != TEE_SUCCESS)
+			goto func_exit;
+	}
+
+	res = crypto_ops.mac.final(ctx, TEE_ALG_HMAC_SHA256, mac, macsize);
+	if (res != TEE_SUCCESS)
+		goto func_exit;
+
+	res = TEE_SUCCESS;
+
+func_exit:
+	free(ctx);
+	return res;
+}
+
+struct tee_rpmb_mem {
+	paddr_t phreq;
+	uint64_t phreq_cookie;
+	paddr_t phresp;
+	uint64_t phresp_cookie;
+	size_t req_size;
+	size_t resp_size;
+};
+
+static void tee_rpmb_free(struct tee_rpmb_mem *mem)
+{
+	if (!mem)
+		return;
+
+	if (mem->phreq) {
+		thread_rpc_free_payload(mem->phreq_cookie);
+		mem->phreq_cookie = 0;
+		mem->phreq = 0;
+	}
+	if (mem->phresp) {
+		thread_rpc_free_payload(mem->phresp_cookie);
+		mem->phresp_cookie = 0;
+		mem->phresp = 0;
+	}
+}
+
+
+static TEE_Result tee_rpmb_alloc(size_t req_size, size_t resp_size,
+		struct tee_rpmb_mem *mem, void **req, void **resp)
+{
+	TEE_Result res = TEE_SUCCESS;
+	size_t req_s = ROUNDUP(req_size, sizeof(uint32_t));
+	size_t resp_s = ROUNDUP(resp_size, sizeof(uint32_t));
+
+	if (!mem)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	memset(mem, 0, sizeof(*mem));
+	thread_rpc_alloc_payload(req_s, &mem->phreq, &mem->phreq_cookie);
+	thread_rpc_alloc_payload(resp_s, &mem->phresp, &mem->phresp_cookie);
+	if (!mem->phreq || !mem->phresp) {
+		res = TEE_ERROR_OUT_OF_MEMORY;
+		goto out;
+	}
+
+	*req = phys_to_virt(mem->phreq, MEM_AREA_NSEC_SHM);
+	*resp = phys_to_virt(mem->phresp, MEM_AREA_NSEC_SHM);
+	if (!*req || !*resp) {
+		res = TEE_ERROR_GENERIC;
+		goto out;
+	}
+
+	mem->req_size = req_size;
+	mem->resp_size = resp_size;
+
+out:
+	if (res != TEE_SUCCESS)
+		tee_rpmb_free(mem);
+	return res;
+}
+
+static TEE_Result tee_rpmb_invoke(struct tee_rpmb_mem *mem)
+{
+	struct optee_msg_param params[2];
+
+	memset(params, 0, sizeof(params));
+	params[0].attr = OPTEE_MSG_ATTR_TYPE_TMEM_INPUT;
+	params[1].attr = OPTEE_MSG_ATTR_TYPE_TMEM_OUTPUT;
+	params[0].u.tmem.buf_ptr = mem->phreq;
+	params[0].u.tmem.size = mem->req_size;
+	params[0].u.tmem.shm_ref = mem->phreq_cookie;
+	params[1].u.tmem.buf_ptr = mem->phresp;
+	params[1].u.tmem.size = mem->resp_size;
+	params[1].u.tmem.shm_ref = mem->phresp_cookie;
+
+	return thread_rpc_cmd(OPTEE_MSG_RPC_CMD_RPMB, 2, params);
+}
+
+static bool is_zero(const uint8_t *buf, size_t size)
+{
+	size_t i;
+
+	for (i = 0; i < size; i++)
+		if (buf[i])
+			return false;
+	return true;
+}
+
+static TEE_Result encrypt_block(uint8_t *out, const uint8_t *in,
+				uint16_t blk_idx, const uint8_t *fek)
+{
+	return tee_fs_crypt_block(out, in, RPMB_DATA_SIZE, blk_idx, fek,
+				  TEE_MODE_ENCRYPT);
+}
+
+static TEE_Result decrypt_block(uint8_t *out, const uint8_t *in,
+				uint16_t blk_idx, const uint8_t *fek)
+{
+	return tee_fs_crypt_block(out, in, RPMB_DATA_SIZE, blk_idx, fek,
+				  TEE_MODE_DECRYPT);
+}
+
+/* Decrypt/copy at most one block of data */
+static TEE_Result decrypt(uint8_t *out, const struct rpmb_data_frame *frm,
+			  size_t size, size_t offset,
+			  uint16_t blk_idx __maybe_unused, const uint8_t *fek)
+{
+	uint8_t *tmp __maybe_unused;
+
+
+	if ((size + offset < size) || (size + offset > RPMB_DATA_SIZE))
+		panic("invalid size or offset");
+
+	if (!fek) {
+		/* Block is not encrypted (not a file data block) */
+		memcpy(out, frm->data + offset, size);
+	} else if (is_zero(fek, TEE_FS_KM_FEK_SIZE)) {
+		/* The file was created with encryption disabled */
+		return TEE_ERROR_SECURITY;
+	} else {
+		/* Block is encrypted */
+		if (size < RPMB_DATA_SIZE) {
+			/*
+			 * Since output buffer is not large enough to hold one
+			 * block we must allocate a temporary buffer.
+			 */
+			tmp = malloc(RPMB_DATA_SIZE);
+			if (!tmp)
+				return TEE_ERROR_OUT_OF_MEMORY;
+			decrypt_block(tmp, frm->data, blk_idx, fek);
+			memcpy(out, tmp + offset, size);
+			free(tmp);
+		} else {
+			decrypt_block(out, frm->data, blk_idx, fek);
+		}
+	}
+
+	return TEE_SUCCESS;
+}
+
+static TEE_Result tee_rpmb_req_pack(struct rpmb_req *req,
+				    struct rpmb_raw_data *rawdata,
+				    uint16_t nbr_frms, uint16_t dev_id,
+				    const uint8_t *fek)
+{
+	TEE_Result res = TEE_ERROR_GENERIC;
+	int i;
+	struct rpmb_data_frame *datafrm;
+
+	if (!req || !rawdata || !nbr_frms)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	/*
+	 * Check write blockcount is not bigger than reliable write
+	 * blockcount.
+	 */
+	if ((rawdata->msg_type == RPMB_MSG_TYPE_REQ_AUTH_DATA_WRITE) &&
+	    (nbr_frms > rpmb_ctx->rel_wr_blkcnt)) {
+		DMSG("wr_blkcnt(%d) > rel_wr_blkcnt(%d)", nbr_frms,
+		     rpmb_ctx->rel_wr_blkcnt);
+		return TEE_ERROR_GENERIC;
+	}
+
+	req->cmd = RPMB_CMD_DATA_REQ;
+	req->dev_id = dev_id;
+
+	/* Allocate memory for construct all data packets and calculate MAC. */
+	datafrm = calloc(nbr_frms, RPMB_DATA_FRAME_SIZE);
+	if (!datafrm)
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	for (i = 0; i < nbr_frms; i++) {
+		u16_to_bytes(rawdata->msg_type, datafrm[i].msg_type);
+
+		if (rawdata->block_count)
+			u16_to_bytes(*rawdata->block_count,
+				     datafrm[i].block_count);
+
+		if (rawdata->blk_idx) {
+			/* Check the block index is within range. */
+			if ((*rawdata->blk_idx + nbr_frms) >
+			    rpmb_ctx->max_blk_idx) {
+				res = TEE_ERROR_GENERIC;
+				goto func_exit;
+			}
+			u16_to_bytes(*rawdata->blk_idx, datafrm[i].address);
+		}
+
+		if (rawdata->write_counter)
+			u32_to_bytes(*rawdata->write_counter,
+				     datafrm[i].write_counter);
+
+		if (rawdata->nonce)
+			memcpy(datafrm[i].nonce, rawdata->nonce,
+			       RPMB_NONCE_SIZE);
+
+		if (rawdata->data) {
+			if (fek)
+				encrypt_block(datafrm[i].data,
+					rawdata->data + (i * RPMB_DATA_SIZE),
+					*rawdata->blk_idx + i, fek);
+			else
+				memcpy(datafrm[i].data,
+				       rawdata->data + (i * RPMB_DATA_SIZE),
+				       RPMB_DATA_SIZE);
+		}
+	}
+
+	if (rawdata->key_mac) {
+		if (rawdata->msg_type == RPMB_MSG_TYPE_REQ_AUTH_DATA_WRITE) {
+			res =
+			    tee_rpmb_mac_calc(rawdata->key_mac,
+					      RPMB_KEY_MAC_SIZE, rpmb_ctx->key,
+					      RPMB_KEY_MAC_SIZE, datafrm,
+					      nbr_frms);
+			if (res != TEE_SUCCESS)
+				goto func_exit;
+		}
+		memcpy(datafrm[nbr_frms - 1].key_mac,
+		       rawdata->key_mac, RPMB_KEY_MAC_SIZE);
+	}
+
+	memcpy(TEE_RPMB_REQ_DATA(req), datafrm,
+	       nbr_frms * RPMB_DATA_FRAME_SIZE);
+
+#ifdef CFG_RPMB_FS_DEBUG_DATA
+	for (i = 0; i < nbr_frms; i++) {
+		DMSG("Dumping data frame %d:", i);
+		DHEXDUMP((uint8_t *)&datafrm[i] + RPMB_STUFF_DATA_SIZE,
+			 512 - RPMB_STUFF_DATA_SIZE);
+	}
+#endif
+
+	res = TEE_SUCCESS;
+func_exit:
+	free(datafrm);
+	return res;
+}
+
+static TEE_Result data_cpy_mac_calc_1b(struct rpmb_raw_data *rawdata,
+				       struct rpmb_data_frame *frm,
+				       uint8_t *fek)
+{
+	TEE_Result res;
+	uint8_t *data;
+	uint16_t idx;
+
+	if (rawdata->len + rawdata->byte_offset > RPMB_DATA_SIZE)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	res = tee_rpmb_mac_calc(rawdata->key_mac, RPMB_KEY_MAC_SIZE,
+				rpmb_ctx->key, RPMB_KEY_MAC_SIZE, frm, 1);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	data = rawdata->data;
+	bytes_to_u16(frm->address, &idx);
+
+	res = decrypt(data, frm, rawdata->len, rawdata->byte_offset, idx, fek);
+	return res;
+}
+
+static TEE_Result tee_rpmb_data_cpy_mac_calc(struct rpmb_data_frame *datafrm,
+					     struct rpmb_raw_data *rawdata,
+					     uint16_t nbr_frms,
+					     struct rpmb_data_frame *lastfrm,
+					     uint8_t *fek)
+{
+	TEE_Result res = TEE_ERROR_GENERIC;
+	int i;
+	uint8_t *ctx = NULL;
+	uint16_t offset;
+	uint32_t size;
+	uint8_t *data;
+	uint16_t start_idx;
+	struct rpmb_data_frame localfrm;
+
+	if (!datafrm || !rawdata || !nbr_frms || !lastfrm)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	if (nbr_frms == 1)
+		return data_cpy_mac_calc_1b(rawdata, lastfrm, fek);
+
+	/* nbr_frms > 1 */
+
+	data = rawdata->data;
+
+	ctx = malloc(rpmb_ctx->hash_ctx_size);
+	if (!ctx) {
+		res = TEE_ERROR_OUT_OF_MEMORY;
+		goto func_exit;
+	}
+
+	res = crypto_ops.mac.init(ctx, TEE_ALG_HMAC_SHA256, rpmb_ctx->key,
+				  RPMB_KEY_MAC_SIZE);
+	if (res != TEE_SUCCESS)
+		goto func_exit;
+
+	/*
+	 * Note: JEDEC JESD84-B51: "In every packet the address is the start
+	 * address of the full access (not address of the individual half a
+	 * sector)"
+	 */
+	bytes_to_u16(lastfrm->address, &start_idx);
+
+	for (i = 0; i < (nbr_frms - 1); i++) {
+
+		/*
+		 * By working on a local copy of the RPMB frame, we ensure that
+		 * the data can not be modified after the MAC is computed but
+		 * before the payload is decrypted/copied to the output buffer.
+		 */
+		memcpy(&localfrm, &datafrm[i], RPMB_DATA_FRAME_SIZE);
+
+		res = crypto_ops.mac.update(ctx, TEE_ALG_HMAC_SHA256,
+					    localfrm.data,
+					    RPMB_MAC_PROTECT_DATA_SIZE);
+		if (res != TEE_SUCCESS)
+			goto func_exit;
+
+		if (i == 0) {
+			/* First block */
+			offset = rawdata->byte_offset;
+			size = RPMB_DATA_SIZE - offset;
+		} else {
+			/* Middle blocks */
+			size = RPMB_DATA_SIZE;
+			offset = 0;
+		}
+
+		res = decrypt(data, &localfrm, size, offset, start_idx + i,
+			      fek);
+		if (res != TEE_SUCCESS)
+			goto func_exit;
+
+		data += size;
+	}
+
+	/* Last block */
+	size = (rawdata->len + rawdata->byte_offset) % RPMB_DATA_SIZE;
+	if (size == 0)
+		size = RPMB_DATA_SIZE;
+	res = decrypt(data, lastfrm, size, 0, start_idx + nbr_frms - 1, fek);
+	if (res != TEE_SUCCESS)
+		goto func_exit;
+
+	/* Update MAC against the last block */
+	res = crypto_ops.mac.update(ctx, TEE_ALG_HMAC_SHA256, lastfrm->data,
+				    RPMB_MAC_PROTECT_DATA_SIZE);
+	if (res != TEE_SUCCESS)
+		goto func_exit;
+
+	res = crypto_ops.mac.final(ctx, TEE_ALG_HMAC_SHA256, rawdata->key_mac,
+				   RPMB_KEY_MAC_SIZE);
+	if (res != TEE_SUCCESS)
+		goto func_exit;
+
+	res = TEE_SUCCESS;
+
+func_exit:
+	free(ctx);
+	return res;
+}
+
+static TEE_Result tee_rpmb_resp_unpack_verify(struct rpmb_data_frame *datafrm,
+					      struct rpmb_raw_data *rawdata,
+					      uint16_t nbr_frms, uint8_t *fek)
+{
+	TEE_Result res = TEE_ERROR_GENERIC;
+	uint16_t msg_type;
+	uint32_t wr_cnt;
+	uint16_t blk_idx;
+	uint16_t op_result;
+	struct rpmb_data_frame lastfrm;
+
+	if (!datafrm || !rawdata || !nbr_frms)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+#ifdef CFG_RPMB_FS_DEBUG_DATA
+	for (uint32_t i = 0; i < nbr_frms; i++) {
+		DMSG("Dumping data frame %d:", i);
+		DHEXDUMP((uint8_t *)&datafrm[i] + RPMB_STUFF_DATA_SIZE,
+			 512 - RPMB_STUFF_DATA_SIZE);
+	}
+#endif
+
+	/* Make sure the last data packet can't be modified once verified */
+	memcpy(&lastfrm, &datafrm[nbr_frms - 1], RPMB_DATA_FRAME_SIZE);
+
+	/* Handle operation result and translate to TEEC error code. */
+	bytes_to_u16(lastfrm.op_result, &op_result);
+	if (rawdata->op_result)
+		*rawdata->op_result = op_result;
+	if (op_result != RPMB_RESULT_OK)
+		return TEE_ERROR_GENERIC;
+
+	/* Check the response msg_type. */
+	bytes_to_u16(lastfrm.msg_type, &msg_type);
+	if (msg_type != rawdata->msg_type) {
+		DMSG("Unexpected msg_type (0x%04x != 0x%04x)", msg_type,
+		     rawdata->msg_type);
+		return TEE_ERROR_GENERIC;
+	}
+
+	if (rawdata->blk_idx) {
+		bytes_to_u16(lastfrm.address, &blk_idx);
+		if (blk_idx != *rawdata->blk_idx) {
+			DMSG("Unexpected block index");
+			return TEE_ERROR_GENERIC;
+		}
+	}
+
+	if (rawdata->write_counter) {
+		wr_cnt = *rawdata->write_counter;
+		bytes_to_u32(lastfrm.write_counter, rawdata->write_counter);
+		if (msg_type == RPMB_MSG_TYPE_RESP_AUTH_DATA_WRITE) {
+			/* Verify the write counter is incremented by 1 */
+			if (*rawdata->write_counter != wr_cnt + 1) {
+				DMSG("Counter mismatched (0x%04x/0x%04x)",
+				     *rawdata->write_counter, wr_cnt + 1);
+				return TEE_ERROR_SECURITY;
+			}
+			rpmb_ctx->wr_cnt++;
+		}
+	}
+
+	if (rawdata->nonce) {
+		if (buf_compare_ct(rawdata->nonce, lastfrm.nonce,
+				   RPMB_NONCE_SIZE) != 0) {
+			DMSG("Nonce mismatched");
+			return TEE_ERROR_SECURITY;
+		}
+	}
+
+	if (rawdata->key_mac) {
+		if (msg_type == RPMB_MSG_TYPE_RESP_AUTH_DATA_READ) {
+			if (!rawdata->data)
+				return TEE_ERROR_GENERIC;
+
+			res = tee_rpmb_data_cpy_mac_calc(datafrm, rawdata,
+							 nbr_frms, &lastfrm,
+							 fek);
+
+			if (res != TEE_SUCCESS)
+				return res;
+		} else {
+			/*
+			 * There should be only one data frame for
+			 * other msg types.
+			 */
+			if (nbr_frms != 1)
+				return TEE_ERROR_GENERIC;
+
+			res = tee_rpmb_mac_calc(rawdata->key_mac,
+						RPMB_KEY_MAC_SIZE,
+						rpmb_ctx->key,
+						RPMB_KEY_MAC_SIZE,
+						&lastfrm, 1);
+
+			if (res != TEE_SUCCESS)
+				return res;
+		}
+
+#ifndef CFG_RPMB_FS_NO_MAC
+		if (buf_compare_ct(rawdata->key_mac,
+				   (datafrm + nbr_frms - 1)->key_mac,
+				   RPMB_KEY_MAC_SIZE) != 0) {
+			DMSG("MAC mismatched:");
+#ifdef CFG_RPMB_FS_DEBUG_DATA
+			DHEXDUMP((uint8_t *)rawdata->key_mac, 32);
+#endif
+			return TEE_ERROR_SECURITY;
+		}
+#endif /* !CFG_RPMB_FS_NO_MAC */
+	}
+
+	return TEE_SUCCESS;
+}
+
+static TEE_Result tee_rpmb_get_dev_info(uint16_t dev_id,
+					struct rpmb_dev_info *dev_info)
+{
+	TEE_Result res = TEE_ERROR_GENERIC;
+	struct tee_rpmb_mem mem;
+	struct rpmb_dev_info *di;
+	struct rpmb_req *req = NULL;
+	uint8_t *resp = NULL;
+	uint32_t req_size;
+	uint32_t resp_size;
+
+	if (!dev_info)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	req_size = sizeof(struct rpmb_req);
+	resp_size = sizeof(struct rpmb_dev_info);
+	res = tee_rpmb_alloc(req_size, resp_size, &mem,
+			     (void *)&req, (void *)&resp);
+	if (res != TEE_SUCCESS)
+		goto func_exit;
+
+	req->cmd = RPMB_CMD_GET_DEV_INFO;
+	req->dev_id = dev_id;
+
+	di = (struct rpmb_dev_info *)resp;
+	di->ret_code = RPMB_CMD_GET_DEV_INFO_RET_ERROR;
+
+	res = tee_rpmb_invoke(&mem);
+	if (res != TEE_SUCCESS)
+		goto func_exit;
+
+	if (di->ret_code != RPMB_CMD_GET_DEV_INFO_RET_OK) {
+		res = TEE_ERROR_GENERIC;
+		goto func_exit;
+	}
+
+	memcpy((uint8_t *)dev_info, resp, sizeof(struct rpmb_dev_info));
+
+#ifdef CFG_RPMB_FS_DEBUG_DATA
+	DMSG("Dumping dev_info:");
+	DHEXDUMP((uint8_t *)dev_info, sizeof(struct rpmb_dev_info));
+#endif
+
+	res = TEE_SUCCESS;
+
+func_exit:
+	tee_rpmb_free(&mem);
+	return res;
+}
+
+static TEE_Result tee_rpmb_init_read_wr_cnt(uint16_t dev_id,
+					    uint32_t *wr_cnt,
+					    uint16_t *op_result)
+{
+	TEE_Result res = TEE_ERROR_GENERIC;
+	struct tee_rpmb_mem mem;
+	uint16_t msg_type;
+	uint8_t nonce[RPMB_NONCE_SIZE];
+	uint8_t hmac[RPMB_KEY_MAC_SIZE];
+	struct rpmb_req *req = NULL;
+	struct rpmb_data_frame *resp = NULL;
+	struct rpmb_raw_data rawdata;
+	uint32_t req_size;
+	uint32_t resp_size;
+
+	if (!wr_cnt)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	req_size = sizeof(struct rpmb_req) + RPMB_DATA_FRAME_SIZE;
+	resp_size = RPMB_DATA_FRAME_SIZE;
+	res = tee_rpmb_alloc(req_size, resp_size, &mem,
+			     (void *)&req, (void *)&resp);
+	if (res != TEE_SUCCESS)
+		goto func_exit;
+
+	res = crypto_ops.prng.read(nonce, RPMB_NONCE_SIZE);
+	if (res != TEE_SUCCESS)
+		goto func_exit;
+
+	msg_type = RPMB_MSG_TYPE_REQ_WRITE_COUNTER_VAL_READ;
+
+	memset(&rawdata, 0x00, sizeof(struct rpmb_raw_data));
+	rawdata.msg_type = msg_type;
+	rawdata.nonce = nonce;
+
+	res = tee_rpmb_req_pack(req, &rawdata, 1, dev_id, NULL);
+	if (res != TEE_SUCCESS)
+		goto func_exit;
+
+	res = tee_rpmb_invoke(&mem);
+	if (res != TEE_SUCCESS)
+		goto func_exit;
+
+	msg_type = RPMB_MSG_TYPE_RESP_WRITE_COUNTER_VAL_READ;
+
+	memset(&rawdata, 0x00, sizeof(struct rpmb_raw_data));
+	rawdata.msg_type = msg_type;
+	rawdata.op_result = op_result;
+	rawdata.write_counter = wr_cnt;
+	rawdata.nonce = nonce;
+	rawdata.key_mac = hmac;
+
+	res = tee_rpmb_resp_unpack_verify(resp, &rawdata, 1, NULL);
+	if (res != TEE_SUCCESS)
+		goto func_exit;
+
+	res = TEE_SUCCESS;
+
+func_exit:
+	tee_rpmb_free(&mem);
+	return res;
+}
+
+static TEE_Result tee_rpmb_verify_key_sync_counter(uint16_t dev_id)
+{
+	uint16_t op_result = 0;
+	TEE_Result res = TEE_ERROR_GENERIC;
+
+	res = tee_rpmb_init_read_wr_cnt(dev_id, &rpmb_ctx->wr_cnt,
+					&op_result);
+
+	if (res == TEE_SUCCESS) {
+		rpmb_ctx->key_verified = true;
+		rpmb_ctx->wr_cnt_synced = true;
+	}
+
+	DMSG("Verify key returning 0x%x\n", res);
+	return res;
+}
+
+#ifdef CFG_RPMB_WRITE_KEY
+static TEE_Result tee_rpmb_write_key(uint16_t dev_id)
+{
+	TEE_Result res = TEE_ERROR_GENERIC;
+	struct tee_rpmb_mem mem = { 0 };
+	uint16_t msg_type;
+	struct rpmb_req *req = NULL;
+	struct rpmb_data_frame *resp = NULL;
+	struct rpmb_raw_data rawdata;
+	uint32_t req_size;
+	uint32_t resp_size;
+
+	req_size = sizeof(struct rpmb_req) + RPMB_DATA_FRAME_SIZE;
+	resp_size = RPMB_DATA_FRAME_SIZE;
+	res = tee_rpmb_alloc(req_size, resp_size, &mem,
+			     (void *)&req, (void *)&resp);
+	if (res != TEE_SUCCESS)
+		goto func_exit;
+
+	msg_type = RPMB_MSG_TYPE_REQ_AUTH_KEY_PROGRAM;
+
+	memset(&rawdata, 0x00, sizeof(struct rpmb_raw_data));
+	rawdata.msg_type = msg_type;
+	rawdata.key_mac = rpmb_ctx->key;
+
+	res = tee_rpmb_req_pack(req, &rawdata, 1, dev_id, NULL);
+	if (res != TEE_SUCCESS)
+		goto func_exit;
+
+	res = tee_rpmb_invoke(&mem);
+	if (res != TEE_SUCCESS)
+		goto func_exit;
+
+	msg_type = RPMB_MSG_TYPE_RESP_AUTH_KEY_PROGRAM;
+
+	memset(&rawdata, 0x00, sizeof(struct rpmb_raw_data));
+	rawdata.msg_type = msg_type;
+
+	res = tee_rpmb_resp_unpack_verify(resp, &rawdata, 1, NULL);
+	if (res != TEE_SUCCESS)
+		goto func_exit;
+
+	res = TEE_SUCCESS;
+
+func_exit:
+	tee_rpmb_free(&mem);
+	return res;
+}
+
+static TEE_Result tee_rpmb_write_and_verify_key(uint16_t dev_id)
+{
+	TEE_Result res;
+
+	DMSG("RPMB INIT: Writing Key");
+	res = tee_rpmb_write_key(dev_id);
+	if (res == TEE_SUCCESS) {
+		DMSG("RPMB INIT: Verifying Key");
+		res = tee_rpmb_verify_key_sync_counter(dev_id);
+	}
+	return res;
+}
+#else
+static TEE_Result tee_rpmb_write_and_verify_key(uint16_t dev_id __unused)
+{
+	return TEE_ERROR_BAD_STATE;
+}
+#endif
+
+/* True when all the required crypto functions are available */
+static bool have_crypto_ops(void)
+{
+	return (crypto_ops.mac.init && crypto_ops.mac.update &&
+		crypto_ops.mac.final && crypto_ops.prng.read);
+}
+
+/* This function must never return TEE_SUCCESS if rpmb_ctx == NULL */
+static TEE_Result tee_rpmb_init(uint16_t dev_id)
+{
+	TEE_Result res = TEE_SUCCESS;
+	struct rpmb_dev_info dev_info;
+
+	if (!have_crypto_ops())
+		return TEE_ERROR_NOT_SUPPORTED;
+
+	if (!rpmb_ctx) {
+		rpmb_ctx = calloc(1, sizeof(struct tee_rpmb_ctx));
+		if (!rpmb_ctx)
+			return TEE_ERROR_OUT_OF_MEMORY;
+	} else if (rpmb_ctx->dev_id != dev_id) {
+		memset(rpmb_ctx, 0x00, sizeof(struct tee_rpmb_ctx));
+	}
+
+	rpmb_ctx->dev_id = dev_id;
+
+	if (!rpmb_ctx->dev_info_synced) {
+		DMSG("RPMB: Syncing device information");
+
+		dev_info.rpmb_size_mult = 0;
+		dev_info.rel_wr_sec_c = 0;
+		res = tee_rpmb_get_dev_info(dev_id, &dev_info);
+		if (res != TEE_SUCCESS)
+			goto func_exit;
+
+		DMSG("RPMB: RPMB size is %d*128 KB", dev_info.rpmb_size_mult);
+		DMSG("RPMB: Reliable Write Sector Count is %d",
+		     dev_info.rel_wr_sec_c);
+
+		if (dev_info.rpmb_size_mult == 0) {
+			res = TEE_ERROR_GENERIC;
+			goto func_exit;
+		}
+
+		rpmb_ctx->max_blk_idx = (dev_info.rpmb_size_mult *
+					 RPMB_SIZE_SINGLE / RPMB_DATA_SIZE) - 1;
+
+		memcpy(rpmb_ctx->cid, dev_info.cid, RPMB_EMMC_CID_SIZE);
+
+		if ((rpmb_ctx->hash_ctx_size == 0) &&
+		    (crypto_ops.mac.get_ctx_size(
+			    TEE_ALG_HMAC_SHA256,
+			    &rpmb_ctx->hash_ctx_size))) {
+			rpmb_ctx->hash_ctx_size = 0;
+			res = TEE_ERROR_GENERIC;
+			goto func_exit;
+		}
+
+#ifdef RPMB_DRIVER_MULTIPLE_WRITE_FIXED
+		rpmb_ctx->rel_wr_blkcnt = dev_info.rel_wr_sec_c * 2;
+#else
+		rpmb_ctx->rel_wr_blkcnt = 1;
+#endif
+
+		rpmb_ctx->dev_info_synced = true;
+	}
+
+	if (!rpmb_ctx->key_derived) {
+		DMSG("RPMB INIT: Deriving key");
+
+		res = tee_rpmb_key_gen(dev_id, rpmb_ctx->key,
+				       RPMB_KEY_MAC_SIZE);
+		if (res != TEE_SUCCESS)
+			goto func_exit;
+
+		rpmb_ctx->key_derived = true;
+	}
+
+	/* Perform a write counter read to verify if the key is ok. */
+	if (!rpmb_ctx->wr_cnt_synced || !rpmb_ctx->key_verified) {
+		DMSG("RPMB INIT: Verifying Key");
+
+		res = tee_rpmb_verify_key_sync_counter(dev_id);
+		if (res != TEE_SUCCESS && !rpmb_ctx->key_verified) {
+			/*
+			 * Need to write the key here and verify it.
+			 */
+			res = tee_rpmb_write_and_verify_key(dev_id);
+		}
+	}
+
+func_exit:
+	return res;
+}
+
+/*
+ * Read RPMB data in bytes.
+ *
+ * @dev_id     Device ID of the eMMC device.
+ * @addr       Byte address of data.
+ * @data       Pointer to the data.
+ * @len        Size of data in bytes.
+ * @fek        Encrypted File Encryption Key or NULL.
+ */
+static TEE_Result tee_rpmb_read(uint16_t dev_id, uint32_t addr, uint8_t *data,
+				uint32_t len, uint8_t *fek)
+{
+	TEE_Result res = TEE_ERROR_GENERIC;
+	struct tee_rpmb_mem mem = { 0 };
+	uint16_t msg_type;
+	uint8_t nonce[RPMB_NONCE_SIZE];
+	uint8_t hmac[RPMB_KEY_MAC_SIZE];
+	struct rpmb_req *req = NULL;
+	struct rpmb_data_frame *resp = NULL;
+	struct rpmb_raw_data rawdata;
+	uint32_t req_size;
+	uint32_t resp_size;
+	uint16_t blk_idx;
+	uint16_t blkcnt;
+	uint8_t byte_offset;
+
+	if (!data || !len)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	blk_idx = addr / RPMB_DATA_SIZE;
+	byte_offset = addr % RPMB_DATA_SIZE;
+
+	blkcnt =
+	    ROUNDUP(len + byte_offset, RPMB_DATA_SIZE) / RPMB_DATA_SIZE;
+	res = tee_rpmb_init(dev_id);
+	if (res != TEE_SUCCESS)
+		goto func_exit;
+
+	req_size = sizeof(struct rpmb_req) + RPMB_DATA_FRAME_SIZE;
+	resp_size = RPMB_DATA_FRAME_SIZE * blkcnt;
+	res = tee_rpmb_alloc(req_size, resp_size, &mem,
+			     (void *)&req, (void *)&resp);
+	if (res != TEE_SUCCESS)
+		goto func_exit;
+
+	msg_type = RPMB_MSG_TYPE_REQ_AUTH_DATA_READ;
+	res = crypto_ops.prng.read(nonce, RPMB_NONCE_SIZE);
+	if (res != TEE_SUCCESS)
+		goto func_exit;
+
+	memset(&rawdata, 0x00, sizeof(struct rpmb_raw_data));
+	rawdata.msg_type = msg_type;
+	rawdata.nonce = nonce;
+	rawdata.blk_idx = &blk_idx;
+	res = tee_rpmb_req_pack(req, &rawdata, 1, dev_id, NULL);
+	if (res != TEE_SUCCESS)
+		goto func_exit;
+
+	req->block_count = blkcnt;
+
+	DMSG("Read %u block%s at index %u", blkcnt, ((blkcnt > 1) ? "s" : ""),
+	     blk_idx);
+
+	res = tee_rpmb_invoke(&mem);
+	if (res != TEE_SUCCESS)
+		goto func_exit;
+
+	msg_type = RPMB_MSG_TYPE_RESP_AUTH_DATA_READ;
+
+	memset(&rawdata, 0x00, sizeof(struct rpmb_raw_data));
+	rawdata.msg_type = msg_type;
+	rawdata.block_count = &blkcnt;
+	rawdata.blk_idx = &blk_idx;
+	rawdata.nonce = nonce;
+	rawdata.key_mac = hmac;
+	rawdata.data = data;
+
+	rawdata.len = len;
+	rawdata.byte_offset = byte_offset;
+
+	res = tee_rpmb_resp_unpack_verify(resp, &rawdata, blkcnt, fek);
+	if (res != TEE_SUCCESS)
+		goto func_exit;
+
+	res = TEE_SUCCESS;
+
+func_exit:
+	tee_rpmb_free(&mem);
+	return res;
+}
+
+static TEE_Result tee_rpmb_write_blk(uint16_t dev_id, uint16_t blk_idx,
+				     const uint8_t *data_blks, uint16_t blkcnt,
+				     const uint8_t *fek)
+{
+	TEE_Result res;
+	struct tee_rpmb_mem mem;
+	uint16_t msg_type;
+	uint32_t wr_cnt;
+	uint8_t hmac[RPMB_KEY_MAC_SIZE];
+	struct rpmb_req *req = NULL;
+	struct rpmb_data_frame *resp = NULL;
+	struct rpmb_raw_data rawdata;
+	uint32_t req_size;
+	uint32_t resp_size;
+	uint32_t nbr_writes;
+	uint16_t tmp_blkcnt;
+	uint16_t tmp_blk_idx;
+	uint16_t i;
+
+	DMSG("Write %u block%s at index %u", blkcnt, ((blkcnt > 1) ? "s" : ""),
+	     blk_idx);
+
+	if (!data_blks || !blkcnt)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	res = tee_rpmb_init(dev_id);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	/*
+	 * We need to split data when block count
+	 * is bigger than reliable block write count.
+	 */
+	if (blkcnt < rpmb_ctx->rel_wr_blkcnt)
+		req_size = sizeof(struct rpmb_req) +
+		    RPMB_DATA_FRAME_SIZE * blkcnt;
+	else
+		req_size = sizeof(struct rpmb_req) +
+		    RPMB_DATA_FRAME_SIZE * rpmb_ctx->rel_wr_blkcnt;
+
+	resp_size = RPMB_DATA_FRAME_SIZE;
+	res = tee_rpmb_alloc(req_size, resp_size, &mem,
+			     (void *)&req, (void *)&resp);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	nbr_writes = blkcnt / rpmb_ctx->rel_wr_blkcnt;
+	if (blkcnt % rpmb_ctx->rel_wr_blkcnt > 0)
+		nbr_writes += 1;
+
+	tmp_blkcnt = rpmb_ctx->rel_wr_blkcnt;
+	tmp_blk_idx = blk_idx;
+	for (i = 0; i < nbr_writes; i++) {
+		/*
+		 * To handle the last write of block count which is
+		 * equal or smaller than reliable write block count.
+		 */
+		if (i == nbr_writes - 1)
+			tmp_blkcnt = blkcnt - rpmb_ctx->rel_wr_blkcnt *
+			    (nbr_writes - 1);
+
+		msg_type = RPMB_MSG_TYPE_REQ_AUTH_DATA_WRITE;
+		wr_cnt = rpmb_ctx->wr_cnt;
+
+		memset(req, 0x00, req_size);
+		memset(resp, 0x00, resp_size);
+
+		memset(&rawdata, 0x00, sizeof(struct rpmb_raw_data));
+		rawdata.msg_type = msg_type;
+		rawdata.block_count = &tmp_blkcnt;
+		rawdata.blk_idx = &tmp_blk_idx;
+		rawdata.write_counter = &wr_cnt;
+		rawdata.key_mac = hmac;
+		rawdata.data = (uint8_t *)data_blks +
+				i * rpmb_ctx->rel_wr_blkcnt * RPMB_DATA_SIZE;
+
+		res = tee_rpmb_req_pack(req, &rawdata, tmp_blkcnt, dev_id,
+					fek);
+		if (res != TEE_SUCCESS)
+			goto out;
+
+		res = tee_rpmb_invoke(&mem);
+		if (res != TEE_SUCCESS) {
+			/*
+			 * To force wr_cnt sync next time, as it might get
+			 * out of sync due to inconsistent operation result!
+			 */
+			rpmb_ctx->wr_cnt_synced = false;
+			goto out;
+		}
+
+		msg_type = RPMB_MSG_TYPE_RESP_AUTH_DATA_WRITE;
+
+		memset(&rawdata, 0x00, sizeof(struct rpmb_raw_data));
+		rawdata.msg_type = msg_type;
+		rawdata.block_count = &tmp_blkcnt;
+		rawdata.blk_idx = &tmp_blk_idx;
+		rawdata.write_counter = &wr_cnt;
+		rawdata.key_mac = hmac;
+
+		res = tee_rpmb_resp_unpack_verify(resp, &rawdata, 1, NULL);
+		if (res != TEE_SUCCESS) {
+			/*
+			 * To force wr_cnt sync next time, as it might get
+			 * out of sync due to inconsistent operation result!
+			 */
+			rpmb_ctx->wr_cnt_synced = false;
+			goto out;
+		}
+
+		tmp_blk_idx += tmp_blkcnt;
+	}
+
+out:
+	tee_rpmb_free(&mem);
+	return res;
+}
+
+static bool tee_rpmb_write_is_atomic(uint16_t dev_id __unused, uint32_t addr,
+				     uint32_t len)
+{
+	uint8_t byte_offset = addr % RPMB_DATA_SIZE;
+	uint16_t blkcnt = ROUNDUP(len + byte_offset,
+				  RPMB_DATA_SIZE) / RPMB_DATA_SIZE;
+
+	return (blkcnt <= rpmb_ctx->rel_wr_blkcnt);
+}
+
+/*
+ * Write RPMB data in bytes.
+ *
+ * @dev_id     Device ID of the eMMC device.
+ * @addr       Byte address of data.
+ * @data       Pointer to the data.
+ * @len        Size of data in bytes.
+ * @fek        Encrypted File Encryption Key or NULL.
+ */
+static TEE_Result tee_rpmb_write(uint16_t dev_id, uint32_t addr,
+				 const uint8_t *data, uint32_t len,
+				 uint8_t *fek)
+{
+	TEE_Result res = TEE_ERROR_GENERIC;
+	uint8_t *data_tmp = NULL;
+	uint16_t blk_idx;
+	uint16_t blkcnt;
+	uint8_t byte_offset;
+
+	blk_idx = addr / RPMB_DATA_SIZE;
+	byte_offset = addr % RPMB_DATA_SIZE;
+
+	blkcnt =
+	    ROUNDUP(len + byte_offset, RPMB_DATA_SIZE) / RPMB_DATA_SIZE;
+
+	if (byte_offset == 0 && (len % RPMB_DATA_SIZE) == 0) {
+		res = tee_rpmb_write_blk(dev_id, blk_idx, data, blkcnt, fek);
+		if (res != TEE_SUCCESS)
+			goto func_exit;
+	} else {
+		data_tmp = calloc(blkcnt, RPMB_DATA_SIZE);
+		if (!data_tmp) {
+			res = TEE_ERROR_OUT_OF_MEMORY;
+			goto func_exit;
+		}
+
+		/* Read the complete blocks */
+		res = tee_rpmb_read(dev_id, blk_idx * RPMB_DATA_SIZE, data_tmp,
+				    blkcnt * RPMB_DATA_SIZE, fek);
+		if (res != TEE_SUCCESS)
+			goto func_exit;
+
+		/* Partial update of the data blocks */
+		memcpy(data_tmp + byte_offset, data, len);
+
+		res = tee_rpmb_write_blk(dev_id, blk_idx, data_tmp, blkcnt,
+					 fek);
+		if (res != TEE_SUCCESS)
+			goto func_exit;
+	}
+
+	res = TEE_SUCCESS;
+
+func_exit:
+	free(data_tmp);
+	return res;
+}
+
+/*
+ * Read the RPMB write counter.
+ *
+ * @dev_id     Device ID of the eMMC device.
+ * @counter    Pointer to the counter.
+ */
+static TEE_Result tee_rpmb_get_write_counter(uint16_t dev_id,
+					     uint32_t *counter)
+{
+	TEE_Result res = TEE_SUCCESS;
+
+	if (!counter)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	if (!rpmb_ctx || !rpmb_ctx->wr_cnt_synced) {
+		res = tee_rpmb_init(dev_id);
+		if (res != TEE_SUCCESS)
+			goto func_exit;
+	}
+
+	*counter = rpmb_ctx->wr_cnt;
+
+func_exit:
+	return res;
+}
+
+/*
+ * Read the RPMB max block.
+ *
+ * @dev_id     Device ID of the eMMC device.
+ * @counter    Pointer to receive the max block.
+ */
+static TEE_Result tee_rpmb_get_max_block(uint16_t dev_id, uint32_t *max_block)
+{
+	TEE_Result res = TEE_SUCCESS;
+
+	if (!max_block)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	if (!rpmb_ctx || !rpmb_ctx->dev_info_synced) {
+		res = tee_rpmb_init(dev_id);
+		if (res != TEE_SUCCESS)
+			goto func_exit;
+	}
+
+	*max_block = rpmb_ctx->max_blk_idx;
+
+func_exit:
+	return res;
+}
+
+/*
+ * End of lower interface to RPMB device
+ */
+
+static TEE_Result get_fat_start_address(uint32_t *addr);
+
+static void dump_fat(void)
+{
+	TEE_Result res = TEE_ERROR_GENERIC;
+	struct rpmb_fat_entry *fat_entries = NULL;
+	uint32_t fat_address;
+	size_t size;
+	int i;
+	bool last_entry_found = false;
+
+	res = get_fat_start_address(&fat_address);
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	size = N_ENTRIES * sizeof(struct rpmb_fat_entry);
+	fat_entries = malloc(size);
+	if (!fat_entries) {
+		res = TEE_ERROR_OUT_OF_MEMORY;
+		goto out;
+	}
+
+	while (!last_entry_found) {
+		res = tee_rpmb_read(CFG_RPMB_FS_DEV_ID, fat_address,
+				    (uint8_t *)fat_entries, size, NULL);
+		if (res != TEE_SUCCESS)
+			goto out;
+
+		for (i = 0; i < N_ENTRIES; i++) {
+
+			FMSG("flags 0x%x, size %d, address 0x%x, filename '%s'",
+				fat_entries[i].flags,
+				fat_entries[i].data_size,
+				fat_entries[i].start_address,
+				fat_entries[i].filename);
+
+			if ((fat_entries[i].flags & FILE_IS_LAST_ENTRY) != 0) {
+				last_entry_found = true;
+				break;
+			}
+
+			/* Move to next fat_entry. */
+			fat_address += sizeof(struct rpmb_fat_entry);
+		}
+	}
+
+out:
+	free(fat_entries);
+}
+
+#if (TRACE_LEVEL >= TRACE_DEBUG)
+static void dump_fh(struct rpmb_file_handle *fh)
+{
+	DMSG("fh->filename=%s", fh->filename);
+	DMSG("fh->pos=%u", fh->pos);
+	DMSG("fh->rpmb_fat_address=%u", fh->rpmb_fat_address);
+	DMSG("fh->fat_entry.start_address=%u", fh->fat_entry.start_address);
+	DMSG("fh->fat_entry.data_size=%u", fh->fat_entry.data_size);
+}
+#else
+static void dump_fh(struct rpmb_file_handle *fh __unused)
+{
+}
+#endif
+
+static struct rpmb_file_handle *alloc_file_handle(const char *filename)
+{
+	struct rpmb_file_handle *fh = NULL;
+
+	fh = calloc(1, sizeof(struct rpmb_file_handle));
+	if (!fh)
+		return NULL;
+
+	if (filename)
+		strlcpy(fh->filename, filename, sizeof(fh->filename));
+
+	return fh;
+}
+
+/**
+ * write_fat_entry: Store info in a fat_entry to RPMB.
+ */
+static TEE_Result write_fat_entry(struct rpmb_file_handle *fh,
+				  bool update_write_counter)
+{
+	TEE_Result res = TEE_ERROR_GENERIC;
+
+	/* Protect partition data. */
+	if (fh->rpmb_fat_address < sizeof(struct rpmb_fs_partition)) {
+		res = TEE_ERROR_ACCESS_CONFLICT;
+		goto out;
+	}
+
+	if (fh->rpmb_fat_address % sizeof(struct rpmb_fat_entry) != 0) {
+		res = TEE_ERROR_BAD_PARAMETERS;
+		goto out;
+	}
+
+	if (update_write_counter) {
+		res = tee_rpmb_get_write_counter(CFG_RPMB_FS_DEV_ID,
+						 &fh->fat_entry.write_counter);
+		if (res != TEE_SUCCESS)
+			goto out;
+	}
+
+	res = tee_rpmb_write(CFG_RPMB_FS_DEV_ID, fh->rpmb_fat_address,
+			     (uint8_t *)&fh->fat_entry,
+			     sizeof(struct rpmb_fat_entry), NULL);
+
+	dump_fat();
+
+out:
+	return res;
+}
+
+/**
+ * rpmb_fs_setup: Setup rpmb fs.
+ * Set initial partition and FS values and write to RPMB.
+ * Store frequently used data in RAM.
+ */
+static TEE_Result rpmb_fs_setup(void)
+{
+	TEE_Result res = TEE_ERROR_GENERIC;
+	struct rpmb_fs_partition *partition_data = NULL;
+	struct rpmb_file_handle *fh = NULL;
+	uint32_t max_rpmb_block = 0;
+
+	if (fs_par) {
+		res = TEE_SUCCESS;
+		goto out;
+	}
+
+	res = tee_rpmb_get_max_block(CFG_RPMB_FS_DEV_ID, &max_rpmb_block);
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	partition_data = calloc(1, sizeof(struct rpmb_fs_partition));
+	if (!partition_data) {
+		res = TEE_ERROR_OUT_OF_MEMORY;
+		goto out;
+	}
+
+	res = tee_rpmb_read(CFG_RPMB_FS_DEV_ID, RPMB_STORAGE_START_ADDRESS,
+			    (uint8_t *)partition_data,
+			    sizeof(struct rpmb_fs_partition), NULL);
+	if (res != TEE_SUCCESS)
+		goto out;
+
+#ifndef CFG_RPMB_RESET_FAT
+	if (partition_data->rpmb_fs_magic == RPMB_FS_MAGIC) {
+		if (partition_data->fs_version == FS_VERSION) {
+			res = TEE_SUCCESS;
+			goto store_fs_par;
+		} else {
+			/* Wrong software is in use. */
+			res = TEE_ERROR_ACCESS_DENIED;
+			goto out;
+		}
+	}
+#else
+	EMSG("**** Clearing Storage ****");
+#endif
+
+	/* Setup new partition data. */
+	partition_data->rpmb_fs_magic = RPMB_FS_MAGIC;
+	partition_data->fs_version = FS_VERSION;
+	partition_data->fat_start_address = RPMB_FS_FAT_START_ADDRESS;
+
+	/* Initial FAT entry with FILE_IS_LAST_ENTRY flag set. */
+	fh = alloc_file_handle(NULL);
+	if (!fh) {
+		res = TEE_ERROR_OUT_OF_MEMORY;
+		goto out;
+	}
+	fh->fat_entry.flags = FILE_IS_LAST_ENTRY;
+	fh->rpmb_fat_address = partition_data->fat_start_address;
+
+	/* Write init FAT entry and partition data to RPMB. */
+	res = write_fat_entry(fh, true);
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	res =
+	    tee_rpmb_get_write_counter(CFG_RPMB_FS_DEV_ID,
+				       &partition_data->write_counter);
+	if (res != TEE_SUCCESS)
+		goto out;
+	res = tee_rpmb_write(CFG_RPMB_FS_DEV_ID, RPMB_STORAGE_START_ADDRESS,
+			     (uint8_t *)partition_data,
+			     sizeof(struct rpmb_fs_partition), NULL);
+
+#ifndef CFG_RPMB_RESET_FAT
+store_fs_par:
+#endif
+
+	/* Store FAT start address. */
+	fs_par = calloc(1, sizeof(struct rpmb_fs_parameters));
+	if (!fs_par) {
+		res = TEE_ERROR_OUT_OF_MEMORY;
+		goto out;
+	}
+
+	fs_par->fat_start_address = partition_data->fat_start_address;
+	fs_par->max_rpmb_address = max_rpmb_block << RPMB_BLOCK_SIZE_SHIFT;
+
+	dump_fat();
+
+out:
+	free(fh);
+	free(partition_data);
+	return res;
+}
+
+/**
+ * get_fat_start_address:
+ * FAT start_address from fs_par.
+ */
+static TEE_Result get_fat_start_address(uint32_t *addr)
+{
+	if (!fs_par)
+		return TEE_ERROR_NO_DATA;
+
+	*addr = fs_par->fat_start_address;
+
+	return TEE_SUCCESS;
+}
+
+/**
+ * read_fat: Read FAT entries
+ * Return matching FAT entry for read, rm rename and stat.
+ * Build up memory pool and return matching entry for write operation.
+ * "Last FAT entry" can be returned during write.
+ */
+static TEE_Result read_fat(struct rpmb_file_handle *fh, tee_mm_pool_t *p)
+{
+	TEE_Result res = TEE_ERROR_GENERIC;
+	tee_mm_entry_t *mm = NULL;
+	struct rpmb_fat_entry *fat_entries = NULL;
+	uint32_t fat_address;
+	size_t size;
+	int i;
+	bool entry_found = false;
+	bool last_entry_found = false;
+	bool expand_fat = false;
+	struct rpmb_file_handle last_fh;
+
+	DMSG("fat_address %d", fh->rpmb_fat_address);
+
+	res = rpmb_fs_setup();
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	res = get_fat_start_address(&fat_address);
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	size = N_ENTRIES * sizeof(struct rpmb_fat_entry);
+	fat_entries = malloc(size);
+	if (!fat_entries) {
+		res = TEE_ERROR_OUT_OF_MEMORY;
+		goto out;
+	}
+
+	/*
+	 * The pool is used to represent the current RPMB layout. To find
+	 * a slot for the file tee_mm_alloc is called on the pool. Thus
+	 * if it is not NULL the entire FAT must be traversed to fill in
+	 * the pool.
+	 */
+	while (!last_entry_found && (!entry_found || p)) {
+		res = tee_rpmb_read(CFG_RPMB_FS_DEV_ID, fat_address,
+				    (uint8_t *)fat_entries, size, NULL);
+		if (res != TEE_SUCCESS)
+			goto out;
+
+		for (i = 0; i < N_ENTRIES; i++) {
+			/*
+			 * Look for an entry, matching filenames. (read, rm,
+			 * rename and stat.). Only store first filename match.
+			 */
+			if (fh->filename &&
+			    (strcmp(fh->filename,
+				    fat_entries[i].filename) == 0) &&
+			    (fat_entries[i].flags & FILE_IS_ACTIVE) &&
+			    (!entry_found)) {
+				entry_found = true;
+				fh->rpmb_fat_address = fat_address;
+				memcpy(&fh->fat_entry, &fat_entries[i],
+				       sizeof(struct rpmb_fat_entry));
+				if (!p)
+					break;
+			}
+
+			/* Add existing files to memory pool. (write) */
+			if (p) {
+				if ((fat_entries[i].flags & FILE_IS_ACTIVE) &&
+				    (fat_entries[i].data_size > 0)) {
+
+					mm = tee_mm_alloc2
+						(p,
+						 fat_entries[i].start_address,
+						 fat_entries[i].data_size);
+					if (!mm) {
+						res = TEE_ERROR_OUT_OF_MEMORY;
+						goto out;
+					}
+				}
+
+				/* Unused FAT entries can be reused (write) */
+				if (((fat_entries[i].flags & FILE_IS_ACTIVE) ==
+				     0) && (fh->rpmb_fat_address == 0)) {
+					fh->rpmb_fat_address = fat_address;
+					memcpy(&fh->fat_entry, &fat_entries[i],
+					       sizeof(struct rpmb_fat_entry));
+				}
+			}
+
+			if ((fat_entries[i].flags & FILE_IS_LAST_ENTRY) != 0) {
+				last_entry_found = true;
+
+				/*
+				 * If the last entry was reached and was chosen
+				 * by the previous check, then the FAT needs to
+				 * be expanded.
+				 * fh->rpmb_fat_address is the address chosen
+				 * to store the files FAT entry and fat_address
+				 * is the current FAT entry address being
+				 * compared.
+				 */
+				if (p && fh->rpmb_fat_address == fat_address)
+					expand_fat = true;
+				break;
+			}
+
+			/* Move to next fat_entry. */
+			fat_address += sizeof(struct rpmb_fat_entry);
+		}
+	}
+
+	/*
+	 * Represent the FAT table in the pool.
+	 */
+	if (p) {
+		/*
+		 * Since fat_address is the start of the last entry it needs to
+		 * be moved up by an entry.
+		 */
+		fat_address += sizeof(struct rpmb_fat_entry);
+
+		/* Make room for yet a FAT entry and add to memory pool. */
+		if (expand_fat)
+			fat_address += sizeof(struct rpmb_fat_entry);
+
+		mm = tee_mm_alloc2(p, RPMB_STORAGE_START_ADDRESS, fat_address);
+		if (!mm) {
+			res = TEE_ERROR_OUT_OF_MEMORY;
+			goto out;
+		}
+
+		if (expand_fat) {
+			/*
+			 * Point fat_address to the beginning of the new
+			 * entry.
+			 */
+			fat_address -= sizeof(struct rpmb_fat_entry);
+			memset(&last_fh, 0, sizeof(last_fh));
+			last_fh.fat_entry.flags = FILE_IS_LAST_ENTRY;
+			last_fh.rpmb_fat_address = fat_address;
+			res = write_fat_entry(&last_fh, true);
+			if (res != TEE_SUCCESS)
+				goto out;
+		}
+	}
+
+	if (fh->filename && !fh->rpmb_fat_address)
+		res = TEE_ERROR_ITEM_NOT_FOUND;
+
+out:
+	free(fat_entries);
+	return res;
+}
+
+static TEE_Result generate_fek(struct rpmb_fat_entry *fe)
+{
+	TEE_Result res;
+
+again:
+	res = tee_fs_generate_fek(fe->fek, sizeof(fe->fek));
+	if (res != TEE_SUCCESS)
+		return res;
+
+	if (is_zero(fe->fek, sizeof(fe->fek)))
+		goto again;
+
+	return res;
+}
+
+static TEE_Result rpmb_fs_open_internal(const char *file, bool create,
+					struct tee_file_handle **ret_fh)
+{
+	struct rpmb_file_handle *fh = NULL;
+	size_t filelen;
+	tee_mm_pool_t p;
+	bool pool_result;
+	TEE_Result res = TEE_ERROR_GENERIC;
+
+	mutex_lock(&rpmb_mutex);
+
+	filelen = strlen(file);
+	if (filelen >= TEE_RPMB_FS_FILENAME_LENGTH - 1 || filelen == 0) {
+		res = TEE_ERROR_BAD_PARAMETERS;
+		goto out;
+	}
+
+	if (file[filelen - 1] == '/') {
+		res = TEE_ERROR_BAD_PARAMETERS;
+		goto out;
+	}
+
+	fh = alloc_file_handle(file);
+	if (!fh) {
+		res = TEE_ERROR_OUT_OF_MEMORY;
+		goto out;
+	}
+
+	/* We need to do setup in order to make sure fs_par is filled in */
+	res = rpmb_fs_setup();
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	if (create) {
+		/* Upper memory allocation must be used for RPMB_FS. */
+		pool_result = tee_mm_init(&p,
+					  RPMB_STORAGE_START_ADDRESS,
+					  fs_par->max_rpmb_address,
+					  RPMB_BLOCK_SIZE_SHIFT,
+					  TEE_MM_POOL_HI_ALLOC);
+
+		if (!pool_result) {
+			res = TEE_ERROR_OUT_OF_MEMORY;
+			goto out;
+		}
+
+		res = read_fat(fh, &p);
+		tee_mm_final(&p);
+		if (res != TEE_SUCCESS)
+			goto out;
+	} else {
+		res = read_fat(fh, NULL);
+		if (res != TEE_SUCCESS)
+			goto out;
+	}
+
+	/*
+	 * If this is opened with create and the entry found was not active
+	 * then this is a new file and the FAT entry must be written
+	 */
+	if (create) {
+		if ((fh->fat_entry.flags & FILE_IS_ACTIVE) == 0) {
+			memset(&fh->fat_entry, 0,
+				sizeof(struct rpmb_fat_entry));
+			memcpy(fh->fat_entry.filename, file, strlen(file));
+			/* Start address and size are 0 */
+			fh->fat_entry.flags = FILE_IS_ACTIVE;
+
+			res = generate_fek(&fh->fat_entry);
+			if (res != TEE_SUCCESS)
+				goto out;
+			DMSG("GENERATE FEK key: %p",
+			     (void *)fh->fat_entry.fek);
+			DHEXDUMP(fh->fat_entry.fek, sizeof(fh->fat_entry.fek));
+
+			res = write_fat_entry(fh, true);
+			if (res != TEE_SUCCESS)
+				goto out;
+		}
+	}
+
+	res = TEE_SUCCESS;
+
+out:
+	if (res == TEE_SUCCESS)
+		*ret_fh = (struct tee_file_handle *)fh;
+	else
+		free(fh);
+
+	mutex_unlock(&rpmb_mutex);
+	return res;
+}
+
+static void rpmb_fs_close(struct tee_file_handle **tfh)
+{
+	struct rpmb_file_handle *fh = (struct rpmb_file_handle *)*tfh;
+
+	free(fh);
+	*tfh = NULL;
+}
+
+static TEE_Result rpmb_fs_read(struct tee_file_handle *tfh, void *buf,
+			       size_t *len)
+{
+	TEE_Result res;
+	struct rpmb_file_handle *fh = (struct rpmb_file_handle *)tfh;
+	size_t size = *len;
+
+	if (!size)
+		return TEE_SUCCESS;
+
+	mutex_lock(&rpmb_mutex);
+
+	dump_fh(fh);
+
+	res = read_fat(fh, NULL);
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	if (fh->pos >= fh->fat_entry.data_size) {
+		*len = 0;
+		goto out;
+	}
+
+	size = MIN(size, fh->fat_entry.data_size - fh->pos);
+	if (size) {
+		res = tee_rpmb_read(CFG_RPMB_FS_DEV_ID,
+				    fh->fat_entry.start_address + fh->pos, buf,
+				    size, fh->fat_entry.fek);
+		if (res != TEE_SUCCESS)
+			goto out;
+	}
+	*len = size;
+	fh->pos += size;
+
+out:
+	mutex_unlock(&rpmb_mutex);
+	return res;
+}
+
+static TEE_Result rpmb_fs_write(struct tee_file_handle *tfh, const void *buf,
+				size_t size)
+{
+	TEE_Result res;
+	struct rpmb_file_handle *fh = (struct rpmb_file_handle *)tfh;
+	tee_mm_pool_t p;
+	bool pool_result = false;
+	tee_mm_entry_t *mm;
+	size_t end;
+	size_t newsize;
+	uint8_t *newbuf = NULL;
+	uintptr_t newaddr;
+	uint32_t start_addr;
+
+	if (!size)
+		return TEE_SUCCESS;
+
+	mutex_lock(&rpmb_mutex);
+
+	if (!fs_par) {
+		res = TEE_ERROR_GENERIC;
+		goto out;
+	}
+
+	dump_fh(fh);
+
+	/* Upper memory allocation must be used for RPMB_FS. */
+	pool_result = tee_mm_init(&p,
+				  RPMB_STORAGE_START_ADDRESS,
+				  fs_par->max_rpmb_address,
+				  RPMB_BLOCK_SIZE_SHIFT,
+				  TEE_MM_POOL_HI_ALLOC);
+	if (!pool_result) {
+		res = TEE_ERROR_OUT_OF_MEMORY;
+		goto out;
+	}
+
+	res = read_fat(fh, &p);
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	if (fh->fat_entry.flags & FILE_IS_LAST_ENTRY)
+		panic("invalid last entry flag");
+
+	end = fh->pos + size;
+	start_addr = fh->fat_entry.start_address + fh->pos;
+
+	if (end <= fh->fat_entry.data_size &&
+	    tee_rpmb_write_is_atomic(CFG_RPMB_FS_DEV_ID, start_addr, size)) {
+
+		DMSG("Updating data in-place");
+		res = tee_rpmb_write(CFG_RPMB_FS_DEV_ID, start_addr, buf,
+				     size, fh->fat_entry.fek);
+		if (res != TEE_SUCCESS)
+			goto out;
+	} else {
+		/*
+		 * File must be extended, or update cannot be atomic: allocate,
+		 * read, update, write.
+		 */
+
+		DMSG("Need to re-allocate");
+		newsize = MAX(end, fh->fat_entry.data_size);
+		mm = tee_mm_alloc(&p, newsize);
+		newbuf = calloc(newsize, 1);
+		if (!mm || !newbuf) {
+			res = TEE_ERROR_OUT_OF_MEMORY;
+			goto out;
+		}
+
+		if (fh->fat_entry.data_size) {
+			res = tee_rpmb_read(CFG_RPMB_FS_DEV_ID,
+					    fh->fat_entry.start_address,
+					    newbuf, fh->fat_entry.data_size,
+					    fh->fat_entry.fek);
+			if (res != TEE_SUCCESS)
+				goto out;
+		}
+
+		memcpy(newbuf + fh->pos, buf, size);
+
+		newaddr = tee_mm_get_smem(mm);
+		res = tee_rpmb_write(CFG_RPMB_FS_DEV_ID, newaddr, newbuf,
+				     newsize, fh->fat_entry.fek);
+		if (res != TEE_SUCCESS)
+			goto out;
+
+		fh->fat_entry.data_size = newsize;
+		fh->fat_entry.start_address = newaddr;
+		res = write_fat_entry(fh, true);
+		if (res != TEE_SUCCESS)
+			goto out;
+	}
+
+	fh->pos += size;
+out:
+	mutex_unlock(&rpmb_mutex);
+	if (pool_result)
+		tee_mm_final(&p);
+	if (newbuf)
+		free(newbuf);
+
+	return res;
+}
+
+static TEE_Result rpmb_fs_seek(struct tee_file_handle *tfh, int32_t offset,
+			       TEE_Whence whence, int32_t *new_offs)
+
+{
+	struct rpmb_file_handle *fh = (struct rpmb_file_handle *)tfh;
+	TEE_Result res;
+	tee_fs_off_t new_pos;
+
+	mutex_lock(&rpmb_mutex);
+
+	res = read_fat(fh, NULL);
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	switch (whence) {
+	case TEE_DATA_SEEK_SET:
+		new_pos = offset;
+		break;
+
+	case TEE_DATA_SEEK_CUR:
+		new_pos = fh->pos + offset;
+		break;
+
+	case TEE_DATA_SEEK_END:
+		new_pos = fh->fat_entry.data_size + offset;
+		break;
+
+	default:
+		res = TEE_ERROR_BAD_PARAMETERS;
+		goto out;
+	}
+
+	if (new_pos < 0)
+		new_pos = 0;
+
+	if (new_pos > TEE_DATA_MAX_POSITION) {
+		EMSG("Position is beyond TEE_DATA_MAX_POSITION");
+		res = TEE_ERROR_BAD_PARAMETERS;
+		goto out;
+	}
+
+	fh->pos = new_pos;
+	if (new_offs)
+		*new_offs = new_pos;
+out:
+	mutex_unlock(&rpmb_mutex);
+	return res;
+}
+
+static TEE_Result rpmb_fs_remove(const char *filename)
+{
+	TEE_Result res = TEE_ERROR_GENERIC;
+	struct rpmb_file_handle *fh = NULL;
+
+	mutex_lock(&rpmb_mutex);
+
+	if (!filename || strlen(filename) >= TEE_RPMB_FS_FILENAME_LENGTH - 1) {
+		res = TEE_ERROR_BAD_PARAMETERS;
+		goto out;
+	}
+
+	fh = alloc_file_handle(filename);
+	if (!fh) {
+		res = TEE_ERROR_OUT_OF_MEMORY;
+		goto out;
+	}
+
+	res = read_fat(fh, NULL);
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	/* Clear this file entry. */
+	memset(&fh->fat_entry, 0, sizeof(struct rpmb_fat_entry));
+	res = write_fat_entry(fh, false);
+
+out:
+	mutex_unlock(&rpmb_mutex);
+	free(fh);
+	return res;
+}
+
+static  TEE_Result rpmb_fs_rename(const char *old_name, const char *new_name,
+				  bool overwrite)
+{
+	TEE_Result res = TEE_ERROR_GENERIC;
+	struct rpmb_file_handle *fh_old = NULL;
+	struct rpmb_file_handle *fh_new = NULL;
+	uint32_t old_len;
+	uint32_t new_len;
+
+	mutex_lock(&rpmb_mutex);
+
+	if (!old_name || !new_name) {
+		res = TEE_ERROR_BAD_PARAMETERS;
+		goto out;
+	}
+
+	old_len = strlen(old_name);
+	new_len = strlen(new_name);
+
+	if ((old_len >= TEE_RPMB_FS_FILENAME_LENGTH - 1) ||
+	    (new_len >= TEE_RPMB_FS_FILENAME_LENGTH - 1) || (new_len == 0)) {
+
+		res = TEE_ERROR_BAD_PARAMETERS;
+		goto out;
+	}
+
+	fh_old = alloc_file_handle(old_name);
+	if (!fh_old) {
+		res = TEE_ERROR_OUT_OF_MEMORY;
+		goto out;
+	}
+
+	fh_new = alloc_file_handle(new_name);
+	if (!fh_new) {
+		res = TEE_ERROR_OUT_OF_MEMORY;
+		goto out;
+	}
+
+	res = read_fat(fh_old, NULL);
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	res = read_fat(fh_new, NULL);
+	if (res == TEE_SUCCESS) {
+		if (!overwrite) {
+			res = TEE_ERROR_BAD_PARAMETERS;
+			goto out;
+		}
+
+		/* Clear this file entry. */
+		memset(&fh_new->fat_entry, 0, sizeof(struct rpmb_fat_entry));
+		res = write_fat_entry(fh_new, false);
+		if (res != TEE_SUCCESS)
+			goto out;
+	}
+
+	memset(fh_old->fat_entry.filename, 0, TEE_RPMB_FS_FILENAME_LENGTH);
+	memcpy(fh_old->fat_entry.filename, new_name, new_len);
+
+	res = write_fat_entry(fh_old, false);
+
+out:
+	mutex_unlock(&rpmb_mutex);
+	free(fh_old);
+	free(fh_new);
+
+	return res;
+}
+
+static TEE_Result rpmb_fs_truncate(struct tee_file_handle *tfh, size_t length)
+{
+	struct rpmb_file_handle *fh = (struct rpmb_file_handle *)tfh;
+	tee_mm_pool_t p;
+	bool pool_result = false;
+	tee_mm_entry_t *mm;
+	uint32_t newsize;
+	uint8_t *newbuf = NULL;
+	uintptr_t newaddr;
+	TEE_Result res = TEE_ERROR_GENERIC;
+
+	mutex_lock(&rpmb_mutex);
+
+	if (length > INT32_MAX) {
+		res = TEE_ERROR_BAD_PARAMETERS;
+		goto out;
+	}
+	newsize = length;
+
+	res = read_fat(fh, NULL);
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	if (newsize > fh->fat_entry.data_size) {
+		/* Extend file */
+
+		pool_result = tee_mm_init(&p,
+					  RPMB_STORAGE_START_ADDRESS,
+					  fs_par->max_rpmb_address,
+					  RPMB_BLOCK_SIZE_SHIFT,
+					  TEE_MM_POOL_HI_ALLOC);
+		if (!pool_result) {
+			res = TEE_ERROR_OUT_OF_MEMORY;
+			goto out;
+		}
+		res = read_fat(fh, &p);
+		if (res != TEE_SUCCESS)
+			goto out;
+
+		mm = tee_mm_alloc(&p, newsize);
+		newbuf = calloc(newsize, 1);
+		if (!mm || !newbuf) {
+			res = TEE_ERROR_OUT_OF_MEMORY;
+			goto out;
+		}
+
+		if (fh->fat_entry.data_size) {
+			res = tee_rpmb_read(CFG_RPMB_FS_DEV_ID,
+					    fh->fat_entry.start_address,
+					    newbuf, fh->fat_entry.data_size,
+					    fh->fat_entry.fek);
+			if (res != TEE_SUCCESS)
+				goto out;
+		}
+
+		newaddr = tee_mm_get_smem(mm);
+		res = tee_rpmb_write(CFG_RPMB_FS_DEV_ID, newaddr, newbuf,
+				     newsize, fh->fat_entry.fek);
+		if (res != TEE_SUCCESS)
+			goto out;
+
+	} else {
+		/* Don't change file location */
+		newaddr = fh->fat_entry.start_address;
+	}
+
+	/* fh->pos is unchanged */
+	fh->fat_entry.data_size = newsize;
+	fh->fat_entry.start_address = newaddr;
+	res = write_fat_entry(fh, true);
+
+out:
+	mutex_unlock(&rpmb_mutex);
+	if (pool_result)
+		tee_mm_final(&p);
+	if (newbuf)
+		free(newbuf);
+
+	return res;
+}
+
+static void rpmb_fs_dir_free(struct tee_fs_dir *dir)
+{
+	struct tee_rpmb_fs_dirent *e;
+
+	if (!dir)
+		return;
+
+	free(dir->current);
+
+	while ((e = SIMPLEQ_FIRST(&dir->next))) {
+		SIMPLEQ_REMOVE_HEAD(&dir->next, link);
+		free(e);
+	}
+}
+
+static TEE_Result rpmb_fs_dir_populate(const char *path,
+				       struct tee_fs_dir *dir)
+{
+	struct tee_rpmb_fs_dirent *current = NULL;
+	struct rpmb_fat_entry *fat_entries = NULL;
+	uint32_t fat_address;
+	uint32_t filelen;
+	char *filename;
+	int i;
+	bool last_entry_found = false;
+	bool matched;
+	struct tee_rpmb_fs_dirent *next = NULL;
+	uint32_t pathlen;
+	TEE_Result res = TEE_ERROR_GENERIC;
+	uint32_t size;
+	char temp;
+
+	mutex_lock(&rpmb_mutex);
+
+	res = rpmb_fs_setup();
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	res = get_fat_start_address(&fat_address);
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	size = N_ENTRIES * sizeof(struct rpmb_fat_entry);
+	fat_entries = malloc(size);
+	if (!fat_entries) {
+		res = TEE_ERROR_OUT_OF_MEMORY;
+		goto out;
+	}
+
+	pathlen = strlen(path);
+	while (!last_entry_found) {
+		res = tee_rpmb_read(CFG_RPMB_FS_DEV_ID, fat_address,
+				    (uint8_t *)fat_entries, size, NULL);
+		if (res != TEE_SUCCESS)
+			goto out;
+
+		for (i = 0; i < N_ENTRIES; i++) {
+			filename = fat_entries[i].filename;
+			if (fat_entries[i].flags & FILE_IS_ACTIVE) {
+				matched = false;
+				filelen = strlen(filename);
+				if (filelen > pathlen) {
+					temp = filename[pathlen];
+					filename[pathlen] = '\0';
+					if (strcmp(filename, path) == 0)
+						matched = true;
+
+					filename[pathlen] = temp;
+				}
+
+				if (matched) {
+					next = malloc(sizeof(*next));
+					if (!next) {
+						res = TEE_ERROR_OUT_OF_MEMORY;
+						goto out;
+					}
+
+					memset(next, 0, sizeof(*next));
+					next->entry.d_name = next->name;
+					memcpy(next->name,
+						&filename[pathlen],
+						filelen - pathlen);
+
+					SIMPLEQ_INSERT_TAIL(&dir->next, next,
+							    link);
+					current = next;
+				}
+			}
+
+			if (fat_entries[i].flags & FILE_IS_LAST_ENTRY) {
+				last_entry_found = true;
+				break;
+			}
+
+			/* Move to next fat_entry. */
+			fat_address += sizeof(struct rpmb_fat_entry);
+		}
+	}
+
+	if (current)
+		res = TEE_SUCCESS;
+	else
+		res = TEE_ERROR_ITEM_NOT_FOUND; /* No directories were found. */
+
+out:
+	mutex_unlock(&rpmb_mutex);
+	if (res != TEE_SUCCESS)
+		rpmb_fs_dir_free(dir);
+	if (fat_entries)
+		free(fat_entries);
+
+	return res;
+}
+
+static TEE_Result rpmb_fs_opendir(const char *path, struct tee_fs_dir **dir)
+{
+	uint32_t len;
+	uint32_t max_size;
+	char path_local[TEE_RPMB_FS_FILENAME_LENGTH];
+	TEE_Result res = TEE_ERROR_GENERIC;
+	struct tee_fs_dir *rpmb_dir = NULL;
+
+	if (!path || !dir) {
+		res = TEE_ERROR_BAD_PARAMETERS;
+		goto out;
+	}
+
+	/*
+	 * There must be room for at least the NULL char and a char for the
+	 * filename after the path.
+	 */
+	max_size = TEE_RPMB_FS_FILENAME_LENGTH - 2;
+	len = strlen(path);
+	if (len > max_size || len == 0) {
+		res = TEE_ERROR_BAD_PARAMETERS;
+		goto out;
+	}
+
+	memset(path_local, 0, sizeof(path_local));
+	memcpy(path_local, path, len);
+
+	/* Add a slash to correctly match the full directory name. */
+	if (path_local[len - 1] != '/')
+		path_local[len] = '/';
+
+	rpmb_dir = calloc(1, sizeof(*rpmb_dir));
+	if (!rpmb_dir) {
+		res = TEE_ERROR_OUT_OF_MEMORY;
+		goto out;
+	}
+	SIMPLEQ_INIT(&rpmb_dir->next);
+
+	res = rpmb_fs_dir_populate(path_local, rpmb_dir);
+	if (res != TEE_SUCCESS) {
+		free(rpmb_dir);
+		rpmb_dir = NULL;
+		goto out;
+	}
+
+	*dir = rpmb_dir;
+
+out:
+	return res;
+}
+
+static TEE_Result rpmb_fs_readdir(struct tee_fs_dir *dir,
+				  struct tee_fs_dirent **ent)
+{
+	if (!dir)
+		return TEE_ERROR_GENERIC;
+
+	free(dir->current);
+
+	dir->current = SIMPLEQ_FIRST(&dir->next);
+	if (!dir->current)
+		return TEE_ERROR_ITEM_NOT_FOUND;
+
+	SIMPLEQ_REMOVE_HEAD(&dir->next, link);
+
+	*ent = &dir->current->entry;
+	return TEE_SUCCESS;
+}
+
+static void rpmb_fs_closedir(struct tee_fs_dir *dir)
+{
+	if (dir) {
+		rpmb_fs_dir_free(dir);
+		free(dir);
+	}
+}
+
+static TEE_Result rpmb_fs_open(const char *file, struct tee_file_handle **fh)
+{
+	return rpmb_fs_open_internal(file, false, fh);
+}
+
+static TEE_Result rpmb_fs_create(const char *file, struct tee_file_handle **fh)
+{
+	return rpmb_fs_open_internal(file, true, fh);
+}
+
+const struct tee_file_operations rpmb_fs_ops = {
+	.open = rpmb_fs_open,
+	.create = rpmb_fs_create,
+	.close = rpmb_fs_close,
+	.read = rpmb_fs_read,
+	.write = rpmb_fs_write,
+	.seek = rpmb_fs_seek,
+	.truncate = rpmb_fs_truncate,
+	.rename = rpmb_fs_rename,
+	.remove = rpmb_fs_remove,
+	.opendir = rpmb_fs_opendir,
+	.closedir = rpmb_fs_closedir,
+	.readdir = rpmb_fs_readdir,
+};
diff --git a/core/tee/tee_sql_fs.c b/core/tee/tee_sql_fs.c
new file mode 100644
index 0000000..e38e1bc
--- /dev/null
+++ b/core/tee/tee_sql_fs.c
@@ -0,0 +1,610 @@
+/*
+ * Copyright (c) 2016, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/*
+ * This file implements the tee_file_operations structure for a secure
+ * filesystem based on an SQLite database in normal world.
+ * The atomicity of each operation is ensured by using SQL transactions.
+ * The main purpose of the code below is to perform block encryption and
+ * authentication of the file data, and properly handle seeking through the
+ * file. One file (in the sense of struct tee_file_operations) maps to one
+ * file in the SQL filesystem, and has the following structure:
+ *
+ * [       File meta-data       ][      Block #0        ][Block #1]...
+ * [meta_header|sql_fs_file_meta][block_header|user data][        ]...
+ *
+ * meta_header and block_header are defined in tee_fs_key_manager.h.
+ */
+
+#include <assert.h>
+#include <kernel/mutex.h>
+#include <optee_msg_supplicant.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <string_ext.h>
+#include <sys/queue.h>
+#include <tee/tee_cryp_provider.h>
+#include <tee/tee_fs.h>
+#include <tee/tee_fs_defs.h>
+#include <tee/tee_fs_key_manager.h>
+#include <tee/tee_fs_rpc.h>
+#include <trace.h>
+#include <utee_defines.h>
+#include <util.h>
+
+/* Block size for encryption */
+#define BLOCK_SHIFT 12
+#define BLOCK_SIZE (1 << BLOCK_SHIFT)
+
+struct sql_fs_file_meta {
+	size_t length;
+};
+
+/* File descriptor */
+struct sql_fs_fd {
+	struct sql_fs_file_meta meta;
+	uint8_t encrypted_fek[TEE_FS_KM_FEK_SIZE];
+	tee_fs_off_t pos;
+	int fd; /* returned by normal world */
+	int flags; /* open flags */
+};
+
+struct tee_fs_dir {
+	int nw_dir;
+	struct tee_fs_dirent d;
+};
+
+static struct mutex sql_fs_mutex = MUTEX_INITIALIZER;
+
+/*
+ * Interface with tee-supplicant
+ */
+
+static TEE_Result sql_fs_begin_transaction_rpc(void)
+{
+	return tee_fs_rpc_begin_transaction(OPTEE_MSG_RPC_CMD_SQL_FS);
+}
+
+static TEE_Result sql_fs_end_transaction_rpc(bool rollback)
+{
+	return tee_fs_rpc_end_transaction(OPTEE_MSG_RPC_CMD_SQL_FS,
+					     rollback);
+}
+
+static TEE_Result sql_fs_opendir_rpc(const char *name, struct tee_fs_dir **d)
+{
+	return tee_fs_rpc_opendir(OPTEE_MSG_RPC_CMD_SQL_FS, name, d);
+}
+
+static TEE_Result sql_fs_readdir_rpc(struct tee_fs_dir *d,
+				     struct tee_fs_dirent **ent)
+{
+	return tee_fs_rpc_readdir(OPTEE_MSG_RPC_CMD_SQL_FS, d, ent);
+}
+
+static TEE_Result sql_fs_remove_rpc(const char *file)
+{
+	return tee_fs_rpc_remove(OPTEE_MSG_RPC_CMD_SQL_FS, file);
+}
+
+static TEE_Result sql_fs_rename_rpc(const char *old, const char *nw,
+				    bool overwrite)
+{
+	return tee_fs_rpc_rename(OPTEE_MSG_RPC_CMD_SQL_FS, old, nw, overwrite);
+}
+
+static void sql_fs_closedir_rpc(struct tee_fs_dir *d)
+{
+	if (d)
+		tee_fs_rpc_closedir(OPTEE_MSG_RPC_CMD_SQL_FS, d);
+}
+
+/*
+ * End of interface with tee-supplicant
+ */
+
+static size_t meta_size(void)
+{
+	return tee_fs_get_header_size(META_FILE) +
+	       sizeof(struct sql_fs_file_meta);
+}
+
+static size_t block_header_size(void)
+{
+	return tee_fs_get_header_size(BLOCK_FILE);
+}
+
+static size_t block_size_raw(void)
+{
+	return block_header_size() + BLOCK_SIZE;
+}
+
+/* Return the block number from a position in the user data */
+static ssize_t block_num(tee_fs_off_t pos)
+{
+	return pos / BLOCK_SIZE;
+}
+
+/* Return the position of a block in the DB file */
+static ssize_t block_pos_raw(size_t block_num)
+{
+	return meta_size() + block_num * block_size_raw();
+}
+
+static TEE_Result write_meta(struct sql_fs_fd *fdp)
+{
+	TEE_Result res;
+	size_t ct_size = meta_size();
+	void *ct;
+	struct tee_fs_rpc_operation op;
+
+	res = tee_fs_rpc_write_init(&op, OPTEE_MSG_RPC_CMD_SQL_FS, fdp->fd, 0,
+				    ct_size, &ct);
+	if (res != TEE_SUCCESS)
+		return res;
+
+
+	res = tee_fs_encrypt_file(META_FILE,
+				  (const uint8_t *)&fdp->meta,
+				  sizeof(fdp->meta), ct, &ct_size,
+				  fdp->encrypted_fek);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	return tee_fs_rpc_write_final(&op);
+}
+
+static TEE_Result create_meta(struct sql_fs_fd *fdp, const char *fname)
+{
+	TEE_Result res;
+
+	memset(&fdp->meta, 0, sizeof(fdp->meta));
+
+	res = tee_fs_generate_fek(fdp->encrypted_fek, TEE_FS_KM_FEK_SIZE);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = tee_fs_rpc_create(OPTEE_MSG_RPC_CMD_SQL_FS, fname, &fdp->fd);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	return write_meta(fdp);
+}
+
+static TEE_Result read_meta(struct sql_fs_fd *fdp, const char *fname)
+{
+	TEE_Result res;
+	size_t msize = meta_size();
+	size_t out_size = sizeof(fdp->meta);
+	void *meta;
+	size_t bytes;
+	struct tee_fs_rpc_operation op;
+
+	res = tee_fs_rpc_open(OPTEE_MSG_RPC_CMD_SQL_FS, fname, &fdp->fd);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = tee_fs_rpc_read_init(&op, OPTEE_MSG_RPC_CMD_SQL_FS, fdp->fd, 0,
+				   msize, &meta);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = tee_fs_rpc_read_final(&op, &bytes);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	return tee_fs_decrypt_file(META_FILE, meta, msize,
+				   (uint8_t *)&fdp->meta, &out_size,
+				   fdp->encrypted_fek);
+}
+
+/*
+ * Read one block of user data.
+ * Returns:
+ *  < 0: read error
+ *    0: block does not exist (reading past last block)
+ *  > 0: success
+ */
+static TEE_Result read_block(struct sql_fs_fd *fdp, size_t bnum, uint8_t *data)
+{
+	TEE_Result res;
+	size_t ct_size = block_size_raw();
+	size_t out_size = BLOCK_SIZE;
+	ssize_t pos = block_pos_raw(bnum);
+	size_t bytes;
+	void *ct;
+	struct tee_fs_rpc_operation op;
+
+	res = tee_fs_rpc_read_init(&op, OPTEE_MSG_RPC_CMD_SQL_FS, fdp->fd, pos,
+				   ct_size, &ct);
+	if (res != TEE_SUCCESS)
+		return res;
+	res = tee_fs_rpc_read_final(&op, &bytes);
+	if (res != TEE_SUCCESS)
+		return res;
+	if (!bytes)
+		return TEE_SUCCESS; /* Block does not exist */
+
+	return tee_fs_decrypt_file(BLOCK_FILE, ct, bytes, data,
+				   &out_size, fdp->encrypted_fek);
+}
+
+/* Write one block of user data */
+static TEE_Result write_block(struct sql_fs_fd *fdp, size_t bnum, uint8_t *data)
+{
+	TEE_Result res;
+	size_t ct_size = block_size_raw();
+	ssize_t pos = block_pos_raw(bnum);
+	void *ct;
+	struct tee_fs_rpc_operation op;
+
+	res = tee_fs_rpc_write_init(&op, OPTEE_MSG_RPC_CMD_SQL_FS, fdp->fd, pos,
+				    ct_size, &ct);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = tee_fs_encrypt_file(BLOCK_FILE, data, BLOCK_SIZE, ct,
+				  &ct_size, fdp->encrypted_fek);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	return tee_fs_rpc_write_final(&op);
+}
+
+/*
+ * Partial write (< BLOCK_SIZE) into a block: read/update/write
+ * To save memory, passing data == NULL is equivalent to passing a buffer
+ * filled with zeroes.
+ */
+static TEE_Result write_block_partial(struct sql_fs_fd *fdp, size_t bnum,
+				      const uint8_t *data, size_t len,
+				      size_t offset)
+{
+	TEE_Result res;
+	size_t buf_size = BLOCK_SIZE;
+	uint8_t *buf = NULL;
+
+	if ((offset >= buf_size) || (offset + len > buf_size))
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	buf = malloc(buf_size);
+	if (!buf)
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	res = read_block(fdp, bnum, buf);
+	if (res != TEE_SUCCESS)
+		goto exit;
+
+	if (data)
+		memcpy(buf + offset, data, len);
+	else
+		memset(buf + offset, 0, len);
+
+	res = write_block(fdp, bnum, buf);
+exit:
+	free(buf);
+	return res;
+}
+
+static TEE_Result sql_fs_ftruncate_internal(struct sql_fs_fd *fdp,
+					    tee_fs_off_t new_length)
+{
+	TEE_Result res;
+	tee_fs_off_t old_length;
+
+	old_length = (tee_fs_off_t)fdp->meta.length;
+
+	if (new_length == old_length)
+		return TEE_SUCCESS;
+
+	sql_fs_begin_transaction_rpc();
+
+	if (new_length < old_length) {
+		/* Trim unused blocks */
+		int old_last_block = block_num(old_length);
+		int last_block = block_num(new_length);
+		tee_fs_off_t off;
+
+		if (last_block < old_last_block) {
+			off = block_pos_raw(last_block);
+			res = tee_fs_rpc_truncate(OPTEE_MSG_RPC_CMD_SQL_FS,
+						  fdp->fd, off);
+			if (res != TEE_SUCCESS)
+				goto exit;
+		}
+	} else {
+		/* Extend file with zeroes */
+		tee_fs_off_t off = old_length % BLOCK_SIZE;
+		size_t bnum = block_num(old_length);
+		size_t end_bnum = block_num(new_length);
+
+		while (bnum <= end_bnum) {
+			size_t len = (size_t)BLOCK_SIZE - (size_t)off;
+
+			res = write_block_partial(fdp, bnum, NULL, len, off);
+			if (res != TEE_SUCCESS)
+				goto exit;
+			off = 0;
+			bnum++;
+		}
+	}
+
+	fdp->meta.length = new_length;
+	res = write_meta(fdp);
+exit:
+	sql_fs_end_transaction_rpc(res != TEE_SUCCESS);
+	return res;
+}
+
+static TEE_Result sql_fs_seek(struct tee_file_handle *fh, int32_t offset,
+			      TEE_Whence whence, int32_t *new_offs)
+{
+	TEE_Result res;
+	struct sql_fs_fd *fdp = (struct sql_fs_fd *)fh;
+	tee_fs_off_t pos;
+
+	mutex_lock(&sql_fs_mutex);
+
+	switch (whence) {
+	case TEE_DATA_SEEK_SET:
+		pos = offset;
+		break;
+
+	case TEE_DATA_SEEK_CUR:
+		pos = fdp->pos + offset;
+		break;
+
+	case TEE_DATA_SEEK_END:
+		pos = fdp->meta.length + offset;
+		break;
+
+	default:
+		res = TEE_ERROR_BAD_PARAMETERS;
+		goto exit_ret;
+	}
+
+	if (pos > TEE_DATA_MAX_POSITION) {
+		EMSG("Position is beyond TEE_DATA_MAX_POSITION");
+		res = TEE_ERROR_BAD_PARAMETERS;
+		goto exit_ret;
+	}
+
+	if (pos < 0)
+		pos = 0;
+
+	fdp->pos = pos;
+	if (new_offs)
+		*new_offs = pos;
+	res = TEE_SUCCESS;
+exit_ret:
+	mutex_unlock(&sql_fs_mutex);
+	return res;
+}
+
+static void sql_fs_close(struct tee_file_handle **fh)
+{
+	struct sql_fs_fd *fdp = (struct sql_fs_fd *)*fh;
+
+	if (fdp) {
+	tee_fs_rpc_close(OPTEE_MSG_RPC_CMD_SQL_FS, fdp->fd);
+		free(fdp);
+		*fh = NULL;
+	}
+}
+
+static TEE_Result open_internal(const char *file, bool create,
+				struct tee_file_handle **fh)
+{
+	TEE_Result res;
+	struct sql_fs_fd *fdp;
+	bool created = false;
+
+	fdp = calloc(1, sizeof(*fdp));
+	if (!fdp)
+		return TEE_ERROR_OUT_OF_MEMORY;
+	fdp->fd = -1;
+
+	mutex_lock(&sql_fs_mutex);
+
+	if (create)
+		res = create_meta(fdp, file);
+	else
+		res = read_meta(fdp, file);
+
+	if (res == TEE_SUCCESS) {
+		*fh = (struct tee_file_handle *)fdp;
+	} else {
+		if (fdp && fdp->fd != -1)
+			tee_fs_rpc_close(OPTEE_MSG_RPC_CMD_SQL_FS, fdp->fd);
+		if (created)
+			tee_fs_rpc_remove(OPTEE_MSG_RPC_CMD_SQL_FS, file);
+		free(fdp);
+	}
+	mutex_unlock(&sql_fs_mutex);
+	return res;
+}
+
+static TEE_Result sql_fs_open(const char *file, struct tee_file_handle **fh)
+{
+	return open_internal(file, false, fh);
+}
+
+static TEE_Result sql_fs_create(const char *file, struct tee_file_handle **fh)
+{
+	return open_internal(file, true, fh);
+}
+
+
+static TEE_Result sql_fs_read(struct tee_file_handle *fh, void *buf,
+			      size_t *len)
+{
+	TEE_Result res;
+	struct sql_fs_fd *fdp = (struct sql_fs_fd *)fh;
+	size_t remain_bytes = *len;
+	uint8_t *data_ptr = buf;
+	uint8_t *block = NULL;
+	int start_block_num;
+	int end_block_num;
+
+	mutex_lock(&sql_fs_mutex);
+
+	if ((fdp->pos + remain_bytes) < remain_bytes ||
+	    fdp->pos > (tee_fs_off_t)fdp->meta.length)
+		remain_bytes = 0;
+	else if (fdp->pos + remain_bytes > fdp->meta.length)
+		remain_bytes = fdp->meta.length - fdp->pos;
+
+	*len = remain_bytes;
+
+	if (!remain_bytes) {
+		res = TEE_SUCCESS;
+		goto exit;
+	}
+
+	start_block_num = block_num(fdp->pos);
+	end_block_num = block_num(fdp->pos + remain_bytes - 1);
+
+	block = malloc(BLOCK_SIZE);
+	if (!block) {
+		res = TEE_ERROR_OUT_OF_MEMORY;
+		goto exit;
+	}
+
+	while (start_block_num <= end_block_num) {
+		tee_fs_off_t offset = fdp->pos % BLOCK_SIZE;
+		size_t size_to_read = MIN(remain_bytes, (size_t)BLOCK_SIZE);
+
+		if (size_to_read + offset > BLOCK_SIZE)
+			size_to_read = BLOCK_SIZE - offset;
+
+		/*
+		 * REVISIT: implement read_block_partial() since we have
+		 * write_block_partial()?
+		 */
+		res = read_block(fdp, start_block_num, block);
+		if (res != TEE_SUCCESS)
+			goto exit;
+
+		memcpy(data_ptr, block + offset, size_to_read);
+
+		data_ptr += size_to_read;
+		remain_bytes -= size_to_read;
+		fdp->pos += size_to_read;
+
+		start_block_num++;
+	}
+	res = TEE_SUCCESS;
+exit:
+	free(block);
+	mutex_unlock(&sql_fs_mutex);
+	return res;
+}
+
+static TEE_Result sql_fs_write(struct tee_file_handle *fh, const void *buf,
+			       size_t len)
+{
+	TEE_Result res;
+	struct sql_fs_fd *fdp = (struct sql_fs_fd *)fh;
+	size_t remain_bytes = len;
+	const uint8_t *data_ptr = buf;
+	int start_block_num;
+	int end_block_num;
+
+	if (!len)
+		return TEE_SUCCESS;
+
+	mutex_lock(&sql_fs_mutex);
+
+	sql_fs_begin_transaction_rpc();
+
+	if (fdp->meta.length < (size_t)fdp->pos) {
+		/* Fill hole */
+		res = sql_fs_ftruncate_internal(fdp, fdp->pos);
+		if (res != TEE_SUCCESS)
+			goto exit;
+	}
+
+	start_block_num = block_num(fdp->pos);
+	end_block_num = block_num(fdp->pos + len - 1);
+
+	while (start_block_num <= end_block_num) {
+		tee_fs_off_t offset = fdp->pos % BLOCK_SIZE;
+		size_t size_to_write = MIN(remain_bytes, (size_t)BLOCK_SIZE);
+
+		if (size_to_write + offset > BLOCK_SIZE)
+			size_to_write = BLOCK_SIZE - offset;
+
+		res = write_block_partial(fdp, start_block_num, data_ptr,
+					  size_to_write, offset);
+		if (res != TEE_SUCCESS)
+			goto exit;
+
+		data_ptr += size_to_write;
+		remain_bytes -= size_to_write;
+		fdp->pos += size_to_write;
+
+		start_block_num++;
+	}
+
+	if (fdp->meta.length < (size_t)fdp->pos) {
+		fdp->meta.length = fdp->pos;
+		res = write_meta(fdp);
+	}
+exit:
+	sql_fs_end_transaction_rpc(res != TEE_SUCCESS);
+	mutex_unlock(&sql_fs_mutex);
+	return res;
+}
+
+static TEE_Result sql_fs_truncate(struct tee_file_handle *fh, size_t len)
+{
+	TEE_Result res;
+	struct sql_fs_fd *fdp = (struct sql_fs_fd *)fh;
+
+	mutex_lock(&sql_fs_mutex);
+	res = sql_fs_ftruncate_internal(fdp, len);
+	mutex_unlock(&sql_fs_mutex);
+
+	return res;
+}
+
+const struct tee_file_operations sql_fs_ops = {
+	.open = sql_fs_open,
+	.create = sql_fs_create,
+	.close = sql_fs_close,
+	.read = sql_fs_read,
+	.write = sql_fs_write,
+	.seek = sql_fs_seek,
+	.truncate = sql_fs_truncate,
+
+	.opendir = sql_fs_opendir_rpc,
+	.closedir = sql_fs_closedir_rpc,
+	.readdir = sql_fs_readdir_rpc,
+	.rename = sql_fs_rename_rpc,
+	.remove = sql_fs_remove_rpc,
+};
diff --git a/core/tee/tee_svc.c b/core/tee/tee_svc.c
new file mode 100644
index 0000000..0c90443
--- /dev/null
+++ b/core/tee/tee_svc.c
@@ -0,0 +1,1111 @@
+/*
+ * Copyright (c) 2014, STMicroelectronics International N.V.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+#include <util.h>
+#include <kernel/tee_common_otp.h>
+#include <kernel/tee_common.h>
+#include <tee_api_types.h>
+#include <kernel/tee_ta_manager.h>
+#include <utee_types.h>
+#include <tee/tee_svc.h>
+#include <tee/tee_cryp_utl.h>
+#include <mm/tee_mmu.h>
+#include <mm/tee_mm.h>
+#include <mm/core_memprot.h>
+#include <kernel/tee_time.h>
+
+#include <user_ta_header.h>
+#include <trace.h>
+#include <kernel/trace_ta.h>
+#include <kernel/chip_services.h>
+#include <kernel/pseudo_ta.h>
+#include <mm/mobj.h>
+
+vaddr_t tee_svc_uref_base;
+
+void syscall_log(const void *buf __maybe_unused, size_t len __maybe_unused)
+{
+#ifdef CFG_TEE_CORE_TA_TRACE
+	char *kbuf;
+
+	if (len == 0)
+		return;
+
+	kbuf = malloc(len + 1);
+	if (kbuf == NULL)
+		return;
+
+	if (tee_svc_copy_from_user(kbuf, buf, len) == TEE_SUCCESS) {
+		kbuf[len] = '\0';
+		trace_ext_puts(kbuf);
+	}
+
+	free(kbuf);
+#endif
+}
+
+TEE_Result syscall_not_supported(void)
+{
+	return TEE_ERROR_NOT_SUPPORTED;
+}
+
+/* Configuration properties */
+/* API implementation version */
+static const char api_vers[] = TO_STR(CFG_TEE_API_VERSION);
+
+/* Implementation description (implementation-dependent) */
+static const char descr[] = TO_STR(CFG_TEE_IMPL_DESCR);
+
+/*
+ * TA persistent time protection level
+ * 100: Persistent time based on an REE-controlled real-time clock
+ * and on the TEE Trusted Storage for the storage of origins (default).
+ * 1000: Persistent time based on a TEE-controlled real-time clock
+ * and the TEE Trusted Storage.
+ * The real-time clock MUST be out of reach of software attacks
+ * from the REE.
+ */
+static const uint32_t ta_time_prot_lvl = 100;
+
+/* Elliptic Curve Cryptographic support */
+#ifdef CFG_CRYPTO_ECC
+static const uint32_t crypto_ecc_en = 1;
+#else
+static const uint32_t crypto_ecc_en;
+#endif
+
+/*
+ * Trusted storage anti rollback protection level
+ * 0 (or missing): No antirollback protection (default)
+ * 100: Antirollback enforced at REE level
+ * 1000: Antirollback TEE-controlled hardware
+ */
+static const uint32_t ts_antiroll_prot_lvl;
+
+/* Trusted OS implementation version */
+static const char trustedos_impl_version[] = TO_STR(TEE_IMPL_VERSION);
+
+/* Trusted OS implementation version (binary value) */
+static const uint32_t trustedos_impl_bin_version; /* 0 by default */
+
+/* Trusted OS implementation manufacturer name */
+static const char trustedos_manufacturer[] = TO_STR(CFG_TEE_MANUFACTURER);
+
+/* Trusted firmware version */
+static const char fw_impl_version[] = TO_STR(CFG_TEE_FW_IMPL_VERSION);
+
+/* Trusted firmware version (binary value) */
+static const uint32_t fw_impl_bin_version; /* 0 by default */
+
+/* Trusted firmware manufacturer name */
+static const char fw_manufacturer[] = TO_STR(CFG_TEE_FW_MANUFACTURER);
+
+static TEE_Result get_prop_tee_dev_id(struct tee_ta_session *sess __unused,
+				      void *buf, size_t *blen)
+{
+	TEE_Result res;
+	TEE_UUID uuid;
+	const size_t nslen = 5;
+	uint8_t data[5 + FVR_DIE_ID_NUM_REGS * sizeof(uint32_t)] = {
+	    'O', 'P', 'T', 'E', 'E' };
+
+	if (*blen < sizeof(uuid)) {
+		*blen = sizeof(uuid);
+		return TEE_ERROR_SHORT_BUFFER;
+	}
+	*blen = sizeof(uuid);
+
+	if (tee_otp_get_die_id(data + nslen, sizeof(data) - nslen))
+		return TEE_ERROR_BAD_STATE;
+
+	res = tee_hash_createdigest(TEE_ALG_SHA256, data, sizeof(data),
+				    (uint8_t *)&uuid, sizeof(uuid));
+	if (res != TEE_SUCCESS)
+		return TEE_ERROR_BAD_STATE;
+
+	/*
+	 * Changes the random value into and UUID as specifiec
+	 * in RFC 4122. The magic values are from the example
+	 * code in the RFC.
+	 *
+	 * TEE_UUID is defined slightly different from the RFC,
+	 * but close enough for our purpose.
+	 */
+
+	uuid.timeHiAndVersion &= 0x0fff;
+	uuid.timeHiAndVersion |= 5 << 12;
+
+	/* uuid.clock_seq_hi_and_reserved in the RFC */
+	uuid.clockSeqAndNode[0] &= 0x3f;
+	uuid.clockSeqAndNode[0] |= 0x80;
+
+	return tee_svc_copy_to_user(buf, &uuid, sizeof(TEE_UUID));
+}
+
+static TEE_Result get_prop_tee_sys_time_prot_level(
+			struct tee_ta_session *sess __unused,
+			void *buf, size_t *blen)
+{
+	uint32_t prot;
+
+	if (*blen < sizeof(prot)) {
+		*blen = sizeof(prot);
+		return TEE_ERROR_SHORT_BUFFER;
+	}
+	*blen = sizeof(prot);
+	prot = tee_time_get_sys_time_protection_level();
+	return tee_svc_copy_to_user(buf, &prot, sizeof(prot));
+}
+
+static TEE_Result get_prop_client_id(struct tee_ta_session *sess __unused,
+				     void *buf, size_t *blen)
+{
+	if (*blen < sizeof(TEE_Identity)) {
+		*blen = sizeof(TEE_Identity);
+		return TEE_ERROR_SHORT_BUFFER;
+	}
+	*blen = sizeof(TEE_Identity);
+	return tee_svc_copy_to_user(buf, &sess->clnt_id, sizeof(TEE_Identity));
+}
+
+static TEE_Result get_prop_ta_app_id(struct tee_ta_session *sess,
+				     void *buf, size_t *blen)
+{
+	if (*blen < sizeof(TEE_UUID)) {
+		*blen = sizeof(TEE_UUID);
+		return TEE_ERROR_SHORT_BUFFER;
+	}
+	*blen = sizeof(TEE_UUID);
+	return tee_svc_copy_to_user(buf, &sess->ctx->uuid, sizeof(TEE_UUID));
+}
+
+/* Properties of the set TEE_PROPSET_CURRENT_CLIENT */
+const struct tee_props tee_propset_client[] = {
+	{
+		.name = "gpd.client.identity",
+		.prop_type = USER_TA_PROP_TYPE_IDENTITY,
+		.get_prop_func = get_prop_client_id
+	},
+};
+
+/* Properties of the set TEE_PROPSET_CURRENT_TA */
+const struct tee_props tee_propset_ta[] = {
+	{
+		.name = "gpd.ta.appID",
+		.prop_type = USER_TA_PROP_TYPE_UUID,
+		.get_prop_func = get_prop_ta_app_id
+	},
+
+	/*
+	 * Following properties are processed directly in libutee:
+	 *	TA_PROP_STR_SINGLE_INSTANCE
+	 *	TA_PROP_STR_MULTI_SESSION
+	 *	TA_PROP_STR_KEEP_ALIVE
+	 *	TA_PROP_STR_DATA_SIZE
+	 *	TA_PROP_STR_STACK_SIZE
+	 *	TA_PROP_STR_VERSION
+	 *	TA_PROP_STR_DESCRIPTION
+	 *	USER_TA_PROP_TYPE_STRING,
+	 *	TA_DESCRIPTION
+	 */
+};
+
+/* Properties of the set TEE_PROPSET_TEE_IMPLEMENTATION */
+const struct tee_props tee_propset_tee[] = {
+	{
+		.name = "gpd.tee.apiversion",
+		.prop_type = USER_TA_PROP_TYPE_STRING,
+		.data = api_vers,
+		.len = sizeof(api_vers),
+	},
+	{
+		.name = "gpd.tee.description",
+		.prop_type = USER_TA_PROP_TYPE_STRING,
+		.data = descr, .len = sizeof(descr)
+	},
+	{
+		.name = "gpd.tee.deviceID",
+		.prop_type = USER_TA_PROP_TYPE_UUID,
+		.get_prop_func = get_prop_tee_dev_id
+	},
+	{
+		.name = "gpd.tee.systemTime.protectionLevel",
+		.prop_type = USER_TA_PROP_TYPE_U32,
+		.get_prop_func = get_prop_tee_sys_time_prot_level
+	},
+	{
+		.name = "gpd.tee.TAPersistentTime.protectionLevel",
+		.prop_type = USER_TA_PROP_TYPE_U32,
+		.data = &ta_time_prot_lvl,
+		.len = sizeof(ta_time_prot_lvl)
+	},
+	{
+		.name = "gpd.tee.cryptography.ecc",
+		.prop_type = USER_TA_PROP_TYPE_BOOL,
+		.data = &crypto_ecc_en,
+		.len = sizeof(crypto_ecc_en)
+	},
+	{
+		.name = "gpd.tee.trustedStorage.antiRollback.protectionLevel",
+		.prop_type = USER_TA_PROP_TYPE_U32,
+		.data = &ts_antiroll_prot_lvl,
+		.len = sizeof(ts_antiroll_prot_lvl)
+	},
+	{
+		.name = "gpd.tee.trustedos.implementation.version",
+		.prop_type = USER_TA_PROP_TYPE_STRING,
+		.data = trustedos_impl_version,
+		.len = sizeof(trustedos_impl_version)
+	},
+	{
+		.name = "gpd.tee.trustedos.implementation.binaryversion",
+		.prop_type = USER_TA_PROP_TYPE_U32,
+		.data = &trustedos_impl_bin_version,
+		.len = sizeof(trustedos_impl_bin_version)
+	},
+	{
+		.name = "gpd.tee.trustedos.manufacturer",
+		.prop_type = USER_TA_PROP_TYPE_STRING,
+		.data = trustedos_manufacturer,
+		.len = sizeof(trustedos_manufacturer)
+	},
+	{
+		.name = "gpd.tee.firmware.implementation.version",
+		.prop_type = USER_TA_PROP_TYPE_STRING,
+		.data = fw_impl_version,
+		.len = sizeof(fw_impl_version)
+	},
+	{
+		.name = "gpd.tee.firmware.implementation.binaryversion",
+		.prop_type = USER_TA_PROP_TYPE_U32,
+		.data = &fw_impl_bin_version,
+		.len = sizeof(fw_impl_bin_version)
+	},
+	{
+		.name = "gpd.tee.firmware.manufacturer",
+		.prop_type = USER_TA_PROP_TYPE_STRING,
+		.data = fw_manufacturer,
+		.len = sizeof(fw_manufacturer)
+	},
+
+	/*
+	 * Following properties are processed directly in libutee:
+	 *	gpd.tee.arith.maxBigIntSize
+	 */
+};
+
+__weak const struct tee_vendor_props vendor_props_client;
+__weak const struct tee_vendor_props vendor_props_ta;
+__weak const struct tee_vendor_props vendor_props_tee;
+
+static void get_prop_set(unsigned long prop_set,
+			 const struct tee_props **props,
+			 size_t *size,
+			 const struct tee_props **vendor_props,
+			 size_t *vendor_size)
+{
+	if ((TEE_PropSetHandle)prop_set == TEE_PROPSET_CURRENT_CLIENT) {
+		*props = tee_propset_client;
+		*size = ARRAY_SIZE(tee_propset_client);
+		*vendor_props = vendor_props_client.props;
+		*vendor_size = vendor_props_client.len;
+	} else if ((TEE_PropSetHandle)prop_set == TEE_PROPSET_CURRENT_TA) {
+		*props = tee_propset_ta;
+		*size = ARRAY_SIZE(tee_propset_ta);
+		*vendor_props = vendor_props_ta.props;
+		*vendor_size = vendor_props_ta.len;
+	} else if ((TEE_PropSetHandle)prop_set ==
+		   TEE_PROPSET_TEE_IMPLEMENTATION) {
+		*props = tee_propset_tee;
+		*size = ARRAY_SIZE(tee_propset_tee);
+		*vendor_props = vendor_props_tee.props;
+		*vendor_size = vendor_props_tee.len;
+	} else {
+		*props = NULL;
+		*size = 0;
+		*vendor_props = NULL;
+		*vendor_size = 0;
+	}
+}
+
+static const struct tee_props *get_prop_struct(unsigned long prop_set,
+					       unsigned long index)
+{
+	const struct tee_props *props;
+	const struct tee_props *vendor_props;
+	size_t size;
+	size_t vendor_size;
+
+	get_prop_set(prop_set, &props, &size, &vendor_props, &vendor_size);
+
+	if (index < size)
+		return &(props[index]);
+	index -= size;
+
+	if (index < vendor_size)
+		return &(vendor_props[index]);
+
+	return NULL;
+}
+
+/*
+ * prop_set is part of TEE_PROPSET_xxx
+ * index is the index in the Property Set to retrieve
+ * if name is not NULL, the name of "index" property is returned
+ * if buf is not NULL, the property is returned
+ */
+TEE_Result syscall_get_property(unsigned long prop_set,
+				unsigned long index,
+				void *name, uint32_t *name_len,
+				void *buf, uint32_t *blen,
+				uint32_t *prop_type)
+{
+	struct tee_ta_session *sess;
+	TEE_Result res;
+	TEE_Result res2;
+	const struct tee_props *prop;
+	uint32_t klen;
+	size_t klen_size;
+	uint32_t elen;
+
+	prop = get_prop_struct(prop_set, index);
+	if (!prop)
+		return TEE_ERROR_ITEM_NOT_FOUND;
+
+	res = tee_ta_get_current_session(&sess);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	/* Get the property type */
+	if (prop_type) {
+		res = tee_svc_copy_to_user(prop_type, &prop->prop_type,
+					   sizeof(*prop_type));
+		if (res != TEE_SUCCESS)
+			return res;
+	}
+
+	/* Get the property */
+	if (buf && blen) {
+		res = tee_svc_copy_from_user(&klen, blen, sizeof(klen));
+		if (res != TEE_SUCCESS)
+			return res;
+
+		if (prop->get_prop_func) {
+			klen_size = klen;
+			res = prop->get_prop_func(sess, buf, &klen_size);
+			klen = klen_size;
+			res2 = tee_svc_copy_to_user(blen, &klen, sizeof(*blen));
+		} else {
+			if (klen < prop->len)
+				res = TEE_ERROR_SHORT_BUFFER;
+			else
+				res = tee_svc_copy_to_user(buf, prop->data,
+							   prop->len);
+			res2 = tee_svc_copy_to_user(blen, &prop->len,
+						    sizeof(*blen));
+		}
+		if (res2 != TEE_SUCCESS)
+			return res2;
+		if (res != TEE_SUCCESS)
+			return res;
+	}
+
+	/* Get the property name */
+	if (name && name_len) {
+		res = tee_svc_copy_from_user(&klen, name_len, sizeof(klen));
+		if (res != TEE_SUCCESS)
+			return res;
+
+		elen = strlen(prop->name) + 1;
+
+		if (klen < elen)
+			res = TEE_ERROR_SHORT_BUFFER;
+		else
+			res = tee_svc_copy_to_user(name, prop->name, elen);
+		res2 = tee_svc_copy_to_user(name_len, &elen, sizeof(*name_len));
+		if (res2 != TEE_SUCCESS)
+			return res2;
+		if (res != TEE_SUCCESS)
+			return res;
+	}
+
+	return res;
+}
+
+/*
+ * prop_set is part of TEE_PROPSET_xxx
+ */
+TEE_Result syscall_get_property_name_to_index(unsigned long prop_set,
+					      void *name,
+					      unsigned long name_len,
+					      uint32_t *index)
+{
+	TEE_Result res;
+	struct tee_ta_session *sess;
+	const struct tee_props *props;
+	size_t size;
+	const struct tee_props *vendor_props;
+	size_t vendor_size;
+	char *kname = 0;
+	uint32_t i;
+
+	get_prop_set(prop_set, &props, &size, &vendor_props, &vendor_size);
+	if (!props)
+		return TEE_ERROR_ITEM_NOT_FOUND;
+
+	res = tee_ta_get_current_session(&sess);
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	if (!name || !name_len) {
+		res = TEE_ERROR_BAD_PARAMETERS;
+		goto out;
+	}
+
+	kname = malloc(name_len);
+	if (!kname)
+		return TEE_ERROR_OUT_OF_MEMORY;
+	res = tee_svc_copy_from_user(kname, name, name_len);
+	if (res != TEE_SUCCESS)
+		goto out;
+	kname[name_len - 1] = 0;
+
+	res = TEE_ERROR_ITEM_NOT_FOUND;
+	for (i = 0; i < size; i++) {
+		if (!strcmp(kname, props[i].name)) {
+			res = tee_svc_copy_to_user(index, &i, sizeof(*index));
+			goto out;
+		}
+	}
+	for (i = size; i < size + vendor_size; i++) {
+		if (!strcmp(kname, vendor_props[i - size].name)) {
+			res = tee_svc_copy_to_user(index, &i, sizeof(*index));
+			goto out;
+		}
+	}
+
+out:
+	free(kname);
+	return res;
+}
+
+static void utee_param_to_param(struct tee_ta_param *p, struct utee_params *up)
+{
+	size_t n;
+	uint32_t types = up->types;
+
+	p->types = types;
+	for (n = 0; n < TEE_NUM_PARAMS; n++) {
+		uintptr_t a = up->vals[n * 2];
+		size_t b = up->vals[n * 2 + 1];
+
+		switch (TEE_PARAM_TYPE_GET(types, n)) {
+		case TEE_PARAM_TYPE_MEMREF_INPUT:
+		case TEE_PARAM_TYPE_MEMREF_OUTPUT:
+		case TEE_PARAM_TYPE_MEMREF_INOUT:
+			p->u[n].mem.mobj = &mobj_virt;
+			p->u[n].mem.offs = a;
+			p->u[n].mem.size = b;
+			break;
+		case TEE_PARAM_TYPE_VALUE_INPUT:
+		case TEE_PARAM_TYPE_VALUE_INOUT:
+			p->u[n].val.a = a;
+			p->u[n].val.b = b;
+			break;
+		default:
+			memset(&p->u[n], 0, sizeof(p->u[n]));
+			break;
+		}
+	}
+}
+
+static TEE_Result alloc_temp_sec_mem(size_t size, struct mobj **mobj,
+				     uint8_t **va)
+{
+	/* Allocate section in secure DDR */
+	mutex_lock(&tee_ta_mutex);
+#ifdef CFG_PAGED_USER_TA
+	*mobj = mobj_seccpy_shm_alloc(size);
+#else
+	*mobj = mobj_mm_alloc(mobj_sec_ddr, size, &tee_mm_sec_ddr);
+#endif
+	mutex_unlock(&tee_ta_mutex);
+	if (!*mobj)
+		return TEE_ERROR_GENERIC;
+
+	*va = mobj_get_va(*mobj, 0);
+	return TEE_SUCCESS;
+}
+
+/*
+ * TA invokes some TA with parameter.
+ * If some parameters are memory references:
+ * - either the memref is inside TA private RAM: TA is not allowed to expose
+ *   its private RAM: use a temporary memory buffer and copy the data.
+ * - or the memref is not in the TA private RAM:
+ *   - if the memref was mapped to the TA, TA is allowed to expose it.
+ *   - if so, converts memref virtual address into a physical address.
+ */
+static TEE_Result tee_svc_copy_param(struct tee_ta_session *sess,
+				     struct tee_ta_session *called_sess,
+				     struct utee_params *callee_params,
+				     struct tee_ta_param *param,
+				     void *tmp_buf_va[TEE_NUM_PARAMS],
+				     struct mobj **mobj_tmp)
+{
+	size_t n;
+	TEE_Result res;
+	size_t req_mem = 0;
+	size_t s;
+	uint8_t *dst = 0;
+	bool ta_private_memref[TEE_NUM_PARAMS];
+	struct user_ta_ctx *utc = to_user_ta_ctx(sess->ctx);
+	void *va;
+	size_t dst_offs;
+
+	/* fill 'param' input struct with caller params description buffer */
+	if (!callee_params) {
+		memset(param, 0, sizeof(*param));
+	} else {
+		res = tee_mmu_check_access_rights(utc,
+			TEE_MEMORY_ACCESS_READ | TEE_MEMORY_ACCESS_ANY_OWNER,
+			(uaddr_t)callee_params, sizeof(struct utee_params));
+		if (res != TEE_SUCCESS)
+			return res;
+		utee_param_to_param(param, callee_params);
+	}
+
+	if (called_sess && is_pseudo_ta_ctx(called_sess->ctx)) {
+		/*
+		 * static TA, borrow the mapping of the calling
+		 * during this call.
+		 */
+		return TEE_SUCCESS;
+	}
+
+	/* All mobj in param are of type MOJB_TYPE_VIRT */
+
+	for (n = 0; n < TEE_NUM_PARAMS; n++) {
+
+		ta_private_memref[n] = false;
+
+		switch (TEE_PARAM_TYPE_GET(param->types, n)) {
+		case TEE_PARAM_TYPE_MEMREF_INPUT:
+		case TEE_PARAM_TYPE_MEMREF_OUTPUT:
+		case TEE_PARAM_TYPE_MEMREF_INOUT:
+			va = (void *)param->u[n].mem.offs;
+			s = param->u[n].mem.size;
+			if (!va) {
+				if (!s)
+					return TEE_ERROR_BAD_PARAMETERS;
+				break;
+			}
+			/* uTA cannot expose its private memory */
+			if (tee_mmu_is_vbuf_inside_ta_private(utc, va, s)) {
+
+				s = ROUNDUP(s, sizeof(uint32_t));
+				/* Check overflow */
+				if (req_mem + s < req_mem)
+					return TEE_ERROR_BAD_PARAMETERS;
+				req_mem += s;
+				ta_private_memref[n] = true;
+				break;
+			}
+
+			res = tee_mmu_vbuf_to_mobj_offs(utc, va, s,
+							&param->u[n].mem.mobj,
+							&param->u[n].mem.offs);
+			if (res != TEE_SUCCESS)
+				return res;
+			break;
+		default:
+			break;
+		}
+	}
+
+	if (req_mem == 0)
+		return TEE_SUCCESS;
+
+	res = alloc_temp_sec_mem(req_mem, mobj_tmp, &dst);
+	if (res != TEE_SUCCESS)
+		return res;
+	dst_offs = 0;
+
+	for (n = 0; n < TEE_NUM_PARAMS; n++) {
+
+		if (!ta_private_memref[n])
+			continue;
+
+		s = ROUNDUP(param->u[n].mem.size, sizeof(uint32_t));
+
+		switch (TEE_PARAM_TYPE_GET(param->types, n)) {
+		case TEE_PARAM_TYPE_MEMREF_INPUT:
+		case TEE_PARAM_TYPE_MEMREF_INOUT:
+			va = (void *)param->u[n].mem.offs;
+			if (va) {
+				res = tee_svc_copy_from_user(dst, va,
+						param->u[n].mem.size);
+				if (res != TEE_SUCCESS)
+					return res;
+				param->u[n].mem.offs = dst_offs;
+				param->u[n].mem.mobj = *mobj_tmp;
+				tmp_buf_va[n] = dst;
+				dst += s;
+				dst_offs += s;
+			}
+			break;
+
+		case TEE_PARAM_TYPE_MEMREF_OUTPUT:
+			va = (void *)param->u[n].mem.offs;
+			if (va) {
+				param->u[n].mem.offs = dst_offs;
+				param->u[n].mem.mobj = *mobj_tmp;
+				tmp_buf_va[n] = dst;
+				dst += s;
+				dst_offs += s;
+			}
+			break;
+
+		default:
+			continue;
+		}
+	}
+
+	return TEE_SUCCESS;
+}
+
+/*
+ * Back from execution of service: update parameters passed from TA:
+ * If some parameters were memory references:
+ * - either the memref was temporary: copy back data and update size
+ * - or it was the original TA memref: update only the size value.
+ */
+static TEE_Result tee_svc_update_out_param(
+		struct tee_ta_session *sess,
+		struct tee_ta_session *called_sess,
+		struct tee_ta_param *param,
+		void *tmp_buf_va[TEE_NUM_PARAMS],
+		struct utee_params *usr_param)
+{
+	size_t n;
+	void *p;
+	struct user_ta_ctx *utc = to_user_ta_ctx(sess->ctx);
+	bool have_private_mem_map = is_user_ta_ctx(called_sess->ctx);
+
+	for (n = 0; n < TEE_NUM_PARAMS; n++) {
+		switch (TEE_PARAM_TYPE_GET(param->types, n)) {
+		case TEE_PARAM_TYPE_MEMREF_OUTPUT:
+		case TEE_PARAM_TYPE_MEMREF_INOUT:
+			p = (void *)(uintptr_t)usr_param->vals[n * 2];
+
+			/* outside TA private => memref is valid, update size */
+			if (!tee_mmu_is_vbuf_inside_ta_private(utc, p,
+					param->u[n].mem.size)) {
+				usr_param->vals[n * 2 + 1] =
+					param->u[n].mem.size;
+				break;
+			}
+
+			/*
+			 * If we called a kernel TA the parameters are in shared
+			 * memory and no copy is needed.
+			 */
+			if (have_private_mem_map &&
+			    param->u[n].mem.size <=
+			    usr_param->vals[n * 2 + 1]) {
+				uint8_t *src = tmp_buf_va[n];
+				TEE_Result res;
+
+				res = tee_svc_copy_to_user(p, src,
+						 param->u[n].mem.size);
+				if (res != TEE_SUCCESS)
+					return res;
+
+			}
+			usr_param->vals[n * 2 + 1] = param->u[n].mem.size;
+			break;
+
+		case TEE_PARAM_TYPE_VALUE_OUTPUT:
+		case TEE_PARAM_TYPE_VALUE_INOUT:
+			usr_param->vals[n * 2] = param->u[n].val.a;
+			usr_param->vals[n * 2 + 1] = param->u[n].val.b;
+			break;
+
+		default:
+			continue;
+		}
+	}
+
+	return TEE_SUCCESS;
+}
+
+/* Called when a TA calls an OpenSession on another TA */
+TEE_Result syscall_open_ta_session(const TEE_UUID *dest,
+			unsigned long cancel_req_to,
+			struct utee_params *usr_param, uint32_t *ta_sess,
+			uint32_t *ret_orig)
+{
+	TEE_Result res;
+	uint32_t ret_o = TEE_ORIGIN_TEE;
+	struct tee_ta_session *s = NULL;
+	struct tee_ta_session *sess;
+	struct mobj *mobj_param = NULL;
+	TEE_UUID *uuid = malloc(sizeof(TEE_UUID));
+	struct tee_ta_param *param = malloc(sizeof(struct tee_ta_param));
+	TEE_Identity *clnt_id = malloc(sizeof(TEE_Identity));
+	void *tmp_buf_va[TEE_NUM_PARAMS];
+	struct user_ta_ctx *utc;
+
+	if (uuid == NULL || param == NULL || clnt_id == NULL) {
+		res = TEE_ERROR_OUT_OF_MEMORY;
+		goto out_free_only;
+	}
+
+	memset(param, 0, sizeof(struct tee_ta_param));
+
+	res = tee_ta_get_current_session(&sess);
+	if (res != TEE_SUCCESS)
+		goto out_free_only;
+	utc = to_user_ta_ctx(sess->ctx);
+
+	res = tee_svc_copy_from_user(uuid, dest, sizeof(TEE_UUID));
+	if (res != TEE_SUCCESS)
+		goto function_exit;
+
+	clnt_id->login = TEE_LOGIN_TRUSTED_APP;
+	memcpy(&clnt_id->uuid, &sess->ctx->uuid, sizeof(TEE_UUID));
+
+	res = tee_svc_copy_param(sess, NULL, usr_param, param, tmp_buf_va,
+				 &mobj_param);
+	if (res != TEE_SUCCESS)
+		goto function_exit;
+
+	/*
+	 * Find session of a multi session TA or a static TA
+	 * In such a case, there is no need to ask the supplicant for the TA
+	 * code
+	 */
+	res = tee_ta_open_session(&ret_o, &s, &utc->open_sessions, uuid,
+				  clnt_id, cancel_req_to, param);
+	if (res != TEE_SUCCESS)
+		goto function_exit;
+
+	res = tee_svc_update_out_param(sess, s, param, tmp_buf_va, usr_param);
+
+function_exit:
+	if (mobj_param) {
+		mutex_lock(&tee_ta_mutex);
+		mobj_free(mobj_param);
+		mutex_unlock(&tee_ta_mutex);
+	}
+	if (res == TEE_SUCCESS)
+		tee_svc_copy_kaddr_to_uref(ta_sess, s);
+	tee_svc_copy_to_user(ret_orig, &ret_o, sizeof(ret_o));
+
+out_free_only:
+	free(param);
+	free(uuid);
+	free(clnt_id);
+	return res;
+}
+
+TEE_Result syscall_close_ta_session(unsigned long ta_sess)
+{
+	TEE_Result res;
+	struct tee_ta_session *sess;
+	TEE_Identity clnt_id;
+	struct tee_ta_session *s = tee_svc_uref_to_kaddr(ta_sess);
+	struct user_ta_ctx *utc;
+
+	res = tee_ta_get_current_session(&sess);
+	if (res != TEE_SUCCESS)
+		return res;
+	utc = to_user_ta_ctx(sess->ctx);
+
+	clnt_id.login = TEE_LOGIN_TRUSTED_APP;
+	memcpy(&clnt_id.uuid, &sess->ctx->uuid, sizeof(TEE_UUID));
+
+	return tee_ta_close_session(s, &utc->open_sessions, &clnt_id);
+}
+
+TEE_Result syscall_invoke_ta_command(unsigned long ta_sess,
+			unsigned long cancel_req_to, unsigned long cmd_id,
+			struct utee_params *usr_param, uint32_t *ret_orig)
+{
+	TEE_Result res;
+	TEE_Result res2;
+	uint32_t ret_o = TEE_ORIGIN_TEE;
+	struct tee_ta_param param = { 0 };
+	TEE_Identity clnt_id;
+	struct tee_ta_session *sess;
+	struct tee_ta_session *called_sess;
+	struct mobj *mobj_param = NULL;
+	void *tmp_buf_va[TEE_NUM_PARAMS];
+	struct user_ta_ctx *utc;
+
+	res = tee_ta_get_current_session(&sess);
+	if (res != TEE_SUCCESS)
+		return res;
+	utc = to_user_ta_ctx(sess->ctx);
+
+	called_sess = tee_ta_get_session(
+				(vaddr_t)tee_svc_uref_to_kaddr(ta_sess), true,
+				&utc->open_sessions);
+	if (!called_sess)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	clnt_id.login = TEE_LOGIN_TRUSTED_APP;
+	memcpy(&clnt_id.uuid, &sess->ctx->uuid, sizeof(TEE_UUID));
+
+	res = tee_svc_copy_param(sess, called_sess, usr_param, &param,
+				 tmp_buf_va, &mobj_param);
+	if (res != TEE_SUCCESS)
+		goto function_exit;
+
+	res = tee_ta_invoke_command(&ret_o, called_sess, &clnt_id,
+				    cancel_req_to, cmd_id, &param);
+
+	res2 = tee_svc_update_out_param(sess, called_sess, &param, tmp_buf_va,
+					usr_param);
+	if (res2 != TEE_SUCCESS) {
+		/*
+		 * Spec for TEE_InvokeTACommand() says:
+		 * "If the return origin is different from
+		 * TEE_ORIGIN_TRUSTED_APP, then the function has failed
+		 * before it could reach the destination Trusted
+		 * Application."
+		 *
+		 * But if we can't update params to the caller we have no
+		 * choice we need to return some error to indicate that
+		 * parameters aren't updated as expected.
+		 */
+		ret_o = TEE_ORIGIN_TEE;
+		res = res2;
+	}
+
+function_exit:
+	tee_ta_put_session(called_sess);
+	if (mobj_param) {
+		mutex_lock(&tee_ta_mutex);
+		mobj_free(mobj_param);
+		mutex_unlock(&tee_ta_mutex);
+	}
+	if (ret_orig)
+		tee_svc_copy_to_user(ret_orig, &ret_o, sizeof(ret_o));
+	return res;
+}
+
+TEE_Result syscall_check_access_rights(unsigned long flags, const void *buf,
+				       size_t len)
+{
+	TEE_Result res;
+	struct tee_ta_session *s;
+
+	res = tee_ta_get_current_session(&s);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	return tee_mmu_check_access_rights(to_user_ta_ctx(s->ctx), flags,
+					   (uaddr_t)buf, len);
+}
+
+TEE_Result tee_svc_copy_from_user(void *kaddr, const void *uaddr, size_t len)
+{
+	TEE_Result res;
+	struct tee_ta_session *s;
+
+	res = tee_ta_get_current_session(&s);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = tee_mmu_check_access_rights(to_user_ta_ctx(s->ctx),
+					TEE_MEMORY_ACCESS_READ |
+					TEE_MEMORY_ACCESS_ANY_OWNER,
+					(uaddr_t)uaddr, len);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	memcpy(kaddr, uaddr, len);
+	return TEE_SUCCESS;
+}
+
+TEE_Result tee_svc_copy_to_user(void *uaddr, const void *kaddr, size_t len)
+{
+	TEE_Result res;
+	struct tee_ta_session *s;
+
+	res = tee_ta_get_current_session(&s);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = tee_mmu_check_access_rights(to_user_ta_ctx(s->ctx),
+					TEE_MEMORY_ACCESS_WRITE |
+					TEE_MEMORY_ACCESS_ANY_OWNER,
+					(uaddr_t)uaddr, len);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	memcpy(uaddr, kaddr, len);
+	return TEE_SUCCESS;
+}
+
+TEE_Result tee_svc_copy_kaddr_to_uref(uint32_t *uref, void *kaddr)
+{
+	uint32_t ref = tee_svc_kaddr_to_uref(kaddr);
+
+	return tee_svc_copy_to_user(uref, &ref, sizeof(ref));
+}
+
+TEE_Result syscall_get_cancellation_flag(uint32_t *cancel)
+{
+	TEE_Result res;
+	struct tee_ta_session *s = NULL;
+	uint32_t c;
+
+	res = tee_ta_get_current_session(&s);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	c = tee_ta_session_is_cancelled(s, NULL);
+
+	return tee_svc_copy_to_user(cancel, &c, sizeof(c));
+}
+
+TEE_Result syscall_unmask_cancellation(uint32_t *old_mask)
+{
+	TEE_Result res;
+	struct tee_ta_session *s = NULL;
+	uint32_t m;
+
+	res = tee_ta_get_current_session(&s);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	m = s->cancel_mask;
+	s->cancel_mask = false;
+	return tee_svc_copy_to_user(old_mask, &m, sizeof(m));
+}
+
+TEE_Result syscall_mask_cancellation(uint32_t *old_mask)
+{
+	TEE_Result res;
+	struct tee_ta_session *s = NULL;
+	uint32_t m;
+
+	res = tee_ta_get_current_session(&s);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	m = s->cancel_mask;
+	s->cancel_mask = true;
+	return tee_svc_copy_to_user(old_mask, &m, sizeof(m));
+}
+
+TEE_Result syscall_wait(unsigned long timeout)
+{
+	TEE_Result res = TEE_SUCCESS;
+	uint32_t mytime = 0;
+	struct tee_ta_session *s;
+	TEE_Time base_time;
+	TEE_Time current_time;
+
+	res = tee_ta_get_current_session(&s);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = tee_time_get_sys_time(&base_time);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	while (true) {
+		res = tee_time_get_sys_time(&current_time);
+		if (res != TEE_SUCCESS)
+			return res;
+
+		if (tee_ta_session_is_cancelled(s, &current_time))
+			return TEE_ERROR_CANCEL;
+
+		mytime = (current_time.seconds - base_time.seconds) * 1000 +
+		    (int)current_time.millis - (int)base_time.millis;
+		if (mytime >= timeout)
+			return TEE_SUCCESS;
+
+		tee_time_wait(timeout - mytime);
+	}
+
+	return res;
+}
+
+TEE_Result syscall_get_time(unsigned long cat, TEE_Time *mytime)
+{
+	TEE_Result res, res2;
+	struct tee_ta_session *s = NULL;
+	TEE_Time t;
+
+	res = tee_ta_get_current_session(&s);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	switch (cat) {
+	case UTEE_TIME_CAT_SYSTEM:
+		res = tee_time_get_sys_time(&t);
+		break;
+	case UTEE_TIME_CAT_TA_PERSISTENT:
+		res = tee_time_get_ta_time((const void *)&s->ctx->uuid, &t);
+		break;
+	case UTEE_TIME_CAT_REE:
+		res = tee_time_get_ree_time(&t);
+		break;
+	default:
+		res = TEE_ERROR_BAD_PARAMETERS;
+		break;
+	}
+
+	if (res == TEE_SUCCESS || res == TEE_ERROR_OVERFLOW) {
+		res2 = tee_svc_copy_to_user(mytime, &t, sizeof(t));
+		if (res2 != TEE_SUCCESS)
+			res = res2;
+	}
+
+	return res;
+}
+
+TEE_Result syscall_set_ta_time(const TEE_Time *mytime)
+{
+	TEE_Result res;
+	struct tee_ta_session *s = NULL;
+	TEE_Time t;
+
+	res = tee_ta_get_current_session(&s);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = tee_svc_copy_from_user(&t, mytime, sizeof(t));
+	if (res != TEE_SUCCESS)
+		return res;
+
+	return tee_time_set_ta_time((const void *)&s->ctx->uuid, &t);
+}
diff --git a/core/tee/tee_svc_cryp.c b/core/tee/tee_svc_cryp.c
new file mode 100644
index 0000000..c668a3e
--- /dev/null
+++ b/core/tee/tee_svc_cryp.c
@@ -0,0 +1,3554 @@
+/*
+ * Copyright (c) 2014, STMicroelectronics International N.V.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <assert.h>
+#include <tee_api_types.h>
+#include <kernel/tee_ta_manager.h>
+#include <utee_defines.h>
+#include <mm/tee_mmu.h>
+#include <tee/tee_svc.h>
+#include <tee/tee_svc_cryp.h>
+#include <tee/tee_cryp_utl.h>
+#include <sys/queue.h>
+#include <tee/tee_obj.h>
+#include <tee/tee_cryp_provider.h>
+#include <trace.h>
+#include <string_ext.h>
+#include <string.h>
+#include <util.h>
+#if defined(CFG_CRYPTO_HKDF) || defined(CFG_CRYPTO_CONCAT_KDF) || \
+	defined(CFG_CRYPTO_PBKDF2)
+#include <tee_api_defines_extensions.h>
+#endif
+#if defined(CFG_CRYPTO_HKDF)
+#include <tee/tee_cryp_hkdf.h>
+#endif
+#if defined(CFG_CRYPTO_CONCAT_KDF)
+#include <tee/tee_cryp_concat_kdf.h>
+#endif
+#if defined(CFG_CRYPTO_PBKDF2)
+#include <tee/tee_cryp_pbkdf2.h>
+#endif
+
+typedef void (*tee_cryp_ctx_finalize_func_t) (void *ctx, uint32_t algo);
+struct tee_cryp_state {
+	TAILQ_ENTRY(tee_cryp_state) link;
+	uint32_t algo;
+	uint32_t mode;
+	vaddr_t key1;
+	vaddr_t key2;
+	size_t ctx_size;
+	void *ctx;
+	tee_cryp_ctx_finalize_func_t ctx_finalize;
+};
+
+struct tee_cryp_obj_secret {
+	uint32_t key_size;
+	uint32_t alloc_size;
+
+	/*
+	 * Pseudo code visualize layout of structure
+	 * Next follows data, such as:
+	 *	uint8_t data[alloc_size]
+	 * key_size must never exceed alloc_size
+	 */
+};
+
+#define TEE_TYPE_ATTR_OPTIONAL       0x0
+#define TEE_TYPE_ATTR_REQUIRED       0x1
+#define TEE_TYPE_ATTR_OPTIONAL_GROUP 0x2
+#define TEE_TYPE_ATTR_SIZE_INDICATOR 0x4
+#define TEE_TYPE_ATTR_GEN_KEY_OPT    0x8
+#define TEE_TYPE_ATTR_GEN_KEY_REQ    0x10
+
+    /* Handle storing of generic secret keys of varying lengths */
+#define ATTR_OPS_INDEX_SECRET     0
+    /* Convert to/from big-endian byte array and provider-specific bignum */
+#define ATTR_OPS_INDEX_BIGNUM     1
+    /* Convert to/from value attribute depending on direction */
+#define ATTR_OPS_INDEX_VALUE      2
+
+struct tee_cryp_obj_type_attrs {
+	uint32_t attr_id;
+	uint16_t flags;
+	uint16_t ops_index;
+	uint16_t raw_offs;
+	uint16_t raw_size;
+};
+
+#define RAW_DATA(_x, _y)	\
+	.raw_offs = offsetof(_x, _y), .raw_size = MEMBER_SIZE(_x, _y)
+
+static const struct tee_cryp_obj_type_attrs
+	tee_cryp_obj_secret_value_attrs[] = {
+	{
+	.attr_id = TEE_ATTR_SECRET_VALUE,
+	.flags = TEE_TYPE_ATTR_REQUIRED | TEE_TYPE_ATTR_SIZE_INDICATOR,
+	.ops_index = ATTR_OPS_INDEX_SECRET,
+	.raw_offs = 0,
+	.raw_size = 0
+	},
+};
+
+static const struct tee_cryp_obj_type_attrs tee_cryp_obj_rsa_pub_key_attrs[] = {
+	{
+	.attr_id = TEE_ATTR_RSA_MODULUS,
+	.flags = TEE_TYPE_ATTR_REQUIRED | TEE_TYPE_ATTR_SIZE_INDICATOR,
+	.ops_index = ATTR_OPS_INDEX_BIGNUM,
+	RAW_DATA(struct rsa_public_key, n)
+	},
+
+	{
+	.attr_id = TEE_ATTR_RSA_PUBLIC_EXPONENT,
+	.flags = TEE_TYPE_ATTR_REQUIRED,
+	.ops_index = ATTR_OPS_INDEX_BIGNUM,
+	RAW_DATA(struct rsa_public_key, e)
+	},
+};
+
+static const struct tee_cryp_obj_type_attrs tee_cryp_obj_rsa_keypair_attrs[] = {
+	{
+	.attr_id = TEE_ATTR_RSA_MODULUS,
+	.flags = TEE_TYPE_ATTR_REQUIRED | TEE_TYPE_ATTR_SIZE_INDICATOR,
+	.ops_index = ATTR_OPS_INDEX_BIGNUM,
+	RAW_DATA(struct rsa_keypair, n)
+	},
+
+	{
+	.attr_id = TEE_ATTR_RSA_PUBLIC_EXPONENT,
+	.flags = TEE_TYPE_ATTR_REQUIRED,
+	.ops_index = ATTR_OPS_INDEX_BIGNUM,
+	RAW_DATA(struct rsa_keypair, e)
+	},
+
+	{
+	.attr_id = TEE_ATTR_RSA_PRIVATE_EXPONENT,
+	.flags = TEE_TYPE_ATTR_REQUIRED,
+	.ops_index = ATTR_OPS_INDEX_BIGNUM,
+	RAW_DATA(struct rsa_keypair, d)
+	},
+
+	{
+	.attr_id = TEE_ATTR_RSA_PRIME1,
+	.flags = TEE_TYPE_ATTR_OPTIONAL_GROUP,
+	.ops_index = ATTR_OPS_INDEX_BIGNUM,
+	RAW_DATA(struct rsa_keypair, p)
+	},
+
+	{
+	.attr_id = TEE_ATTR_RSA_PRIME2,
+	.flags = TEE_TYPE_ATTR_OPTIONAL_GROUP,
+	.ops_index = ATTR_OPS_INDEX_BIGNUM,
+	RAW_DATA(struct rsa_keypair, q)
+	},
+
+	{
+	.attr_id = TEE_ATTR_RSA_EXPONENT1,
+	.flags = TEE_TYPE_ATTR_OPTIONAL_GROUP,
+	.ops_index = ATTR_OPS_INDEX_BIGNUM,
+	RAW_DATA(struct rsa_keypair, dp)
+	},
+
+	{
+	.attr_id = TEE_ATTR_RSA_EXPONENT2,
+	.flags = TEE_TYPE_ATTR_OPTIONAL_GROUP,
+	.ops_index = ATTR_OPS_INDEX_BIGNUM,
+	RAW_DATA(struct rsa_keypair, dq)
+	},
+
+	{
+	.attr_id = TEE_ATTR_RSA_COEFFICIENT,
+	.flags = TEE_TYPE_ATTR_OPTIONAL_GROUP,
+	.ops_index = ATTR_OPS_INDEX_BIGNUM,
+	RAW_DATA(struct rsa_keypair, qp)
+	},
+};
+
+static const struct tee_cryp_obj_type_attrs tee_cryp_obj_dsa_pub_key_attrs[] = {
+	{
+	.attr_id = TEE_ATTR_DSA_PRIME,
+	.flags = TEE_TYPE_ATTR_REQUIRED,
+	.ops_index = ATTR_OPS_INDEX_BIGNUM,
+	RAW_DATA(struct dsa_public_key, p)
+	},
+
+	{
+	.attr_id = TEE_ATTR_DSA_SUBPRIME,
+	.flags = TEE_TYPE_ATTR_REQUIRED | TEE_TYPE_ATTR_SIZE_INDICATOR,
+	.ops_index = ATTR_OPS_INDEX_BIGNUM,
+	RAW_DATA(struct dsa_public_key, q)
+	},
+
+	{
+	.attr_id = TEE_ATTR_DSA_BASE,
+	.flags = TEE_TYPE_ATTR_REQUIRED,
+	.ops_index = ATTR_OPS_INDEX_BIGNUM,
+	RAW_DATA(struct dsa_public_key, g)
+	},
+
+	{
+	.attr_id = TEE_ATTR_DSA_PUBLIC_VALUE,
+	.flags = TEE_TYPE_ATTR_REQUIRED,
+	.ops_index = ATTR_OPS_INDEX_BIGNUM,
+	RAW_DATA(struct dsa_public_key, y)
+	},
+};
+
+static const struct tee_cryp_obj_type_attrs tee_cryp_obj_dsa_keypair_attrs[] = {
+	{
+	.attr_id = TEE_ATTR_DSA_PRIME,
+	.flags = TEE_TYPE_ATTR_REQUIRED | TEE_TYPE_ATTR_GEN_KEY_REQ,
+	.ops_index = ATTR_OPS_INDEX_BIGNUM,
+	RAW_DATA(struct dsa_keypair, p)
+	},
+
+	{
+	.attr_id = TEE_ATTR_DSA_SUBPRIME,
+	.flags = TEE_TYPE_ATTR_REQUIRED | TEE_TYPE_ATTR_SIZE_INDICATOR |
+		 TEE_TYPE_ATTR_GEN_KEY_REQ,
+	.ops_index = ATTR_OPS_INDEX_BIGNUM,
+	RAW_DATA(struct dsa_keypair, q)
+	},
+
+	{
+	.attr_id = TEE_ATTR_DSA_BASE,
+	.flags = TEE_TYPE_ATTR_REQUIRED | TEE_TYPE_ATTR_GEN_KEY_REQ,
+	.ops_index = ATTR_OPS_INDEX_BIGNUM,
+	RAW_DATA(struct dsa_keypair, g)
+	},
+
+	{
+	.attr_id = TEE_ATTR_DSA_PRIVATE_VALUE,
+	.flags = TEE_TYPE_ATTR_REQUIRED,
+	.ops_index = ATTR_OPS_INDEX_BIGNUM,
+	RAW_DATA(struct dsa_keypair, x)
+	},
+
+	{
+	.attr_id = TEE_ATTR_DSA_PUBLIC_VALUE,
+	.flags = TEE_TYPE_ATTR_REQUIRED,
+	.ops_index = ATTR_OPS_INDEX_BIGNUM,
+	RAW_DATA(struct dsa_keypair, y)
+	},
+};
+
+static const struct tee_cryp_obj_type_attrs tee_cryp_obj_dh_keypair_attrs[] = {
+	{
+	.attr_id = TEE_ATTR_DH_PRIME,
+	.flags = TEE_TYPE_ATTR_REQUIRED | TEE_TYPE_ATTR_SIZE_INDICATOR |
+		 TEE_TYPE_ATTR_GEN_KEY_REQ,
+	.ops_index = ATTR_OPS_INDEX_BIGNUM,
+	RAW_DATA(struct dh_keypair, p)
+	},
+
+	{
+	.attr_id = TEE_ATTR_DH_BASE,
+	.flags = TEE_TYPE_ATTR_REQUIRED | TEE_TYPE_ATTR_GEN_KEY_REQ,
+	.ops_index = ATTR_OPS_INDEX_BIGNUM,
+	RAW_DATA(struct dh_keypair, g)
+	},
+
+	{
+	.attr_id = TEE_ATTR_DH_PUBLIC_VALUE,
+	.flags = TEE_TYPE_ATTR_REQUIRED,
+	.ops_index = ATTR_OPS_INDEX_BIGNUM,
+	RAW_DATA(struct dh_keypair, y)
+	},
+
+	{
+	.attr_id = TEE_ATTR_DH_PRIVATE_VALUE,
+	.flags = TEE_TYPE_ATTR_REQUIRED,
+	.ops_index = ATTR_OPS_INDEX_BIGNUM,
+	RAW_DATA(struct dh_keypair, x)
+	},
+
+	{
+	.attr_id = TEE_ATTR_DH_SUBPRIME,
+	.flags = TEE_TYPE_ATTR_OPTIONAL_GROUP |	 TEE_TYPE_ATTR_GEN_KEY_OPT,
+	.ops_index = ATTR_OPS_INDEX_BIGNUM,
+	RAW_DATA(struct dh_keypair, q)
+	},
+
+	{
+	.attr_id = TEE_ATTR_DH_X_BITS,
+	.flags = TEE_TYPE_ATTR_GEN_KEY_OPT,
+	.ops_index = ATTR_OPS_INDEX_VALUE,
+	RAW_DATA(struct dh_keypair, xbits)
+	},
+};
+
+#if defined(CFG_CRYPTO_HKDF)
+static const struct tee_cryp_obj_type_attrs
+	tee_cryp_obj_hkdf_ikm_attrs[] = {
+	{
+	.attr_id = TEE_ATTR_HKDF_IKM,
+	.flags = TEE_TYPE_ATTR_REQUIRED | TEE_TYPE_ATTR_SIZE_INDICATOR,
+	.ops_index = ATTR_OPS_INDEX_SECRET,
+	.raw_offs = 0,
+	.raw_size = 0
+	},
+};
+#endif
+
+#if defined(CFG_CRYPTO_CONCAT_KDF)
+static const struct tee_cryp_obj_type_attrs
+	tee_cryp_obj_concat_kdf_z_attrs[] = {
+	{
+	.attr_id = TEE_ATTR_CONCAT_KDF_Z,
+	.flags = TEE_TYPE_ATTR_REQUIRED | TEE_TYPE_ATTR_SIZE_INDICATOR,
+	.ops_index = ATTR_OPS_INDEX_SECRET,
+	.raw_offs = 0,
+	.raw_size = 0
+	},
+};
+#endif
+
+#if defined(CFG_CRYPTO_PBKDF2)
+static const struct tee_cryp_obj_type_attrs
+	tee_cryp_obj_pbkdf2_passwd_attrs[] = {
+	{
+	.attr_id = TEE_ATTR_PBKDF2_PASSWORD,
+	.flags = TEE_TYPE_ATTR_REQUIRED | TEE_TYPE_ATTR_SIZE_INDICATOR,
+	.ops_index = ATTR_OPS_INDEX_SECRET,
+	.raw_offs = 0,
+	.raw_size = 0
+	},
+};
+#endif
+
+static const struct tee_cryp_obj_type_attrs tee_cryp_obj_ecc_pub_key_attrs[] = {
+	{
+	.attr_id = TEE_ATTR_ECC_PUBLIC_VALUE_X,
+	.flags = TEE_TYPE_ATTR_REQUIRED,
+	.ops_index = ATTR_OPS_INDEX_BIGNUM,
+	RAW_DATA(struct ecc_public_key, x)
+	},
+
+	{
+	.attr_id = TEE_ATTR_ECC_PUBLIC_VALUE_Y,
+	.flags = TEE_TYPE_ATTR_REQUIRED,
+	.ops_index = ATTR_OPS_INDEX_BIGNUM,
+	RAW_DATA(struct ecc_public_key, y)
+	},
+
+	{
+	.attr_id = TEE_ATTR_ECC_CURVE,
+	.flags = TEE_TYPE_ATTR_REQUIRED,
+	.ops_index = ATTR_OPS_INDEX_VALUE,
+	RAW_DATA(struct ecc_public_key, curve)
+	},
+};
+
+static const struct tee_cryp_obj_type_attrs tee_cryp_obj_ecc_keypair_attrs[] = {
+	{
+	.attr_id = TEE_ATTR_ECC_PRIVATE_VALUE,
+	.flags = TEE_TYPE_ATTR_REQUIRED,
+	.ops_index = ATTR_OPS_INDEX_BIGNUM,
+	RAW_DATA(struct ecc_keypair, d)
+	},
+
+	{
+	.attr_id = TEE_ATTR_ECC_PUBLIC_VALUE_X,
+	.flags = TEE_TYPE_ATTR_REQUIRED,
+	.ops_index = ATTR_OPS_INDEX_BIGNUM,
+	RAW_DATA(struct ecc_keypair, x)
+	},
+
+	{
+	.attr_id = TEE_ATTR_ECC_PUBLIC_VALUE_Y,
+	.flags = TEE_TYPE_ATTR_REQUIRED,
+	.ops_index = ATTR_OPS_INDEX_BIGNUM,
+	RAW_DATA(struct ecc_keypair, y)
+	},
+
+	{
+	.attr_id = TEE_ATTR_ECC_CURVE,
+	.flags = TEE_TYPE_ATTR_REQUIRED,
+	.ops_index = ATTR_OPS_INDEX_VALUE,
+	RAW_DATA(struct ecc_keypair, curve)
+	},
+};
+
+struct tee_cryp_obj_type_props {
+	TEE_ObjectType obj_type;
+	uint16_t min_size;	/* may not be smaller than this */
+	uint16_t max_size;	/* may not be larger than this */
+	uint16_t alloc_size;	/* this many bytes are allocated to hold data */
+	uint8_t quanta;		/* may only be an multiple of this */
+
+	uint8_t num_type_attrs;
+	const struct tee_cryp_obj_type_attrs *type_attrs;
+};
+
+#define PROP(obj_type, quanta, min_size, max_size, alloc_size, type_attrs) \
+		{ (obj_type), (min_size), (max_size), (alloc_size), (quanta), \
+		  ARRAY_SIZE(type_attrs), (type_attrs) }
+
+static const struct tee_cryp_obj_type_props tee_cryp_obj_props[] = {
+	PROP(TEE_TYPE_AES, 64, 128, 256,	/* valid sizes 128, 192, 256 */
+		256 / 8 + sizeof(struct tee_cryp_obj_secret),
+		tee_cryp_obj_secret_value_attrs),
+	PROP(TEE_TYPE_DES, 56, 56, 56,
+		/*
+		* Valid size 56 without parity, note that we still allocate
+		* for 64 bits since the key is supplied with parity.
+		*/
+		64 / 8 + sizeof(struct tee_cryp_obj_secret),
+		tee_cryp_obj_secret_value_attrs),
+	PROP(TEE_TYPE_DES3, 56, 112, 168,
+		/*
+		* Valid sizes 112, 168 without parity, note that we still
+		* allocate for with space for the parity since the key is
+		* supplied with parity.
+		*/
+		192 / 8 + sizeof(struct tee_cryp_obj_secret),
+		tee_cryp_obj_secret_value_attrs),
+	PROP(TEE_TYPE_HMAC_MD5, 8, 64, 512,
+		512 / 8 + sizeof(struct tee_cryp_obj_secret),
+		tee_cryp_obj_secret_value_attrs),
+	PROP(TEE_TYPE_HMAC_SHA1, 8, 80, 512,
+		512 / 8 + sizeof(struct tee_cryp_obj_secret),
+		tee_cryp_obj_secret_value_attrs),
+	PROP(TEE_TYPE_HMAC_SHA224, 8, 112, 512,
+		512 / 8 + sizeof(struct tee_cryp_obj_secret),
+		tee_cryp_obj_secret_value_attrs),
+	PROP(TEE_TYPE_HMAC_SHA256, 8, 192, 1024,
+		1024 / 8 + sizeof(struct tee_cryp_obj_secret),
+		tee_cryp_obj_secret_value_attrs),
+	PROP(TEE_TYPE_HMAC_SHA384, 8, 256, 1024,
+		1024 / 8 + sizeof(struct tee_cryp_obj_secret),
+		tee_cryp_obj_secret_value_attrs),
+	PROP(TEE_TYPE_HMAC_SHA512, 8, 256, 1024,
+		1024 / 8 + sizeof(struct tee_cryp_obj_secret),
+		tee_cryp_obj_secret_value_attrs),
+	PROP(TEE_TYPE_GENERIC_SECRET, 8, 0, 4096,
+		4096 / 8 + sizeof(struct tee_cryp_obj_secret),
+		tee_cryp_obj_secret_value_attrs),
+#if defined(CFG_CRYPTO_HKDF)
+	PROP(TEE_TYPE_HKDF_IKM, 8, 0, 4096,
+		4096 / 8 + sizeof(struct tee_cryp_obj_secret),
+		tee_cryp_obj_hkdf_ikm_attrs),
+#endif
+#if defined(CFG_CRYPTO_CONCAT_KDF)
+	PROP(TEE_TYPE_CONCAT_KDF_Z, 8, 0, 4096,
+		4096 / 8 + sizeof(struct tee_cryp_obj_secret),
+		tee_cryp_obj_concat_kdf_z_attrs),
+#endif
+#if defined(CFG_CRYPTO_PBKDF2)
+	PROP(TEE_TYPE_PBKDF2_PASSWORD, 8, 0, 4096,
+		4096 / 8 + sizeof(struct tee_cryp_obj_secret),
+		tee_cryp_obj_pbkdf2_passwd_attrs),
+#endif
+	PROP(TEE_TYPE_RSA_PUBLIC_KEY, 1, 256, 2048,
+		sizeof(struct rsa_public_key),
+		tee_cryp_obj_rsa_pub_key_attrs),
+
+	PROP(TEE_TYPE_RSA_KEYPAIR, 1, 256, 2048,
+		sizeof(struct rsa_keypair),
+		tee_cryp_obj_rsa_keypair_attrs),
+
+	PROP(TEE_TYPE_DSA_PUBLIC_KEY, 64, 512, 3072,
+		sizeof(struct dsa_public_key),
+		tee_cryp_obj_dsa_pub_key_attrs),
+
+	PROP(TEE_TYPE_DSA_KEYPAIR, 64, 512, 3072,
+		sizeof(struct dsa_keypair),
+		tee_cryp_obj_dsa_keypair_attrs),
+
+	PROP(TEE_TYPE_DH_KEYPAIR, 1, 256, 2048,
+		sizeof(struct dh_keypair),
+		tee_cryp_obj_dh_keypair_attrs),
+
+	PROP(TEE_TYPE_ECDSA_PUBLIC_KEY, 1, 192, 521,
+		sizeof(struct ecc_public_key),
+		tee_cryp_obj_ecc_pub_key_attrs),
+
+	PROP(TEE_TYPE_ECDSA_KEYPAIR, 1, 192, 521,
+		sizeof(struct ecc_keypair),
+		tee_cryp_obj_ecc_keypair_attrs),
+
+	PROP(TEE_TYPE_ECDH_PUBLIC_KEY, 1, 192, 521,
+		sizeof(struct ecc_public_key),
+		tee_cryp_obj_ecc_pub_key_attrs),
+
+	PROP(TEE_TYPE_ECDH_KEYPAIR, 1, 192, 521,
+		sizeof(struct ecc_keypair),
+		tee_cryp_obj_ecc_keypair_attrs),
+};
+
+struct attr_ops {
+	TEE_Result (*from_user)(void *attr, const void *buffer, size_t size);
+	TEE_Result (*to_user)(void *attr, struct tee_ta_session *sess,
+			      void *buffer, uint64_t *size);
+	void (*to_binary)(void *attr, void *data, size_t data_len,
+			  size_t *offs);
+	bool (*from_binary)(void *attr, const void *data, size_t data_len,
+			    size_t *offs);
+	TEE_Result (*from_obj)(void *attr, void *src_attr);
+	void (*free)(void *attr);
+	void (*clear)(void *attr);
+};
+
+static void op_u32_to_binary_helper(uint32_t v, uint8_t *data,
+				    size_t data_len, size_t *offs)
+{
+	uint32_t field;
+
+	if (data && (*offs + sizeof(field)) <= data_len) {
+		field = TEE_U32_TO_BIG_ENDIAN(v);
+		memcpy(data + *offs, &field, sizeof(field));
+	}
+	(*offs) += sizeof(field);
+}
+
+static bool op_u32_from_binary_helper(uint32_t *v, const uint8_t *data,
+				      size_t data_len, size_t *offs)
+{
+	uint32_t field;
+
+	if (!data || (*offs + sizeof(field)) > data_len)
+		return false;
+
+	memcpy(&field, data + *offs, sizeof(field));
+	*v = TEE_U32_FROM_BIG_ENDIAN(field);
+	(*offs) += sizeof(field);
+	return true;
+}
+
+static TEE_Result op_attr_secret_value_from_user(void *attr, const void *buffer,
+						 size_t size)
+{
+	struct tee_cryp_obj_secret *key = attr;
+
+	/* Data size has to fit in allocated buffer */
+	if (size > key->alloc_size)
+		return TEE_ERROR_SECURITY;
+	memcpy(key + 1, buffer, size);
+	key->key_size = size;
+	return TEE_SUCCESS;
+}
+
+static TEE_Result op_attr_secret_value_to_user(void *attr,
+			struct tee_ta_session *sess __unused,
+			void *buffer, uint64_t *size)
+{
+	TEE_Result res;
+	struct tee_cryp_obj_secret *key = attr;
+	uint64_t s;
+	uint64_t key_size;
+
+	res = tee_svc_copy_from_user(&s, size, sizeof(s));
+	if (res != TEE_SUCCESS)
+		return res;
+
+	key_size = key->key_size;
+	res = tee_svc_copy_to_user(size, &key_size, sizeof(key_size));
+	if (res != TEE_SUCCESS)
+		return res;
+
+	if (s < key->key_size)
+		return TEE_ERROR_SHORT_BUFFER;
+
+	return tee_svc_copy_to_user(buffer, key + 1, key->key_size);
+}
+
+static void op_attr_secret_value_to_binary(void *attr, void *data,
+					   size_t data_len, size_t *offs)
+{
+	struct tee_cryp_obj_secret *key = attr;
+
+	op_u32_to_binary_helper(key->key_size, data, data_len, offs);
+	if (data && (*offs + key->key_size) <= data_len)
+		memcpy((uint8_t *)data + *offs, key + 1, key->key_size);
+	(*offs) += key->key_size;
+}
+
+static bool op_attr_secret_value_from_binary(void *attr, const void *data,
+					     size_t data_len, size_t *offs)
+{
+	struct tee_cryp_obj_secret *key = attr;
+	uint32_t s;
+
+	if (!op_u32_from_binary_helper(&s, data, data_len, offs))
+		return false;
+
+	if ((*offs + s) > data_len)
+		return false;
+
+	/* Data size has to fit in allocated buffer */
+	if (s > key->alloc_size)
+		return false;
+	key->key_size = s;
+	memcpy(key + 1, (const uint8_t *)data + *offs, s);
+	(*offs) += s;
+	return true;
+}
+
+
+static TEE_Result op_attr_secret_value_from_obj(void *attr, void *src_attr)
+{
+	struct tee_cryp_obj_secret *key = attr;
+	struct tee_cryp_obj_secret *src_key = src_attr;
+
+	if (src_key->key_size > key->alloc_size)
+		return TEE_ERROR_BAD_STATE;
+	memcpy(key + 1, src_key + 1, src_key->key_size);
+	key->key_size = src_key->key_size;
+	return TEE_SUCCESS;
+}
+
+static void op_attr_secret_value_clear(void *attr)
+{
+	struct tee_cryp_obj_secret *key = attr;
+
+	key->key_size = 0;
+	memset(key + 1, 0, key->alloc_size);
+}
+
+static TEE_Result op_attr_bignum_from_user(void *attr, const void *buffer,
+					   size_t size)
+{
+	struct bignum **bn = attr;
+
+	if (!crypto_ops.bignum.bin2bn)
+		return TEE_ERROR_NOT_IMPLEMENTED;
+	return crypto_ops.bignum.bin2bn(buffer, size, *bn);
+}
+
+static TEE_Result op_attr_bignum_to_user(void *attr,
+					 struct tee_ta_session *sess,
+					 void *buffer, uint64_t *size)
+{
+	TEE_Result res;
+	struct bignum **bn = attr;
+	uint64_t req_size;
+	uint64_t s;
+
+	res = tee_svc_copy_from_user(&s, size, sizeof(s));
+	if (res != TEE_SUCCESS)
+		return res;
+
+	req_size = crypto_ops.bignum.num_bytes(*bn);
+	res = tee_svc_copy_to_user(size, &req_size, sizeof(req_size));
+	if (res != TEE_SUCCESS)
+		return res;
+	if (!req_size)
+		return TEE_SUCCESS;
+	if (s < req_size)
+		return TEE_ERROR_SHORT_BUFFER;
+
+	/* Check we can access data using supplied user mode pointer */
+	res = tee_mmu_check_access_rights(to_user_ta_ctx(sess->ctx),
+					  TEE_MEMORY_ACCESS_READ |
+					  TEE_MEMORY_ACCESS_WRITE |
+					  TEE_MEMORY_ACCESS_ANY_OWNER,
+					  (uaddr_t)buffer, req_size);
+	if (res != TEE_SUCCESS)
+		return res;
+	/*
+	* Write the bignum (wich raw data points to) into an array of
+	* bytes (stored in buffer)
+	*/
+	crypto_ops.bignum.bn2bin(*bn, buffer);
+	return TEE_SUCCESS;
+}
+
+static void op_attr_bignum_to_binary(void *attr, void *data, size_t data_len,
+				     size_t *offs)
+{
+	struct bignum **bn = attr;
+	uint32_t n = crypto_ops.bignum.num_bytes(*bn);
+
+	op_u32_to_binary_helper(n, data, data_len, offs);
+
+	if (data && (*offs + n) <= data_len)
+		crypto_ops.bignum.bn2bin(*bn, (uint8_t *)data + *offs);
+	(*offs) += n;
+}
+
+static bool op_attr_bignum_from_binary(void *attr, const void *data,
+				       size_t data_len, size_t *offs)
+{
+	struct bignum **bn = attr;
+	uint32_t n;
+
+	if (!op_u32_from_binary_helper(&n, data, data_len, offs))
+		return false;
+
+	if ((*offs + n) > data_len)
+		return false;
+	if (crypto_ops.bignum.bin2bn((const uint8_t *)data + *offs,
+				     n, *bn) != TEE_SUCCESS)
+		return false;
+	(*offs) += n;
+	return true;
+}
+
+static TEE_Result op_attr_bignum_from_obj(void *attr, void *src_attr)
+{
+	struct bignum **bn = attr;
+	struct bignum **src_bn = src_attr;
+
+	crypto_ops.bignum.copy(*bn, *src_bn);
+	return TEE_SUCCESS;
+}
+
+static void op_attr_bignum_clear(void *attr)
+{
+	struct bignum **bn = attr;
+
+	crypto_ops.bignum.clear(*bn);
+}
+
+static void op_attr_bignum_free(void *attr)
+{
+	struct bignum **bn = attr;
+
+	crypto_ops.bignum.free(*bn);
+	*bn = NULL;
+}
+
+static TEE_Result op_attr_value_from_user(void *attr, const void *buffer,
+					  size_t size)
+{
+	uint32_t *v = attr;
+
+	if (size != sizeof(uint32_t) * 2)
+		return TEE_ERROR_GENERIC; /* "can't happen */
+
+	/* Note that only the first value is copied */
+	memcpy(v, buffer, sizeof(uint32_t));
+	return TEE_SUCCESS;
+}
+
+static TEE_Result op_attr_value_to_user(void *attr,
+					struct tee_ta_session *sess __unused,
+					void *buffer, uint64_t *size)
+{
+	TEE_Result res;
+	uint32_t *v = attr;
+	uint64_t s;
+	uint32_t value[2] = { *v };
+	uint64_t req_size = sizeof(value);
+
+	res = tee_svc_copy_from_user(&s, size, sizeof(s));
+	if (res != TEE_SUCCESS)
+		return res;
+
+	if (s < req_size)
+		return TEE_ERROR_SHORT_BUFFER;
+
+	return tee_svc_copy_to_user(buffer, value, req_size);
+}
+
+static void op_attr_value_to_binary(void *attr, void *data, size_t data_len,
+				    size_t *offs)
+{
+	uint32_t *v = attr;
+
+	op_u32_to_binary_helper(*v, data, data_len, offs);
+}
+
+static bool op_attr_value_from_binary(void *attr, const void *data,
+				      size_t data_len, size_t *offs)
+{
+	uint32_t *v = attr;
+
+	return op_u32_from_binary_helper(v, data, data_len, offs);
+}
+
+static TEE_Result op_attr_value_from_obj(void *attr, void *src_attr)
+{
+	uint32_t *v = attr;
+	uint32_t *src_v = src_attr;
+
+	*v = *src_v;
+	return TEE_SUCCESS;
+}
+
+static void op_attr_value_clear(void *attr)
+{
+	uint32_t *v = attr;
+
+	*v = 0;
+}
+
+static const struct attr_ops attr_ops[] = {
+	[ATTR_OPS_INDEX_SECRET] = {
+		.from_user = op_attr_secret_value_from_user,
+		.to_user = op_attr_secret_value_to_user,
+		.to_binary = op_attr_secret_value_to_binary,
+		.from_binary = op_attr_secret_value_from_binary,
+		.from_obj = op_attr_secret_value_from_obj,
+		.free = op_attr_secret_value_clear, /* not a typo */
+		.clear = op_attr_secret_value_clear,
+	},
+	[ATTR_OPS_INDEX_BIGNUM] = {
+		.from_user = op_attr_bignum_from_user,
+		.to_user = op_attr_bignum_to_user,
+		.to_binary = op_attr_bignum_to_binary,
+		.from_binary = op_attr_bignum_from_binary,
+		.from_obj = op_attr_bignum_from_obj,
+		.free = op_attr_bignum_free,
+		.clear = op_attr_bignum_clear,
+	},
+	[ATTR_OPS_INDEX_VALUE] = {
+		.from_user = op_attr_value_from_user,
+		.to_user = op_attr_value_to_user,
+		.to_binary = op_attr_value_to_binary,
+		.from_binary = op_attr_value_from_binary,
+		.from_obj = op_attr_value_from_obj,
+		.free = op_attr_value_clear, /* not a typo */
+		.clear = op_attr_value_clear,
+	},
+};
+
+TEE_Result syscall_cryp_obj_get_info(unsigned long obj, TEE_ObjectInfo *info)
+{
+	TEE_Result res;
+	struct tee_ta_session *sess;
+	struct tee_obj *o;
+
+	res = tee_ta_get_current_session(&sess);
+	if (res != TEE_SUCCESS)
+		goto exit;
+
+	res = tee_obj_get(to_user_ta_ctx(sess->ctx),
+			  tee_svc_uref_to_vaddr(obj), &o);
+	if (res != TEE_SUCCESS)
+		goto exit;
+
+	res = tee_svc_copy_to_user(info, &o->info, sizeof(o->info));
+
+exit:
+	return res;
+}
+
+TEE_Result syscall_cryp_obj_restrict_usage(unsigned long obj,
+			unsigned long usage)
+{
+	TEE_Result res;
+	struct tee_ta_session *sess;
+	struct tee_obj *o;
+
+	res = tee_ta_get_current_session(&sess);
+	if (res != TEE_SUCCESS)
+		goto exit;
+
+	res = tee_obj_get(to_user_ta_ctx(sess->ctx),
+			  tee_svc_uref_to_vaddr(obj), &o);
+	if (res != TEE_SUCCESS)
+		goto exit;
+
+	o->info.objectUsage &= usage;
+
+exit:
+	return res;
+}
+
+static int tee_svc_cryp_obj_find_type_attr_idx(
+		uint32_t attr_id,
+		const struct tee_cryp_obj_type_props *type_props)
+{
+	size_t n;
+
+	for (n = 0; n < type_props->num_type_attrs; n++) {
+		if (attr_id == type_props->type_attrs[n].attr_id)
+			return n;
+	}
+	return -1;
+}
+
+static const struct tee_cryp_obj_type_props *tee_svc_find_type_props(
+		TEE_ObjectType obj_type)
+{
+	size_t n;
+
+	for (n = 0; n < ARRAY_SIZE(tee_cryp_obj_props); n++) {
+		if (tee_cryp_obj_props[n].obj_type == obj_type)
+			return tee_cryp_obj_props + n;
+	}
+
+	return NULL;
+}
+
+/* Set an attribute on an object */
+static void set_attribute(struct tee_obj *o,
+			  const struct tee_cryp_obj_type_props *props,
+			  uint32_t attr)
+{
+	int idx = tee_svc_cryp_obj_find_type_attr_idx(attr, props);
+
+	if (idx < 0)
+		return;
+	o->have_attrs |= BIT(idx);
+}
+
+/* Get an attribute on an object */
+static uint32_t get_attribute(const struct tee_obj *o,
+			      const struct tee_cryp_obj_type_props *props,
+			      uint32_t attr)
+{
+	int idx = tee_svc_cryp_obj_find_type_attr_idx(attr, props);
+
+	if (idx < 0)
+		return 0;
+	return o->have_attrs & BIT(idx);
+}
+
+TEE_Result syscall_cryp_obj_get_attr(unsigned long obj, unsigned long attr_id,
+			void *buffer, uint64_t *size)
+{
+	TEE_Result res;
+	struct tee_ta_session *sess;
+	struct tee_obj *o;
+	const struct tee_cryp_obj_type_props *type_props;
+	int idx;
+	const struct attr_ops *ops;
+	void *attr;
+
+	res = tee_ta_get_current_session(&sess);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = tee_obj_get(to_user_ta_ctx(sess->ctx),
+			  tee_svc_uref_to_vaddr(obj), &o);
+	if (res != TEE_SUCCESS)
+		return TEE_ERROR_ITEM_NOT_FOUND;
+
+	/* Check that the object is initialized */
+	if (!(o->info.handleFlags & TEE_HANDLE_FLAG_INITIALIZED))
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	/* Check that getting the attribute is allowed */
+	if (!(attr_id & TEE_ATTR_BIT_PROTECTED) &&
+	    !(o->info.objectUsage & TEE_USAGE_EXTRACTABLE))
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	type_props = tee_svc_find_type_props(o->info.objectType);
+	if (!type_props) {
+		/* Unknown object type, "can't happen" */
+		return TEE_ERROR_BAD_STATE;
+	}
+
+	idx = tee_svc_cryp_obj_find_type_attr_idx(attr_id, type_props);
+	if ((idx < 0) || ((o->have_attrs & (1 << idx)) == 0))
+		return TEE_ERROR_ITEM_NOT_FOUND;
+
+	ops = attr_ops + type_props->type_attrs[idx].ops_index;
+	attr = (uint8_t *)o->attr + type_props->type_attrs[idx].raw_offs;
+	return ops->to_user(attr, sess, buffer, size);
+}
+
+void tee_obj_attr_free(struct tee_obj *o)
+{
+	const struct tee_cryp_obj_type_props *tp;
+	size_t n;
+
+	if (!o->attr)
+		return;
+	tp = tee_svc_find_type_props(o->info.objectType);
+	if (!tp)
+		return;
+
+	for (n = 0; n < tp->num_type_attrs; n++) {
+		const struct tee_cryp_obj_type_attrs *ta = tp->type_attrs + n;
+
+		attr_ops[ta->ops_index].free((uint8_t *)o->attr + ta->raw_offs);
+	}
+}
+
+void tee_obj_attr_clear(struct tee_obj *o)
+{
+	const struct tee_cryp_obj_type_props *tp;
+	size_t n;
+
+	if (!o->attr)
+		return;
+	tp = tee_svc_find_type_props(o->info.objectType);
+	if (!tp)
+		return;
+
+	for (n = 0; n < tp->num_type_attrs; n++) {
+		const struct tee_cryp_obj_type_attrs *ta = tp->type_attrs + n;
+
+		attr_ops[ta->ops_index].clear((uint8_t *)o->attr +
+					      ta->raw_offs);
+	}
+}
+
+TEE_Result tee_obj_attr_to_binary(struct tee_obj *o, void *data,
+				  size_t *data_len)
+{
+	const struct tee_cryp_obj_type_props *tp;
+	size_t n;
+	size_t offs = 0;
+	size_t len = data ? *data_len : 0;
+
+	if (o->info.objectType == TEE_TYPE_DATA) {
+		*data_len = 0;
+		return TEE_SUCCESS; /* pure data object */
+	}
+	if (!o->attr)
+		return TEE_ERROR_BAD_STATE;
+	tp = tee_svc_find_type_props(o->info.objectType);
+	if (!tp)
+		return TEE_ERROR_BAD_STATE;
+
+	for (n = 0; n < tp->num_type_attrs; n++) {
+		const struct tee_cryp_obj_type_attrs *ta = tp->type_attrs + n;
+		void *attr = (uint8_t *)o->attr + ta->raw_offs;
+
+		attr_ops[ta->ops_index].to_binary(attr, data, len, &offs);
+	}
+
+	*data_len = offs;
+	if (data && offs > len)
+		return TEE_ERROR_SHORT_BUFFER;
+	return TEE_SUCCESS;
+}
+
+TEE_Result tee_obj_attr_from_binary(struct tee_obj *o, const void *data,
+				    size_t data_len)
+{
+	const struct tee_cryp_obj_type_props *tp;
+	size_t n;
+	size_t offs = 0;
+
+	if (o->info.objectType == TEE_TYPE_DATA)
+		return TEE_SUCCESS; /* pure data object */
+	if (!o->attr)
+		return TEE_ERROR_BAD_STATE;
+	tp = tee_svc_find_type_props(o->info.objectType);
+	if (!tp)
+		return TEE_ERROR_BAD_STATE;
+
+	for (n = 0; n < tp->num_type_attrs; n++) {
+		const struct tee_cryp_obj_type_attrs *ta = tp->type_attrs + n;
+		void *attr = (uint8_t *)o->attr + ta->raw_offs;
+
+		if (!attr_ops[ta->ops_index].from_binary(attr, data, data_len,
+							 &offs))
+			return TEE_ERROR_CORRUPT_OBJECT;
+	}
+	return TEE_SUCCESS;
+}
+
+TEE_Result tee_obj_attr_copy_from(struct tee_obj *o, const struct tee_obj *src)
+{
+	TEE_Result res;
+	const struct tee_cryp_obj_type_props *tp;
+	const struct tee_cryp_obj_type_attrs *ta;
+	size_t n;
+	uint32_t have_attrs = 0;
+	void *attr;
+	void *src_attr;
+
+	if (o->info.objectType == TEE_TYPE_DATA)
+		return TEE_SUCCESS; /* pure data object */
+	if (!o->attr)
+		return TEE_ERROR_BAD_STATE;
+	tp = tee_svc_find_type_props(o->info.objectType);
+	if (!tp)
+		return TEE_ERROR_BAD_STATE;
+
+	if (o->info.objectType == src->info.objectType) {
+		have_attrs = src->have_attrs;
+		for (n = 0; n < tp->num_type_attrs; n++) {
+			ta = tp->type_attrs + n;
+			attr = (uint8_t *)o->attr + ta->raw_offs;
+			src_attr = (uint8_t *)src->attr + ta->raw_offs;
+			res = attr_ops[ta->ops_index].from_obj(attr, src_attr);
+			if (res != TEE_SUCCESS)
+				return res;
+		}
+	} else {
+		const struct tee_cryp_obj_type_props *tp_src;
+		int idx;
+
+		if (o->info.objectType == TEE_TYPE_RSA_PUBLIC_KEY) {
+			if (src->info.objectType != TEE_TYPE_RSA_KEYPAIR)
+				return TEE_ERROR_BAD_PARAMETERS;
+		} else if (o->info.objectType == TEE_TYPE_DSA_PUBLIC_KEY) {
+			if (src->info.objectType != TEE_TYPE_DSA_KEYPAIR)
+				return TEE_ERROR_BAD_PARAMETERS;
+		} else if (o->info.objectType == TEE_TYPE_ECDSA_PUBLIC_KEY) {
+			if (src->info.objectType != TEE_TYPE_ECDSA_KEYPAIR)
+				return TEE_ERROR_BAD_PARAMETERS;
+		} else if (o->info.objectType == TEE_TYPE_ECDH_PUBLIC_KEY) {
+			if (src->info.objectType != TEE_TYPE_ECDH_KEYPAIR)
+				return TEE_ERROR_BAD_PARAMETERS;
+		} else {
+			return TEE_ERROR_BAD_PARAMETERS;
+		}
+
+		tp_src = tee_svc_find_type_props(src->info.objectType);
+		if (!tp_src)
+			return TEE_ERROR_BAD_STATE;
+
+		have_attrs = BIT32(tp->num_type_attrs) - 1;
+		for (n = 0; n < tp->num_type_attrs; n++) {
+			ta = tp->type_attrs + n;
+
+			idx = tee_svc_cryp_obj_find_type_attr_idx(ta->attr_id,
+								  tp_src);
+			if (idx < 0)
+				return TEE_ERROR_BAD_STATE;
+
+			attr = (uint8_t *)o->attr + ta->raw_offs;
+			src_attr = (uint8_t *)src->attr +
+				   tp_src->type_attrs[idx].raw_offs;
+			res = attr_ops[ta->ops_index].from_obj(attr, src_attr);
+			if (res != TEE_SUCCESS)
+				return res;
+		}
+	}
+
+	o->have_attrs = have_attrs;
+	return TEE_SUCCESS;
+}
+
+TEE_Result tee_obj_set_type(struct tee_obj *o, uint32_t obj_type,
+			    size_t max_key_size)
+{
+	TEE_Result res = TEE_SUCCESS;
+	const struct tee_cryp_obj_type_props *type_props;
+
+	/* Can only set type for newly allocated objs */
+	if (o->attr)
+		return TEE_ERROR_BAD_STATE;
+
+	/*
+	 * Verify that maxKeySize is supported and find out how
+	 * much should be allocated.
+	 */
+
+	if (obj_type == TEE_TYPE_DATA) {
+		if (max_key_size)
+			return TEE_ERROR_NOT_SUPPORTED;
+	} else {
+		/* Find description of object */
+		type_props = tee_svc_find_type_props(obj_type);
+		if (!type_props)
+			return TEE_ERROR_NOT_SUPPORTED;
+
+		/* Check that maxKeySize follows restrictions */
+		if (max_key_size % type_props->quanta != 0)
+			return TEE_ERROR_NOT_SUPPORTED;
+		if (max_key_size < type_props->min_size)
+			return TEE_ERROR_NOT_SUPPORTED;
+		if (max_key_size > type_props->max_size)
+			return TEE_ERROR_NOT_SUPPORTED;
+
+		o->attr = calloc(1, type_props->alloc_size);
+		if (!o->attr)
+			return TEE_ERROR_OUT_OF_MEMORY;
+	}
+
+	/* If we have a key structure, pre-allocate the bignums inside */
+	switch (obj_type) {
+	case TEE_TYPE_RSA_PUBLIC_KEY:
+		if (!crypto_ops.acipher.alloc_rsa_public_key)
+			return TEE_ERROR_NOT_IMPLEMENTED;
+		res = crypto_ops.acipher.alloc_rsa_public_key(o->attr,
+							      max_key_size);
+		break;
+	case TEE_TYPE_RSA_KEYPAIR:
+		if (!crypto_ops.acipher.alloc_rsa_keypair)
+			return TEE_ERROR_NOT_IMPLEMENTED;
+		res = crypto_ops.acipher.alloc_rsa_keypair(o->attr,
+							   max_key_size);
+		break;
+	case TEE_TYPE_DSA_PUBLIC_KEY:
+		if (!crypto_ops.acipher.alloc_dsa_public_key)
+			return TEE_ERROR_NOT_IMPLEMENTED;
+		res = crypto_ops.acipher.alloc_dsa_public_key(o->attr,
+							      max_key_size);
+		break;
+	case TEE_TYPE_DSA_KEYPAIR:
+		if (!crypto_ops.acipher.alloc_dsa_keypair)
+			return TEE_ERROR_NOT_IMPLEMENTED;
+		res = crypto_ops.acipher.alloc_dsa_keypair(o->attr,
+							   max_key_size);
+		break;
+	case TEE_TYPE_DH_KEYPAIR:
+		if (!crypto_ops.acipher.alloc_dh_keypair)
+			return TEE_ERROR_NOT_IMPLEMENTED;
+		res = crypto_ops.acipher.alloc_dh_keypair(o->attr,
+							  max_key_size);
+		break;
+	case TEE_TYPE_ECDSA_PUBLIC_KEY:
+	case TEE_TYPE_ECDH_PUBLIC_KEY:
+		if (!crypto_ops.acipher.alloc_ecc_public_key)
+			return TEE_ERROR_NOT_IMPLEMENTED;
+		res = crypto_ops.acipher.alloc_ecc_public_key(o->attr,
+							      max_key_size);
+		break;
+	case TEE_TYPE_ECDSA_KEYPAIR:
+	case TEE_TYPE_ECDH_KEYPAIR:
+		if (!crypto_ops.acipher.alloc_ecc_keypair)
+			return TEE_ERROR_NOT_IMPLEMENTED;
+		res = crypto_ops.acipher.alloc_ecc_keypair(o->attr,
+							   max_key_size);
+		break;
+	default:
+		if (obj_type != TEE_TYPE_DATA) {
+			struct tee_cryp_obj_secret *key = o->attr;
+
+			key->alloc_size = type_props->alloc_size -
+					  sizeof(*key);
+		}
+		break;
+	}
+
+	if (res != TEE_SUCCESS)
+		return res;
+
+	o->info.objectType = obj_type;
+	o->info.maxKeySize = max_key_size;
+	o->info.objectUsage = TEE_USAGE_DEFAULT;
+
+	return TEE_SUCCESS;
+}
+
+TEE_Result syscall_cryp_obj_alloc(unsigned long obj_type,
+			unsigned long max_key_size, uint32_t *obj)
+{
+	TEE_Result res;
+	struct tee_ta_session *sess;
+	struct tee_obj *o;
+
+	if (obj_type == TEE_TYPE_DATA)
+		return TEE_ERROR_NOT_SUPPORTED;
+
+	res = tee_ta_get_current_session(&sess);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	o = tee_obj_alloc();
+	if (!o)
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	res = tee_obj_set_type(o, obj_type, max_key_size);
+	if (res != TEE_SUCCESS) {
+		tee_obj_free(o);
+		return res;
+	}
+
+	tee_obj_add(to_user_ta_ctx(sess->ctx), o);
+
+	res = tee_svc_copy_kaddr_to_uref(obj, o);
+	if (res != TEE_SUCCESS)
+		tee_obj_close(to_user_ta_ctx(sess->ctx), o);
+	return res;
+}
+
+TEE_Result syscall_cryp_obj_close(unsigned long obj)
+{
+	TEE_Result res;
+	struct tee_ta_session *sess;
+	struct tee_obj *o;
+
+	res = tee_ta_get_current_session(&sess);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = tee_obj_get(to_user_ta_ctx(sess->ctx),
+			  tee_svc_uref_to_vaddr(obj), &o);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	/*
+	 * If it's busy it's used by an operation, a client should never have
+	 * this handle.
+	 */
+	if (o->busy)
+		return TEE_ERROR_ITEM_NOT_FOUND;
+
+	tee_obj_close(to_user_ta_ctx(sess->ctx), o);
+	return TEE_SUCCESS;
+}
+
+TEE_Result syscall_cryp_obj_reset(unsigned long obj)
+{
+	TEE_Result res;
+	struct tee_ta_session *sess;
+	struct tee_obj *o;
+
+	res = tee_ta_get_current_session(&sess);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = tee_obj_get(to_user_ta_ctx(sess->ctx),
+			  tee_svc_uref_to_vaddr(obj), &o);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	if ((o->info.handleFlags & TEE_HANDLE_FLAG_PERSISTENT) == 0) {
+		tee_obj_attr_clear(o);
+		o->info.keySize = 0;
+		o->info.objectUsage = TEE_USAGE_DEFAULT;
+	} else {
+		return TEE_ERROR_BAD_PARAMETERS;
+	}
+
+	/* the object is no more initialized */
+	o->info.handleFlags &= ~TEE_HANDLE_FLAG_INITIALIZED;
+
+	return TEE_SUCCESS;
+}
+
+static TEE_Result copy_in_attrs(struct user_ta_ctx *utc,
+			const struct utee_attribute *usr_attrs,
+			uint32_t attr_count, TEE_Attribute *attrs)
+{
+	TEE_Result res;
+	uint32_t n;
+
+	res = tee_mmu_check_access_rights(utc,
+			TEE_MEMORY_ACCESS_READ | TEE_MEMORY_ACCESS_ANY_OWNER,
+			(uaddr_t)usr_attrs,
+			attr_count * sizeof(struct utee_attribute));
+	if (res != TEE_SUCCESS)
+		return res;
+
+	for (n = 0; n < attr_count; n++) {
+		attrs[n].attributeID = usr_attrs[n].attribute_id;
+		if (attrs[n].attributeID & TEE_ATTR_BIT_VALUE) {
+			attrs[n].content.value.a = usr_attrs[n].a;
+			attrs[n].content.value.b = usr_attrs[n].b;
+		} else {
+			uintptr_t buf = usr_attrs[n].a;
+			size_t len = usr_attrs[n].b;
+
+			res = tee_mmu_check_access_rights(utc,
+				TEE_MEMORY_ACCESS_READ |
+				TEE_MEMORY_ACCESS_ANY_OWNER, buf, len);
+			if (res != TEE_SUCCESS)
+				return res;
+			attrs[n].content.ref.buffer = (void *)buf;
+			attrs[n].content.ref.length = len;
+		}
+	}
+
+	return TEE_SUCCESS;
+}
+
+enum attr_usage {
+	ATTR_USAGE_POPULATE,
+	ATTR_USAGE_GENERATE_KEY
+};
+
+static TEE_Result tee_svc_cryp_check_attr(enum attr_usage usage,
+					  const struct tee_cryp_obj_type_props
+						*type_props,
+					  const TEE_Attribute *attrs,
+					  uint32_t attr_count)
+{
+	uint32_t required_flag;
+	uint32_t opt_flag;
+	bool all_opt_needed;
+	uint32_t req_attrs = 0;
+	uint32_t opt_grp_attrs = 0;
+	uint32_t attrs_found = 0;
+	size_t n;
+	uint32_t bit;
+	uint32_t flags;
+	int idx;
+
+	if (usage == ATTR_USAGE_POPULATE) {
+		required_flag = TEE_TYPE_ATTR_REQUIRED;
+		opt_flag = TEE_TYPE_ATTR_OPTIONAL_GROUP;
+		all_opt_needed = true;
+	} else {
+		required_flag = TEE_TYPE_ATTR_GEN_KEY_REQ;
+		opt_flag = TEE_TYPE_ATTR_GEN_KEY_OPT;
+		all_opt_needed = false;
+	}
+
+	/*
+	 * First find out which attributes are required and which belong to
+	 * the optional group
+	 */
+	for (n = 0; n < type_props->num_type_attrs; n++) {
+		bit = 1 << n;
+		flags = type_props->type_attrs[n].flags;
+
+		if (flags & required_flag)
+			req_attrs |= bit;
+		else if (flags & opt_flag)
+			opt_grp_attrs |= bit;
+	}
+
+	/*
+	 * Verify that all required attributes are in place and
+	 * that the same attribute isn't repeated.
+	 */
+	for (n = 0; n < attr_count; n++) {
+		idx = tee_svc_cryp_obj_find_type_attr_idx(
+							attrs[n].attributeID,
+							type_props);
+
+		/* attribute not defined in current object type */
+		if (idx < 0)
+			return TEE_ERROR_ITEM_NOT_FOUND;
+
+		bit = 1 << idx;
+
+		/* attribute not repeated */
+		if ((attrs_found & bit) != 0)
+			return TEE_ERROR_ITEM_NOT_FOUND;
+
+		attrs_found |= bit;
+	}
+	/* Required attribute missing */
+	if ((attrs_found & req_attrs) != req_attrs)
+		return TEE_ERROR_ITEM_NOT_FOUND;
+
+	/*
+	 * If the flag says that "if one of the optional attributes are included
+	 * all of them has to be included" this must be checked.
+	 */
+	if (all_opt_needed && (attrs_found & opt_grp_attrs) != 0 &&
+	    (attrs_found & opt_grp_attrs) != opt_grp_attrs)
+		return TEE_ERROR_ITEM_NOT_FOUND;
+
+	return TEE_SUCCESS;
+}
+
+static TEE_Result tee_svc_cryp_obj_populate_type(
+		struct tee_obj *o,
+		const struct tee_cryp_obj_type_props *type_props,
+		const TEE_Attribute *attrs,
+		uint32_t attr_count)
+{
+	TEE_Result res;
+	uint32_t have_attrs = 0;
+	size_t obj_size = 0;
+	size_t n;
+	int idx;
+	const struct attr_ops *ops;
+	void *attr;
+
+	for (n = 0; n < attr_count; n++) {
+		idx = tee_svc_cryp_obj_find_type_attr_idx(
+							attrs[n].attributeID,
+							type_props);
+		/* attribute not defined in current object type */
+		if (idx < 0)
+			return TEE_ERROR_ITEM_NOT_FOUND;
+
+		have_attrs |= BIT32(idx);
+		ops = attr_ops + type_props->type_attrs[idx].ops_index;
+		attr = (uint8_t *)o->attr +
+		       type_props->type_attrs[idx].raw_offs;
+		if (attrs[n].attributeID & TEE_ATTR_BIT_VALUE)
+			res = ops->from_user(attr, &attrs[n].content.value,
+					     sizeof(attrs[n].content.value));
+		else
+			res = ops->from_user(attr, attrs[n].content.ref.buffer,
+					     attrs[n].content.ref.length);
+		if (res != TEE_SUCCESS)
+			return res;
+
+		/*
+		 * First attr_idx signifies the attribute that gives the size
+		 * of the object
+		 */
+		if (type_props->type_attrs[idx].flags &
+		    TEE_TYPE_ATTR_SIZE_INDICATOR)
+			obj_size += attrs[n].content.ref.length * 8;
+	}
+
+	/*
+	 * We have to do it like this because the parity bits aren't counted
+	 * when telling the size of the key in bits.
+	 */
+	if (o->info.objectType == TEE_TYPE_DES ||
+	    o->info.objectType == TEE_TYPE_DES3)
+		obj_size -= obj_size / 8; /* Exclude parity in size of key */
+
+	o->have_attrs = have_attrs;
+	o->info.keySize = obj_size;
+
+	return TEE_SUCCESS;
+}
+
+TEE_Result syscall_cryp_obj_populate(unsigned long obj,
+			struct utee_attribute *usr_attrs,
+			unsigned long attr_count)
+{
+	TEE_Result res;
+	struct tee_ta_session *sess;
+	struct tee_obj *o;
+	const struct tee_cryp_obj_type_props *type_props;
+	TEE_Attribute *attrs = NULL;
+
+	res = tee_ta_get_current_session(&sess);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = tee_obj_get(to_user_ta_ctx(sess->ctx),
+			  tee_svc_uref_to_vaddr(obj), &o);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	/* Must be a transient object */
+	if ((o->info.handleFlags & TEE_HANDLE_FLAG_PERSISTENT) != 0)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	/* Must not be initialized already */
+	if ((o->info.handleFlags & TEE_HANDLE_FLAG_INITIALIZED) != 0)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	type_props = tee_svc_find_type_props(o->info.objectType);
+	if (!type_props)
+		return TEE_ERROR_NOT_IMPLEMENTED;
+
+	attrs = malloc(sizeof(TEE_Attribute) * attr_count);
+	if (!attrs)
+		return TEE_ERROR_OUT_OF_MEMORY;
+	res = copy_in_attrs(to_user_ta_ctx(sess->ctx), usr_attrs, attr_count,
+			    attrs);
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	res = tee_svc_cryp_check_attr(ATTR_USAGE_POPULATE, type_props,
+				      attrs, attr_count);
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	res = tee_svc_cryp_obj_populate_type(o, type_props, attrs, attr_count);
+	if (res == TEE_SUCCESS)
+		o->info.handleFlags |= TEE_HANDLE_FLAG_INITIALIZED;
+
+out:
+	free(attrs);
+	return res;
+}
+
+TEE_Result syscall_cryp_obj_copy(unsigned long dst, unsigned long src)
+{
+	TEE_Result res;
+	struct tee_ta_session *sess;
+	struct tee_obj *dst_o;
+	struct tee_obj *src_o;
+
+	res = tee_ta_get_current_session(&sess);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = tee_obj_get(to_user_ta_ctx(sess->ctx),
+			  tee_svc_uref_to_vaddr(dst), &dst_o);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = tee_obj_get(to_user_ta_ctx(sess->ctx),
+			  tee_svc_uref_to_vaddr(src), &src_o);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	if ((src_o->info.handleFlags & TEE_HANDLE_FLAG_INITIALIZED) == 0)
+		return TEE_ERROR_BAD_PARAMETERS;
+	if ((dst_o->info.handleFlags & TEE_HANDLE_FLAG_PERSISTENT) != 0)
+		return TEE_ERROR_BAD_PARAMETERS;
+	if ((dst_o->info.handleFlags & TEE_HANDLE_FLAG_INITIALIZED) != 0)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	res = tee_obj_attr_copy_from(dst_o, src_o);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	dst_o->info.handleFlags |= TEE_HANDLE_FLAG_INITIALIZED;
+	dst_o->info.keySize = src_o->info.keySize;
+	dst_o->info.objectUsage = src_o->info.objectUsage;
+	return TEE_SUCCESS;
+}
+
+static TEE_Result tee_svc_obj_generate_key_rsa(
+	struct tee_obj *o, const struct tee_cryp_obj_type_props *type_props,
+	uint32_t key_size,
+	const TEE_Attribute *params, uint32_t param_count)
+{
+	TEE_Result res;
+	struct rsa_keypair *key = o->attr;
+	uint32_t e = TEE_U32_TO_BIG_ENDIAN(65537);
+
+	if (!crypto_ops.acipher.gen_rsa_key || !crypto_ops.bignum.bin2bn)
+		return TEE_ERROR_NOT_IMPLEMENTED;
+
+	/* Copy the present attributes into the obj before starting */
+	res = tee_svc_cryp_obj_populate_type(o, type_props, params,
+					     param_count);
+	if (res != TEE_SUCCESS)
+		return res;
+	if (!get_attribute(o, type_props, TEE_ATTR_RSA_PUBLIC_EXPONENT))
+		crypto_ops.bignum.bin2bn((const uint8_t *)&e, sizeof(e),
+					 key->e);
+	res = crypto_ops.acipher.gen_rsa_key(key, key_size);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	/* Set bits for all known attributes for this object type */
+	o->have_attrs = (1 << type_props->num_type_attrs) - 1;
+
+	return TEE_SUCCESS;
+}
+
+static TEE_Result tee_svc_obj_generate_key_dsa(
+	struct tee_obj *o, const struct tee_cryp_obj_type_props *type_props,
+	uint32_t key_size)
+{
+	TEE_Result res;
+
+	if (!crypto_ops.acipher.gen_dsa_key)
+		return TEE_ERROR_NOT_IMPLEMENTED;
+	res = crypto_ops.acipher.gen_dsa_key(o->attr, key_size);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	/* Set bits for all known attributes for this object type */
+	o->have_attrs = (1 << type_props->num_type_attrs) - 1;
+
+	return TEE_SUCCESS;
+}
+
+static TEE_Result tee_svc_obj_generate_key_dh(
+	struct tee_obj *o, const struct tee_cryp_obj_type_props *type_props,
+	uint32_t key_size __unused,
+	const TEE_Attribute *params, uint32_t param_count)
+{
+	TEE_Result res;
+	struct dh_keypair *tee_dh_key;
+	struct bignum *dh_q = NULL;
+	uint32_t dh_xbits = 0;
+
+	/* Copy the present attributes into the obj before starting */
+	res = tee_svc_cryp_obj_populate_type(o, type_props, params,
+					     param_count);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	tee_dh_key = (struct dh_keypair *)o->attr;
+
+	if (get_attribute(o, type_props, TEE_ATTR_DH_SUBPRIME))
+		dh_q = tee_dh_key->q;
+	if (get_attribute(o, type_props, TEE_ATTR_DH_X_BITS))
+		dh_xbits = tee_dh_key->xbits;
+	if (!crypto_ops.acipher.gen_dh_key)
+		return TEE_ERROR_NOT_IMPLEMENTED;
+	res = crypto_ops.acipher.gen_dh_key(tee_dh_key, dh_q, dh_xbits);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	/* Set bits for the generated public and private key */
+	set_attribute(o, type_props, TEE_ATTR_DH_PUBLIC_VALUE);
+	set_attribute(o, type_props, TEE_ATTR_DH_PRIVATE_VALUE);
+	set_attribute(o, type_props, TEE_ATTR_DH_X_BITS);
+	return TEE_SUCCESS;
+}
+
+static TEE_Result tee_svc_obj_generate_key_ecc(
+	struct tee_obj *o, const struct tee_cryp_obj_type_props *type_props,
+	uint32_t key_size __unused,
+	const TEE_Attribute *params, uint32_t param_count)
+{
+	TEE_Result res;
+	struct ecc_keypair *tee_ecc_key;
+
+	/* Copy the present attributes into the obj before starting */
+	res = tee_svc_cryp_obj_populate_type(o, type_props, params,
+					     param_count);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	tee_ecc_key = (struct ecc_keypair *)o->attr;
+
+	if (!crypto_ops.acipher.gen_ecc_key)
+		return TEE_ERROR_NOT_IMPLEMENTED;
+	res = crypto_ops.acipher.gen_ecc_key(tee_ecc_key);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	/* Set bits for the generated public and private key */
+	set_attribute(o, type_props, TEE_ATTR_ECC_PRIVATE_VALUE);
+	set_attribute(o, type_props, TEE_ATTR_ECC_PUBLIC_VALUE_X);
+	set_attribute(o, type_props, TEE_ATTR_ECC_PUBLIC_VALUE_Y);
+	set_attribute(o, type_props, TEE_ATTR_ECC_CURVE);
+	return TEE_SUCCESS;
+}
+
+TEE_Result syscall_obj_generate_key(unsigned long obj, unsigned long key_size,
+			const struct utee_attribute *usr_params,
+			unsigned long param_count)
+{
+	TEE_Result res;
+	struct tee_ta_session *sess;
+	const struct tee_cryp_obj_type_props *type_props;
+	struct tee_obj *o;
+	struct tee_cryp_obj_secret *key;
+	size_t byte_size;
+	TEE_Attribute *params = NULL;
+
+	res = tee_ta_get_current_session(&sess);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = tee_obj_get(to_user_ta_ctx(sess->ctx),
+			  tee_svc_uref_to_vaddr(obj), &o);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	/* Must be a transient object */
+	if ((o->info.handleFlags & TEE_HANDLE_FLAG_PERSISTENT) != 0)
+		return TEE_ERROR_BAD_STATE;
+
+	/* Must not be initialized already */
+	if ((o->info.handleFlags & TEE_HANDLE_FLAG_INITIALIZED) != 0)
+		return TEE_ERROR_BAD_STATE;
+
+	/* Find description of object */
+	type_props = tee_svc_find_type_props(o->info.objectType);
+	if (!type_props)
+		return TEE_ERROR_NOT_SUPPORTED;
+
+	/* Check that maxKeySize follows restrictions */
+	if (key_size % type_props->quanta != 0)
+		return TEE_ERROR_NOT_SUPPORTED;
+	if (key_size < type_props->min_size)
+		return TEE_ERROR_NOT_SUPPORTED;
+	if (key_size > type_props->max_size)
+		return TEE_ERROR_NOT_SUPPORTED;
+
+	params = malloc(sizeof(TEE_Attribute) * param_count);
+	if (!params)
+		return TEE_ERROR_OUT_OF_MEMORY;
+	res = copy_in_attrs(to_user_ta_ctx(sess->ctx), usr_params, param_count,
+			    params);
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	res = tee_svc_cryp_check_attr(ATTR_USAGE_GENERATE_KEY, type_props,
+				      params, param_count);
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	switch (o->info.objectType) {
+	case TEE_TYPE_AES:
+	case TEE_TYPE_DES:
+	case TEE_TYPE_DES3:
+	case TEE_TYPE_HMAC_MD5:
+	case TEE_TYPE_HMAC_SHA1:
+	case TEE_TYPE_HMAC_SHA224:
+	case TEE_TYPE_HMAC_SHA256:
+	case TEE_TYPE_HMAC_SHA384:
+	case TEE_TYPE_HMAC_SHA512:
+	case TEE_TYPE_GENERIC_SECRET:
+		byte_size = key_size / 8;
+
+		/*
+		 * We have to do it like this because the parity bits aren't
+		 * counted when telling the size of the key in bits.
+		 */
+		if (o->info.objectType == TEE_TYPE_DES ||
+		    o->info.objectType == TEE_TYPE_DES3) {
+			byte_size = (key_size + key_size / 7) / 8;
+		}
+
+		key = (struct tee_cryp_obj_secret *)o->attr;
+		if (byte_size > key->alloc_size) {
+			res = TEE_ERROR_EXCESS_DATA;
+			goto out;
+		}
+
+		res = crypto_ops.prng.read((void *)(key + 1), byte_size);
+		if (res != TEE_SUCCESS)
+			goto out;
+
+		key->key_size = byte_size;
+
+		/* Set bits for all known attributes for this object type */
+		o->have_attrs = (1 << type_props->num_type_attrs) - 1;
+
+		break;
+
+	case TEE_TYPE_RSA_KEYPAIR:
+		res = tee_svc_obj_generate_key_rsa(o, type_props, key_size,
+						   params, param_count);
+		if (res != TEE_SUCCESS)
+			goto out;
+		break;
+
+	case TEE_TYPE_DSA_KEYPAIR:
+		res = tee_svc_obj_generate_key_dsa(o, type_props, key_size);
+		if (res != TEE_SUCCESS)
+			goto out;
+		break;
+
+	case TEE_TYPE_DH_KEYPAIR:
+		res = tee_svc_obj_generate_key_dh(o, type_props, key_size,
+						  params, param_count);
+		if (res != TEE_SUCCESS)
+			goto out;
+		break;
+
+	case TEE_TYPE_ECDSA_KEYPAIR:
+	case TEE_TYPE_ECDH_KEYPAIR:
+		res = tee_svc_obj_generate_key_ecc(o, type_props, key_size,
+						  params, param_count);
+		if (res != TEE_SUCCESS)
+			goto out;
+		break;
+
+	default:
+		res = TEE_ERROR_BAD_FORMAT;
+	}
+
+out:
+	free(params);
+	if (res == TEE_SUCCESS) {
+		o->info.keySize = key_size;
+		o->info.handleFlags |= TEE_HANDLE_FLAG_INITIALIZED;
+	}
+	return res;
+}
+
+static TEE_Result tee_svc_cryp_get_state(struct tee_ta_session *sess,
+					 uint32_t state_id,
+					 struct tee_cryp_state **state)
+{
+	struct tee_cryp_state *s;
+	struct user_ta_ctx *utc = to_user_ta_ctx(sess->ctx);
+
+	TAILQ_FOREACH(s, &utc->cryp_states, link) {
+		if (state_id == (vaddr_t)s) {
+			*state = s;
+			return TEE_SUCCESS;
+		}
+	}
+	return TEE_ERROR_BAD_PARAMETERS;
+}
+
+static void cryp_state_free(struct user_ta_ctx *utc, struct tee_cryp_state *cs)
+{
+	struct tee_obj *o;
+
+	if (tee_obj_get(utc, cs->key1, &o) == TEE_SUCCESS)
+		tee_obj_close(utc, o);
+	if (tee_obj_get(utc, cs->key2, &o) == TEE_SUCCESS)
+		tee_obj_close(utc, o);
+
+	TAILQ_REMOVE(&utc->cryp_states, cs, link);
+	if (cs->ctx_finalize != NULL)
+		cs->ctx_finalize(cs->ctx, cs->algo);
+	free(cs->ctx);
+	free(cs);
+}
+
+static TEE_Result tee_svc_cryp_check_key_type(const struct tee_obj *o,
+					      uint32_t algo,
+					      TEE_OperationMode mode)
+{
+	uint32_t req_key_type;
+	uint32_t req_key_type2 = 0;
+
+	switch (TEE_ALG_GET_MAIN_ALG(algo)) {
+	case TEE_MAIN_ALGO_MD5:
+		req_key_type = TEE_TYPE_HMAC_MD5;
+		break;
+	case TEE_MAIN_ALGO_SHA1:
+		req_key_type = TEE_TYPE_HMAC_SHA1;
+		break;
+	case TEE_MAIN_ALGO_SHA224:
+		req_key_type = TEE_TYPE_HMAC_SHA224;
+		break;
+	case TEE_MAIN_ALGO_SHA256:
+		req_key_type = TEE_TYPE_HMAC_SHA256;
+		break;
+	case TEE_MAIN_ALGO_SHA384:
+		req_key_type = TEE_TYPE_HMAC_SHA384;
+		break;
+	case TEE_MAIN_ALGO_SHA512:
+		req_key_type = TEE_TYPE_HMAC_SHA512;
+		break;
+	case TEE_MAIN_ALGO_AES:
+		req_key_type = TEE_TYPE_AES;
+		break;
+	case TEE_MAIN_ALGO_DES:
+		req_key_type = TEE_TYPE_DES;
+		break;
+	case TEE_MAIN_ALGO_DES3:
+		req_key_type = TEE_TYPE_DES3;
+		break;
+	case TEE_MAIN_ALGO_RSA:
+		req_key_type = TEE_TYPE_RSA_KEYPAIR;
+		if (mode == TEE_MODE_ENCRYPT || mode == TEE_MODE_VERIFY)
+			req_key_type2 = TEE_TYPE_RSA_PUBLIC_KEY;
+		break;
+	case TEE_MAIN_ALGO_DSA:
+		req_key_type = TEE_TYPE_DSA_KEYPAIR;
+		if (mode == TEE_MODE_ENCRYPT || mode == TEE_MODE_VERIFY)
+			req_key_type2 = TEE_TYPE_DSA_PUBLIC_KEY;
+		break;
+	case TEE_MAIN_ALGO_DH:
+		req_key_type = TEE_TYPE_DH_KEYPAIR;
+		break;
+	case TEE_MAIN_ALGO_ECDSA:
+		req_key_type = TEE_TYPE_ECDSA_KEYPAIR;
+		if (mode == TEE_MODE_VERIFY)
+			req_key_type2 = TEE_TYPE_ECDSA_PUBLIC_KEY;
+		break;
+	case TEE_MAIN_ALGO_ECDH:
+		req_key_type = TEE_TYPE_ECDH_KEYPAIR;
+		break;
+#if defined(CFG_CRYPTO_HKDF)
+	case TEE_MAIN_ALGO_HKDF:
+		req_key_type = TEE_TYPE_HKDF_IKM;
+		break;
+#endif
+#if defined(CFG_CRYPTO_CONCAT_KDF)
+	case TEE_MAIN_ALGO_CONCAT_KDF:
+		req_key_type = TEE_TYPE_CONCAT_KDF_Z;
+		break;
+#endif
+#if defined(CFG_CRYPTO_PBKDF2)
+	case TEE_MAIN_ALGO_PBKDF2:
+		req_key_type = TEE_TYPE_PBKDF2_PASSWORD;
+		break;
+#endif
+	default:
+		return TEE_ERROR_BAD_PARAMETERS;
+	}
+
+	if (req_key_type != o->info.objectType &&
+	    req_key_type2 != o->info.objectType)
+		return TEE_ERROR_BAD_PARAMETERS;
+	return TEE_SUCCESS;
+}
+
+TEE_Result syscall_cryp_state_alloc(unsigned long algo, unsigned long mode,
+			unsigned long key1, unsigned long key2,
+			uint32_t *state)
+{
+	TEE_Result res;
+	struct tee_cryp_state *cs;
+	struct tee_ta_session *sess;
+	struct tee_obj *o1 = NULL;
+	struct tee_obj *o2 = NULL;
+	struct user_ta_ctx *utc;
+
+	res = tee_ta_get_current_session(&sess);
+	if (res != TEE_SUCCESS)
+		return res;
+	utc = to_user_ta_ctx(sess->ctx);
+
+	if (key1 != 0) {
+		res = tee_obj_get(utc, tee_svc_uref_to_vaddr(key1), &o1);
+		if (res != TEE_SUCCESS)
+			return res;
+		if (o1->busy)
+			return TEE_ERROR_BAD_PARAMETERS;
+		res = tee_svc_cryp_check_key_type(o1, algo, mode);
+		if (res != TEE_SUCCESS)
+			return res;
+	}
+	if (key2 != 0) {
+		res = tee_obj_get(utc, tee_svc_uref_to_vaddr(key2), &o2);
+		if (res != TEE_SUCCESS)
+			return res;
+		if (o2->busy)
+			return TEE_ERROR_BAD_PARAMETERS;
+		res = tee_svc_cryp_check_key_type(o2, algo, mode);
+		if (res != TEE_SUCCESS)
+			return res;
+	}
+
+	cs = calloc(1, sizeof(struct tee_cryp_state));
+	if (!cs)
+		return TEE_ERROR_OUT_OF_MEMORY;
+	TAILQ_INSERT_TAIL(&utc->cryp_states, cs, link);
+	cs->algo = algo;
+	cs->mode = mode;
+
+	switch (TEE_ALG_GET_CLASS(algo)) {
+	case TEE_OPERATION_CIPHER:
+		if ((algo == TEE_ALG_AES_XTS && (key1 == 0 || key2 == 0)) ||
+		    (algo != TEE_ALG_AES_XTS && (key1 == 0 || key2 != 0))) {
+			res = TEE_ERROR_BAD_PARAMETERS;
+		} else {
+			if (crypto_ops.cipher.get_ctx_size)
+				res = crypto_ops.cipher.get_ctx_size(algo,
+								&cs->ctx_size);
+			else
+				res = TEE_ERROR_NOT_IMPLEMENTED;
+			if (res != TEE_SUCCESS)
+				break;
+			cs->ctx = calloc(1, cs->ctx_size);
+			if (!cs->ctx)
+				res = TEE_ERROR_OUT_OF_MEMORY;
+		}
+		break;
+	case TEE_OPERATION_AE:
+		if (key1 == 0 || key2 != 0) {
+			res = TEE_ERROR_BAD_PARAMETERS;
+		} else {
+			if (crypto_ops.authenc.get_ctx_size)
+				res = crypto_ops.authenc.get_ctx_size(algo,
+								&cs->ctx_size);
+			else
+				res = TEE_ERROR_NOT_IMPLEMENTED;
+			if (res != TEE_SUCCESS)
+				break;
+			cs->ctx = calloc(1, cs->ctx_size);
+			if (!cs->ctx)
+				res = TEE_ERROR_OUT_OF_MEMORY;
+		}
+		break;
+	case TEE_OPERATION_MAC:
+		if (key1 == 0 || key2 != 0) {
+			res = TEE_ERROR_BAD_PARAMETERS;
+		} else {
+			if (crypto_ops.mac.get_ctx_size)
+				res = crypto_ops.mac.get_ctx_size(algo,
+								&cs->ctx_size);
+			else
+				res = TEE_ERROR_NOT_IMPLEMENTED;
+			if (res != TEE_SUCCESS)
+				break;
+			cs->ctx = calloc(1, cs->ctx_size);
+			if (!cs->ctx)
+				res = TEE_ERROR_OUT_OF_MEMORY;
+		}
+		break;
+	case TEE_OPERATION_DIGEST:
+		if (key1 != 0 || key2 != 0) {
+			res = TEE_ERROR_BAD_PARAMETERS;
+		} else {
+			if (crypto_ops.hash.get_ctx_size)
+				res = crypto_ops.hash.get_ctx_size(algo,
+								&cs->ctx_size);
+			else
+				res = TEE_ERROR_NOT_IMPLEMENTED;
+			if (res != TEE_SUCCESS)
+				break;
+			cs->ctx = calloc(1, cs->ctx_size);
+			if (!cs->ctx)
+				res = TEE_ERROR_OUT_OF_MEMORY;
+		}
+		break;
+	case TEE_OPERATION_ASYMMETRIC_CIPHER:
+	case TEE_OPERATION_ASYMMETRIC_SIGNATURE:
+		if (key1 == 0 || key2 != 0)
+			res = TEE_ERROR_BAD_PARAMETERS;
+		break;
+	case TEE_OPERATION_KEY_DERIVATION:
+		if (key1 == 0 || key2 != 0)
+			res = TEE_ERROR_BAD_PARAMETERS;
+		break;
+	default:
+		res = TEE_ERROR_NOT_SUPPORTED;
+		break;
+	}
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	res = tee_svc_copy_kaddr_to_uref(state, cs);
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	/* Register keys */
+	if (o1 != NULL) {
+		o1->busy = true;
+		cs->key1 = (vaddr_t)o1;
+	}
+	if (o2 != NULL) {
+		o2->busy = true;
+		cs->key2 = (vaddr_t)o2;
+	}
+
+out:
+	if (res != TEE_SUCCESS)
+		cryp_state_free(utc, cs);
+	return res;
+}
+
+TEE_Result syscall_cryp_state_copy(unsigned long dst, unsigned long src)
+{
+	TEE_Result res;
+	struct tee_cryp_state *cs_dst;
+	struct tee_cryp_state *cs_src;
+	struct tee_ta_session *sess;
+
+	res = tee_ta_get_current_session(&sess);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = tee_svc_cryp_get_state(sess, tee_svc_uref_to_vaddr(dst), &cs_dst);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = tee_svc_cryp_get_state(sess, tee_svc_uref_to_vaddr(src), &cs_src);
+	if (res != TEE_SUCCESS)
+		return res;
+	if (cs_dst->algo != cs_src->algo || cs_dst->mode != cs_src->mode)
+		return TEE_ERROR_BAD_PARAMETERS;
+	/* "Can't happen" */
+	if (cs_dst->ctx_size != cs_src->ctx_size)
+		return TEE_ERROR_BAD_STATE;
+
+	memcpy(cs_dst->ctx, cs_src->ctx, cs_src->ctx_size);
+	return TEE_SUCCESS;
+}
+
+void tee_svc_cryp_free_states(struct user_ta_ctx *utc)
+{
+	struct tee_cryp_state_head *states = &utc->cryp_states;
+
+	while (!TAILQ_EMPTY(states))
+		cryp_state_free(utc, TAILQ_FIRST(states));
+}
+
+TEE_Result syscall_cryp_state_free(unsigned long state)
+{
+	TEE_Result res;
+	struct tee_cryp_state *cs;
+	struct tee_ta_session *sess;
+
+	res = tee_ta_get_current_session(&sess);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = tee_svc_cryp_get_state(sess, tee_svc_uref_to_vaddr(state), &cs);
+	if (res != TEE_SUCCESS)
+		return res;
+	cryp_state_free(to_user_ta_ctx(sess->ctx), cs);
+	return TEE_SUCCESS;
+}
+
+TEE_Result syscall_hash_init(unsigned long state,
+			     const void *iv __maybe_unused,
+			     size_t iv_len __maybe_unused)
+{
+	TEE_Result res;
+	struct tee_cryp_state *cs;
+	struct tee_ta_session *sess;
+
+	res = tee_ta_get_current_session(&sess);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = tee_svc_cryp_get_state(sess, tee_svc_uref_to_vaddr(state), &cs);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	switch (TEE_ALG_GET_CLASS(cs->algo)) {
+	case TEE_OPERATION_DIGEST:
+		if (!crypto_ops.hash.init)
+			return TEE_ERROR_NOT_IMPLEMENTED;
+		res = crypto_ops.hash.init(cs->ctx, cs->algo);
+		if (res != TEE_SUCCESS)
+			return res;
+		break;
+	case TEE_OPERATION_MAC:
+		{
+			struct tee_obj *o;
+			struct tee_cryp_obj_secret *key;
+
+			res = tee_obj_get(to_user_ta_ctx(sess->ctx),
+					  cs->key1, &o);
+			if (res != TEE_SUCCESS)
+				return res;
+			if ((o->info.handleFlags &
+			     TEE_HANDLE_FLAG_INITIALIZED) == 0)
+				return TEE_ERROR_BAD_PARAMETERS;
+
+			key = (struct tee_cryp_obj_secret *)o->attr;
+			if (!crypto_ops.mac.init)
+				return TEE_ERROR_NOT_IMPLEMENTED;
+			res = crypto_ops.mac.init(cs->ctx, cs->algo,
+						  (void *)(key + 1),
+						  key->key_size);
+			if (res != TEE_SUCCESS)
+				return res;
+			break;
+		}
+	default:
+		return TEE_ERROR_BAD_PARAMETERS;
+	}
+
+	return TEE_SUCCESS;
+}
+
+TEE_Result syscall_hash_update(unsigned long state, const void *chunk,
+			size_t chunk_size)
+{
+	TEE_Result res;
+	struct tee_cryp_state *cs;
+	struct tee_ta_session *sess;
+
+	/* No data, but size provided isn't valid parameters. */
+	if (!chunk && chunk_size)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	/* Zero length hash is valid, but nothing we need to do. */
+	if (!chunk_size)
+		return TEE_SUCCESS;
+
+	res = tee_ta_get_current_session(&sess);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = tee_mmu_check_access_rights(to_user_ta_ctx(sess->ctx),
+					  TEE_MEMORY_ACCESS_READ |
+					  TEE_MEMORY_ACCESS_ANY_OWNER,
+					  (uaddr_t)chunk, chunk_size);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = tee_svc_cryp_get_state(sess, tee_svc_uref_to_vaddr(state), &cs);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	switch (TEE_ALG_GET_CLASS(cs->algo)) {
+	case TEE_OPERATION_DIGEST:
+		if (!crypto_ops.hash.update)
+			return TEE_ERROR_NOT_IMPLEMENTED;
+		res = crypto_ops.hash.update(cs->ctx, cs->algo, chunk,
+					     chunk_size);
+		if (res != TEE_SUCCESS)
+			return res;
+		break;
+	case TEE_OPERATION_MAC:
+		if (!crypto_ops.mac.update)
+			return TEE_ERROR_NOT_IMPLEMENTED;
+		res = crypto_ops.mac.update(cs->ctx, cs->algo, chunk,
+					    chunk_size);
+		if (res != TEE_SUCCESS)
+			return res;
+		break;
+	default:
+		return TEE_ERROR_BAD_PARAMETERS;
+	}
+
+	return TEE_SUCCESS;
+}
+
+TEE_Result syscall_hash_final(unsigned long state, const void *chunk,
+			size_t chunk_size, void *hash, uint64_t *hash_len)
+{
+	TEE_Result res, res2;
+	size_t hash_size;
+	uint64_t hlen;
+	struct tee_cryp_state *cs;
+	struct tee_ta_session *sess;
+
+	/* No data, but size provided isn't valid parameters. */
+	if (!chunk && chunk_size)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	res = tee_ta_get_current_session(&sess);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = tee_mmu_check_access_rights(to_user_ta_ctx(sess->ctx),
+					  TEE_MEMORY_ACCESS_READ |
+					  TEE_MEMORY_ACCESS_ANY_OWNER,
+					  (uaddr_t)chunk, chunk_size);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = tee_svc_copy_from_user(&hlen, hash_len, sizeof(hlen));
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = tee_mmu_check_access_rights(to_user_ta_ctx(sess->ctx),
+					  TEE_MEMORY_ACCESS_READ |
+					  TEE_MEMORY_ACCESS_WRITE |
+					  TEE_MEMORY_ACCESS_ANY_OWNER,
+					  (uaddr_t)hash, hlen);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = tee_svc_cryp_get_state(sess, tee_svc_uref_to_vaddr(state), &cs);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	switch (TEE_ALG_GET_CLASS(cs->algo)) {
+	case TEE_OPERATION_DIGEST:
+		if (!crypto_ops.hash.update || !crypto_ops.hash.final)
+			return TEE_ERROR_NOT_IMPLEMENTED;
+		res = tee_hash_get_digest_size(cs->algo, &hash_size);
+		if (res != TEE_SUCCESS)
+			return res;
+		if (*hash_len < hash_size) {
+			res = TEE_ERROR_SHORT_BUFFER;
+			goto out;
+		}
+
+		if (chunk_size) {
+			res = crypto_ops.hash.update(cs->ctx, cs->algo, chunk,
+						     chunk_size);
+			if (res != TEE_SUCCESS)
+				return res;
+		}
+
+		res = crypto_ops.hash.final(cs->ctx, cs->algo, hash,
+					    hash_size);
+		if (res != TEE_SUCCESS)
+			return res;
+		break;
+
+	case TEE_OPERATION_MAC:
+		if (!crypto_ops.mac.update || !crypto_ops.mac.final)
+			return TEE_ERROR_NOT_IMPLEMENTED;
+		res = tee_mac_get_digest_size(cs->algo, &hash_size);
+		if (res != TEE_SUCCESS)
+			return res;
+		if (*hash_len < hash_size) {
+			res = TEE_ERROR_SHORT_BUFFER;
+			goto out;
+		}
+
+		if (chunk_size) {
+			res = crypto_ops.mac.update(cs->ctx, cs->algo, chunk,
+						    chunk_size);
+			if (res != TEE_SUCCESS)
+				return res;
+		}
+
+		res = crypto_ops.mac.final(cs->ctx, cs->algo, hash, hash_size);
+		if (res != TEE_SUCCESS)
+			return res;
+		break;
+
+	default:
+		return TEE_ERROR_BAD_PARAMETERS;
+	}
+out:
+	hlen = hash_size;
+	res2 = tee_svc_copy_to_user(hash_len, &hlen, sizeof(*hash_len));
+	if (res2 != TEE_SUCCESS)
+		return res2;
+	return res;
+}
+
+TEE_Result syscall_cipher_init(unsigned long state, const void *iv,
+			size_t iv_len)
+{
+	TEE_Result res;
+	struct tee_cryp_state *cs;
+	struct tee_ta_session *sess;
+	struct tee_obj *o;
+	struct tee_cryp_obj_secret *key1;
+	struct user_ta_ctx *utc;
+
+	res = tee_ta_get_current_session(&sess);
+	if (res != TEE_SUCCESS)
+		return res;
+	utc = to_user_ta_ctx(sess->ctx);
+
+	res = tee_svc_cryp_get_state(sess, tee_svc_uref_to_vaddr(state), &cs);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = tee_mmu_check_access_rights(utc,
+					  TEE_MEMORY_ACCESS_READ |
+					  TEE_MEMORY_ACCESS_ANY_OWNER,
+					  (uaddr_t) iv, iv_len);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = tee_obj_get(utc, cs->key1, &o);
+	if (res != TEE_SUCCESS)
+		return res;
+	if ((o->info.handleFlags & TEE_HANDLE_FLAG_INITIALIZED) == 0)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	key1 = o->attr;
+
+	if (!crypto_ops.cipher.init)
+		return TEE_ERROR_NOT_IMPLEMENTED;
+
+	if (tee_obj_get(utc, cs->key2, &o) == TEE_SUCCESS) {
+		struct tee_cryp_obj_secret *key2 = o->attr;
+
+		if ((o->info.handleFlags & TEE_HANDLE_FLAG_INITIALIZED) == 0)
+			return TEE_ERROR_BAD_PARAMETERS;
+
+		res = crypto_ops.cipher.init(cs->ctx, cs->algo, cs->mode,
+					     (uint8_t *)(key1 + 1),
+					     key1->key_size,
+					     (uint8_t *)(key2 + 1),
+					     key2->key_size,
+					     iv, iv_len);
+	} else {
+		res = crypto_ops.cipher.init(cs->ctx, cs->algo, cs->mode,
+					     (uint8_t *)(key1 + 1),
+					     key1->key_size,
+					     NULL,
+					     0,
+					     iv, iv_len);
+	}
+	if (res != TEE_SUCCESS)
+		return res;
+
+	cs->ctx_finalize = crypto_ops.cipher.final;
+	return TEE_SUCCESS;
+}
+
+static TEE_Result tee_svc_cipher_update_helper(unsigned long state,
+			bool last_block, const void *src, size_t src_len,
+			void *dst, uint64_t *dst_len)
+{
+	TEE_Result res;
+	struct tee_cryp_state *cs;
+	struct tee_ta_session *sess;
+	uint64_t dlen;
+
+	res = tee_ta_get_current_session(&sess);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = tee_svc_cryp_get_state(sess, tee_svc_uref_to_vaddr(state), &cs);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = tee_mmu_check_access_rights(to_user_ta_ctx(sess->ctx),
+					  TEE_MEMORY_ACCESS_READ |
+					  TEE_MEMORY_ACCESS_ANY_OWNER,
+					  (uaddr_t)src, src_len);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	if (!dst_len) {
+		dlen = 0;
+	} else {
+		res = tee_svc_copy_from_user(&dlen, dst_len, sizeof(dlen));
+		if (res != TEE_SUCCESS)
+			return res;
+
+		res = tee_mmu_check_access_rights(to_user_ta_ctx(sess->ctx),
+						  TEE_MEMORY_ACCESS_READ |
+						  TEE_MEMORY_ACCESS_WRITE |
+						  TEE_MEMORY_ACCESS_ANY_OWNER,
+						  (uaddr_t)dst, dlen);
+		if (res != TEE_SUCCESS)
+			return res;
+	}
+
+	if (dlen < src_len) {
+		res = TEE_ERROR_SHORT_BUFFER;
+		goto out;
+	}
+
+	if (src_len > 0) {
+		/* Permit src_len == 0 to finalize the operation */
+		res = tee_do_cipher_update(cs->ctx, cs->algo, cs->mode,
+					   last_block, src, src_len, dst);
+	}
+
+	if (last_block && cs->ctx_finalize != NULL) {
+		cs->ctx_finalize(cs->ctx, cs->algo);
+		cs->ctx_finalize = NULL;
+	}
+
+out:
+	if ((res == TEE_SUCCESS || res == TEE_ERROR_SHORT_BUFFER) &&
+	    dst_len != NULL) {
+		TEE_Result res2;
+
+		dlen = src_len;
+		res2 = tee_svc_copy_to_user(dst_len, &dlen, sizeof(*dst_len));
+		if (res2 != TEE_SUCCESS)
+			res = res2;
+	}
+
+	return res;
+}
+
+TEE_Result syscall_cipher_update(unsigned long state, const void *src,
+			size_t src_len, void *dst, uint64_t *dst_len)
+{
+	return tee_svc_cipher_update_helper(state, false /* last_block */,
+					    src, src_len, dst, dst_len);
+}
+
+TEE_Result syscall_cipher_final(unsigned long state, const void *src,
+			size_t src_len, void *dst, uint64_t *dst_len)
+{
+	return tee_svc_cipher_update_helper(state, true /* last_block */,
+					    src, src_len, dst, dst_len);
+}
+
+#if defined(CFG_CRYPTO_HKDF)
+static TEE_Result get_hkdf_params(const TEE_Attribute *params,
+				  uint32_t param_count,
+				  void **salt, size_t *salt_len, void **info,
+				  size_t *info_len, size_t *okm_len)
+{
+	size_t n;
+	enum { SALT = 0x1, LENGTH = 0x2, INFO = 0x4 };
+	uint8_t found = 0;
+
+	*salt = *info = NULL;
+	*salt_len = *info_len = *okm_len = 0;
+
+	for (n = 0; n < param_count; n++) {
+		switch (params[n].attributeID) {
+		case TEE_ATTR_HKDF_SALT:
+			if (!(found & SALT)) {
+				*salt = params[n].content.ref.buffer;
+				*salt_len = params[n].content.ref.length;
+				found |= SALT;
+			}
+			break;
+		case TEE_ATTR_HKDF_OKM_LENGTH:
+			if (!(found & LENGTH)) {
+				*okm_len = params[n].content.value.a;
+				found |= LENGTH;
+			}
+			break;
+		case TEE_ATTR_HKDF_INFO:
+			if (!(found & INFO)) {
+				*info = params[n].content.ref.buffer;
+				*info_len = params[n].content.ref.length;
+				found |= INFO;
+			}
+			break;
+		default:
+			/* Unexpected attribute */
+			return TEE_ERROR_BAD_PARAMETERS;
+		}
+
+	}
+
+	if (!(found & LENGTH))
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	return TEE_SUCCESS;
+}
+#endif
+
+#if defined(CFG_CRYPTO_CONCAT_KDF)
+static TEE_Result get_concat_kdf_params(const TEE_Attribute *params,
+					uint32_t param_count,
+					void **other_info,
+					size_t *other_info_len,
+					size_t *derived_key_len)
+{
+	size_t n;
+	enum { LENGTH = 0x1, INFO = 0x2 };
+	uint8_t found = 0;
+
+	*other_info = NULL;
+	*other_info_len = *derived_key_len = 0;
+
+	for (n = 0; n < param_count; n++) {
+		switch (params[n].attributeID) {
+		case TEE_ATTR_CONCAT_KDF_OTHER_INFO:
+			if (!(found & INFO)) {
+				*other_info = params[n].content.ref.buffer;
+				*other_info_len = params[n].content.ref.length;
+				found |= INFO;
+			}
+			break;
+		case TEE_ATTR_CONCAT_KDF_DKM_LENGTH:
+			if (!(found & LENGTH)) {
+				*derived_key_len = params[n].content.value.a;
+				found |= LENGTH;
+			}
+			break;
+		default:
+			/* Unexpected attribute */
+			return TEE_ERROR_BAD_PARAMETERS;
+		}
+	}
+
+	if (!(found & LENGTH))
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	return TEE_SUCCESS;
+}
+#endif
+
+#if defined(CFG_CRYPTO_PBKDF2)
+static TEE_Result get_pbkdf2_params(const TEE_Attribute *params,
+				   uint32_t param_count, void **salt,
+				   size_t *salt_len, size_t *derived_key_len,
+				   size_t *iteration_count)
+{
+	size_t n;
+	enum { SALT = 0x1, LENGTH = 0x2, COUNT = 0x4 };
+	uint8_t found = 0;
+
+	*salt = NULL;
+	*salt_len = *derived_key_len = *iteration_count = 0;
+
+	for (n = 0; n < param_count; n++) {
+		switch (params[n].attributeID) {
+		case TEE_ATTR_PBKDF2_SALT:
+			if (!(found & SALT)) {
+				*salt = params[n].content.ref.buffer;
+				*salt_len = params[n].content.ref.length;
+				found |= SALT;
+			}
+			break;
+		case TEE_ATTR_PBKDF2_DKM_LENGTH:
+			if (!(found & LENGTH)) {
+				*derived_key_len = params[n].content.value.a;
+				found |= LENGTH;
+			}
+			break;
+		case TEE_ATTR_PBKDF2_ITERATION_COUNT:
+			if (!(found & COUNT)) {
+				*iteration_count = params[n].content.value.a;
+				found |= COUNT;
+			}
+			break;
+		default:
+			/* Unexpected attribute */
+			return TEE_ERROR_BAD_PARAMETERS;
+		}
+	}
+
+	if ((found & (LENGTH|COUNT)) != (LENGTH|COUNT))
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	return TEE_SUCCESS;
+}
+#endif
+
+TEE_Result syscall_cryp_derive_key(unsigned long state,
+			const struct utee_attribute *usr_params,
+			unsigned long param_count, unsigned long derived_key)
+{
+	TEE_Result res = TEE_ERROR_NOT_SUPPORTED;
+	struct tee_ta_session *sess;
+	struct tee_obj *ko;
+	struct tee_obj *so;
+	struct tee_cryp_state *cs;
+	struct tee_cryp_obj_secret *sk;
+	const struct tee_cryp_obj_type_props *type_props;
+	TEE_Attribute *params = NULL;
+	struct user_ta_ctx *utc;
+
+	res = tee_ta_get_current_session(&sess);
+	if (res != TEE_SUCCESS)
+		return res;
+	utc = to_user_ta_ctx(sess->ctx);
+
+	res = tee_svc_cryp_get_state(sess, tee_svc_uref_to_vaddr(state), &cs);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	params = malloc(sizeof(TEE_Attribute) * param_count);
+	if (!params)
+		return TEE_ERROR_OUT_OF_MEMORY;
+	res = copy_in_attrs(utc, usr_params, param_count, params);
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	/* Get key set in operation */
+	res = tee_obj_get(utc, cs->key1, &ko);
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	res = tee_obj_get(utc, tee_svc_uref_to_vaddr(derived_key), &so);
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	/* Find information needed about the object to initialize */
+	sk = so->attr;
+
+	/* Find description of object */
+	type_props = tee_svc_find_type_props(so->info.objectType);
+	if (!type_props) {
+		res = TEE_ERROR_NOT_SUPPORTED;
+		goto out;
+	}
+
+	if (cs->algo == TEE_ALG_DH_DERIVE_SHARED_SECRET) {
+		size_t alloc_size;
+		struct bignum *pub;
+		struct bignum *ss;
+
+		if (!crypto_ops.bignum.allocate ||
+		    !crypto_ops.bignum.free ||
+		    !crypto_ops.bignum.bin2bn ||
+		    !crypto_ops.bignum.bn2bin ||
+		    !crypto_ops.bignum.num_bytes ||
+		    !crypto_ops.acipher.dh_shared_secret) {
+			res = TEE_ERROR_NOT_IMPLEMENTED;
+			goto out;
+		}
+		if (param_count != 1 ||
+		    params[0].attributeID != TEE_ATTR_DH_PUBLIC_VALUE) {
+			res = TEE_ERROR_BAD_PARAMETERS;
+			goto out;
+		}
+
+		alloc_size = params[0].content.ref.length * 8;
+		pub = crypto_ops.bignum.allocate(alloc_size);
+		ss = crypto_ops.bignum.allocate(alloc_size);
+		if (pub && ss) {
+			crypto_ops.bignum.bin2bn(params[0].content.ref.buffer,
+					params[0].content.ref.length, pub);
+			res = crypto_ops.acipher.dh_shared_secret(ko->attr,
+								  pub, ss);
+			if (res == TEE_SUCCESS) {
+				sk->key_size = crypto_ops.bignum.num_bytes(ss);
+				crypto_ops.bignum.bn2bin(ss,
+							 (uint8_t *)(sk + 1));
+				so->info.handleFlags |=
+						TEE_HANDLE_FLAG_INITIALIZED;
+				set_attribute(so, type_props,
+					      TEE_ATTR_SECRET_VALUE);
+			}
+		} else {
+			res = TEE_ERROR_OUT_OF_MEMORY;
+		}
+		crypto_ops.bignum.free(pub);
+		crypto_ops.bignum.free(ss);
+	} else if (TEE_ALG_GET_MAIN_ALG(cs->algo) == TEE_MAIN_ALGO_ECDH) {
+		size_t alloc_size;
+		struct ecc_public_key key_public;
+		uint8_t *pt_secret;
+		unsigned long pt_secret_len;
+
+		if (!crypto_ops.bignum.bin2bn ||
+		    !crypto_ops.acipher.alloc_ecc_public_key ||
+		    !crypto_ops.acipher.free_ecc_public_key ||
+		    !crypto_ops.acipher.ecc_shared_secret) {
+			res = TEE_ERROR_NOT_IMPLEMENTED;
+			goto out;
+		}
+		if (param_count != 2 ||
+		    params[0].attributeID != TEE_ATTR_ECC_PUBLIC_VALUE_X ||
+		    params[1].attributeID != TEE_ATTR_ECC_PUBLIC_VALUE_Y) {
+			res = TEE_ERROR_BAD_PARAMETERS;
+			goto out;
+		}
+
+		switch (cs->algo) {
+		case TEE_ALG_ECDH_P192:
+			alloc_size = 192;
+			break;
+		case TEE_ALG_ECDH_P224:
+			alloc_size = 224;
+			break;
+		case TEE_ALG_ECDH_P256:
+			alloc_size = 256;
+			break;
+		case TEE_ALG_ECDH_P384:
+			alloc_size = 384;
+			break;
+		case TEE_ALG_ECDH_P521:
+			alloc_size = 521;
+			break;
+		default:
+			res = TEE_ERROR_NOT_IMPLEMENTED;
+			goto out;
+		}
+
+		/* Create the public key */
+		res = crypto_ops.acipher.alloc_ecc_public_key(&key_public,
+							      alloc_size);
+		if (res != TEE_SUCCESS)
+			goto out;
+		key_public.curve = ((struct ecc_keypair *)ko->attr)->curve;
+		crypto_ops.bignum.bin2bn(params[0].content.ref.buffer,
+					 params[0].content.ref.length,
+					 key_public.x);
+		crypto_ops.bignum.bin2bn(params[1].content.ref.buffer,
+					 params[1].content.ref.length,
+					 key_public.y);
+
+		pt_secret = (uint8_t *)(sk + 1);
+		pt_secret_len = sk->alloc_size;
+		res = crypto_ops.acipher.ecc_shared_secret(ko->attr,
+				&key_public, pt_secret, &pt_secret_len);
+
+		if (res == TEE_SUCCESS) {
+			sk->key_size = pt_secret_len;
+			so->info.handleFlags |= TEE_HANDLE_FLAG_INITIALIZED;
+			set_attribute(so, type_props, TEE_ATTR_SECRET_VALUE);
+		}
+
+		/* free the public key */
+		crypto_ops.acipher.free_ecc_public_key(&key_public);
+	}
+#if defined(CFG_CRYPTO_HKDF)
+	else if (TEE_ALG_GET_MAIN_ALG(cs->algo) == TEE_MAIN_ALGO_HKDF) {
+		void *salt, *info;
+		size_t salt_len, info_len, okm_len;
+		uint32_t hash_id = TEE_ALG_GET_DIGEST_HASH(cs->algo);
+		struct tee_cryp_obj_secret *ik = ko->attr;
+		const uint8_t *ikm = (const uint8_t *)(ik + 1);
+
+		res = get_hkdf_params(params, param_count, &salt, &salt_len,
+				      &info, &info_len, &okm_len);
+		if (res != TEE_SUCCESS)
+			goto out;
+
+		/* Requested size must fit into the output object's buffer */
+		if (okm_len > ik->alloc_size) {
+			res = TEE_ERROR_BAD_PARAMETERS;
+			goto out;
+		}
+
+		res = tee_cryp_hkdf(hash_id, ikm, ik->key_size, salt, salt_len,
+				    info, info_len, (uint8_t *)(sk + 1),
+				    okm_len);
+		if (res == TEE_SUCCESS) {
+			sk->key_size = okm_len;
+			so->info.handleFlags |= TEE_HANDLE_FLAG_INITIALIZED;
+			set_attribute(so, type_props, TEE_ATTR_SECRET_VALUE);
+		}
+	}
+#endif
+#if defined(CFG_CRYPTO_CONCAT_KDF)
+	else if (TEE_ALG_GET_MAIN_ALG(cs->algo) == TEE_MAIN_ALGO_CONCAT_KDF) {
+		void *info;
+		size_t info_len, derived_key_len;
+		uint32_t hash_id = TEE_ALG_GET_DIGEST_HASH(cs->algo);
+		struct tee_cryp_obj_secret *ss = ko->attr;
+		const uint8_t *shared_secret = (const uint8_t *)(ss + 1);
+
+		res = get_concat_kdf_params(params, param_count, &info,
+					    &info_len, &derived_key_len);
+		if (res != TEE_SUCCESS)
+			goto out;
+
+		/* Requested size must fit into the output object's buffer */
+		if (derived_key_len > ss->alloc_size) {
+			res = TEE_ERROR_BAD_PARAMETERS;
+			goto out;
+		}
+
+		res = tee_cryp_concat_kdf(hash_id, shared_secret, ss->key_size,
+					  info, info_len, (uint8_t *)(sk + 1),
+					  derived_key_len);
+		if (res == TEE_SUCCESS) {
+			sk->key_size = derived_key_len;
+			so->info.handleFlags |= TEE_HANDLE_FLAG_INITIALIZED;
+			set_attribute(so, type_props, TEE_ATTR_SECRET_VALUE);
+		}
+	}
+#endif
+#if defined(CFG_CRYPTO_PBKDF2)
+	else if (TEE_ALG_GET_MAIN_ALG(cs->algo) == TEE_MAIN_ALGO_PBKDF2) {
+		void *salt;
+		size_t salt_len, iteration_count, derived_key_len;
+		uint32_t hash_id = TEE_ALG_GET_DIGEST_HASH(cs->algo);
+		struct tee_cryp_obj_secret *ss = ko->attr;
+		const uint8_t *password = (const uint8_t *)(ss + 1);
+
+		res = get_pbkdf2_params(params, param_count, &salt, &salt_len,
+					&derived_key_len, &iteration_count);
+		if (res != TEE_SUCCESS)
+			goto out;
+
+		/* Requested size must fit into the output object's buffer */
+		if (derived_key_len > ss->alloc_size) {
+			res = TEE_ERROR_BAD_PARAMETERS;
+			goto out;
+		}
+
+		res = tee_cryp_pbkdf2(hash_id, password, ss->key_size, salt,
+				      salt_len, iteration_count,
+				      (uint8_t *)(sk + 1), derived_key_len);
+		if (res == TEE_SUCCESS) {
+			sk->key_size = derived_key_len;
+			so->info.handleFlags |= TEE_HANDLE_FLAG_INITIALIZED;
+			set_attribute(so, type_props, TEE_ATTR_SECRET_VALUE);
+		}
+	}
+#endif
+	else
+		res = TEE_ERROR_NOT_SUPPORTED;
+
+out:
+	free(params);
+	return res;
+}
+
+TEE_Result syscall_cryp_random_number_generate(void *buf, size_t blen)
+{
+	TEE_Result res;
+	struct tee_ta_session *sess;
+
+	res = tee_ta_get_current_session(&sess);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = tee_mmu_check_access_rights(to_user_ta_ctx(sess->ctx),
+					  TEE_MEMORY_ACCESS_WRITE |
+					  TEE_MEMORY_ACCESS_ANY_OWNER,
+					  (uaddr_t)buf, blen);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = crypto_ops.prng.read(buf, blen);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	return res;
+}
+
+TEE_Result syscall_authenc_init(unsigned long state, const void *nonce,
+			size_t nonce_len, size_t tag_len,
+			size_t aad_len, size_t payload_len)
+{
+	TEE_Result res;
+	struct tee_cryp_state *cs;
+	struct tee_ta_session *sess;
+	struct tee_obj *o;
+	struct tee_cryp_obj_secret *key;
+
+	res = tee_ta_get_current_session(&sess);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = tee_svc_cryp_get_state(sess, tee_svc_uref_to_vaddr(state), &cs);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = tee_obj_get(to_user_ta_ctx(sess->ctx), cs->key1, &o);
+	if (res != TEE_SUCCESS)
+		return res;
+	if ((o->info.handleFlags & TEE_HANDLE_FLAG_INITIALIZED) == 0)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	if (!crypto_ops.authenc.init)
+		return TEE_ERROR_NOT_IMPLEMENTED;
+	key = o->attr;
+	res = crypto_ops.authenc.init(cs->ctx, cs->algo, cs->mode,
+				      (uint8_t *)(key + 1), key->key_size,
+				      nonce, nonce_len, tag_len, aad_len,
+				      payload_len);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	cs->ctx_finalize = (tee_cryp_ctx_finalize_func_t)
+				crypto_ops.authenc.final;
+	return TEE_SUCCESS;
+}
+
+TEE_Result syscall_authenc_update_aad(unsigned long state,
+			const void *aad_data, size_t aad_data_len)
+{
+	TEE_Result res;
+	struct tee_cryp_state *cs;
+	struct tee_ta_session *sess;
+
+	res = tee_ta_get_current_session(&sess);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = tee_mmu_check_access_rights(to_user_ta_ctx(sess->ctx),
+					  TEE_MEMORY_ACCESS_READ |
+					  TEE_MEMORY_ACCESS_ANY_OWNER,
+					  (uaddr_t) aad_data,
+					  aad_data_len);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = tee_svc_cryp_get_state(sess, tee_svc_uref_to_vaddr(state), &cs);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	if (!crypto_ops.authenc.update_aad)
+		return TEE_ERROR_NOT_IMPLEMENTED;
+	res = crypto_ops.authenc.update_aad(cs->ctx, cs->algo, cs->mode,
+					    aad_data, aad_data_len);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	return TEE_SUCCESS;
+}
+
+TEE_Result syscall_authenc_update_payload(unsigned long state,
+			const void *src_data, size_t src_len, void *dst_data,
+			uint64_t *dst_len)
+{
+	TEE_Result res;
+	struct tee_cryp_state *cs;
+	struct tee_ta_session *sess;
+	uint64_t dlen;
+	size_t tmp_dlen;
+
+	res = tee_ta_get_current_session(&sess);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = tee_svc_cryp_get_state(sess, tee_svc_uref_to_vaddr(state), &cs);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = tee_mmu_check_access_rights(to_user_ta_ctx(sess->ctx),
+					  TEE_MEMORY_ACCESS_READ |
+					  TEE_MEMORY_ACCESS_ANY_OWNER,
+					  (uaddr_t) src_data, src_len);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = tee_svc_copy_from_user(&dlen, dst_len, sizeof(dlen));
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = tee_mmu_check_access_rights(to_user_ta_ctx(sess->ctx),
+					  TEE_MEMORY_ACCESS_READ |
+					  TEE_MEMORY_ACCESS_WRITE |
+					  TEE_MEMORY_ACCESS_ANY_OWNER,
+					  (uaddr_t)dst_data, dlen);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	if (dlen < src_len) {
+		res = TEE_ERROR_SHORT_BUFFER;
+		goto out;
+	}
+
+	if (!crypto_ops.authenc.update_payload)
+		return TEE_ERROR_NOT_IMPLEMENTED;
+	tmp_dlen = dlen;
+	res = crypto_ops.authenc.update_payload(cs->ctx, cs->algo, cs->mode,
+						src_data, src_len, dst_data,
+						&tmp_dlen);
+	dlen = tmp_dlen;
+
+out:
+	if (res == TEE_SUCCESS || res == TEE_ERROR_SHORT_BUFFER) {
+		TEE_Result res2 = tee_svc_copy_to_user(dst_len, &dlen,
+						       sizeof(*dst_len));
+		if (res2 != TEE_SUCCESS)
+			res = res2;
+	}
+
+	return res;
+}
+
+TEE_Result syscall_authenc_enc_final(unsigned long state,
+			const void *src_data, size_t src_len, void *dst_data,
+			uint64_t *dst_len, void *tag, uint64_t *tag_len)
+{
+	TEE_Result res;
+	struct tee_cryp_state *cs;
+	struct tee_ta_session *sess;
+	uint64_t dlen;
+	uint64_t tlen;
+	size_t tmp_dlen;
+	size_t tmp_tlen;
+
+	res = tee_ta_get_current_session(&sess);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = tee_svc_cryp_get_state(sess, tee_svc_uref_to_vaddr(state), &cs);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	if (cs->mode != TEE_MODE_ENCRYPT)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	res = tee_mmu_check_access_rights(to_user_ta_ctx(sess->ctx),
+					  TEE_MEMORY_ACCESS_READ |
+					  TEE_MEMORY_ACCESS_ANY_OWNER,
+					  (uaddr_t)src_data, src_len);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	if (!dst_len) {
+		dlen = 0;
+	} else {
+		res = tee_svc_copy_from_user(&dlen, dst_len, sizeof(dlen));
+		if (res != TEE_SUCCESS)
+			return res;
+
+		res = tee_mmu_check_access_rights(to_user_ta_ctx(sess->ctx),
+						  TEE_MEMORY_ACCESS_READ |
+						  TEE_MEMORY_ACCESS_WRITE |
+						  TEE_MEMORY_ACCESS_ANY_OWNER,
+						  (uaddr_t)dst_data, dlen);
+		if (res != TEE_SUCCESS)
+			return res;
+	}
+
+	if (dlen < src_len) {
+		res = TEE_ERROR_SHORT_BUFFER;
+		goto out;
+	}
+
+	res = tee_svc_copy_from_user(&tlen, tag_len, sizeof(tlen));
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = tee_mmu_check_access_rights(to_user_ta_ctx(sess->ctx),
+					  TEE_MEMORY_ACCESS_READ |
+					  TEE_MEMORY_ACCESS_WRITE |
+					  TEE_MEMORY_ACCESS_ANY_OWNER,
+					  (uaddr_t)tag, tlen);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	if (!crypto_ops.authenc.enc_final)
+		return TEE_ERROR_NOT_IMPLEMENTED;
+	tmp_dlen = dlen;
+	tmp_tlen = tlen;
+	res = crypto_ops.authenc.enc_final(cs->ctx, cs->algo, src_data,
+					   src_len, dst_data, &tmp_dlen, tag,
+					   &tmp_tlen);
+	dlen = tmp_dlen;
+	tlen = tmp_tlen;
+
+out:
+	if (res == TEE_SUCCESS || res == TEE_ERROR_SHORT_BUFFER) {
+		TEE_Result res2;
+
+		if (dst_len != NULL) {
+			res2 = tee_svc_copy_to_user(dst_len, &dlen,
+						    sizeof(*dst_len));
+			if (res2 != TEE_SUCCESS)
+				return res2;
+		}
+
+		res2 = tee_svc_copy_to_user(tag_len, &tlen, sizeof(*tag_len));
+		if (res2 != TEE_SUCCESS)
+			return res2;
+	}
+
+	return res;
+}
+
+TEE_Result syscall_authenc_dec_final(unsigned long state,
+			const void *src_data, size_t src_len, void *dst_data,
+			uint64_t *dst_len, const void *tag, size_t tag_len)
+{
+	TEE_Result res;
+	struct tee_cryp_state *cs;
+	struct tee_ta_session *sess;
+	uint64_t dlen;
+	size_t tmp_dlen;
+
+	res = tee_ta_get_current_session(&sess);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = tee_svc_cryp_get_state(sess, tee_svc_uref_to_vaddr(state), &cs);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	if (cs->mode != TEE_MODE_DECRYPT)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	res = tee_mmu_check_access_rights(to_user_ta_ctx(sess->ctx),
+					  TEE_MEMORY_ACCESS_READ |
+					  TEE_MEMORY_ACCESS_ANY_OWNER,
+					  (uaddr_t)src_data, src_len);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	if (!dst_len) {
+		dlen = 0;
+	} else {
+		res = tee_svc_copy_from_user(&dlen, dst_len, sizeof(dlen));
+		if (res != TEE_SUCCESS)
+			return res;
+
+		res = tee_mmu_check_access_rights(to_user_ta_ctx(sess->ctx),
+						  TEE_MEMORY_ACCESS_READ |
+						  TEE_MEMORY_ACCESS_WRITE |
+						  TEE_MEMORY_ACCESS_ANY_OWNER,
+						  (uaddr_t)dst_data, dlen);
+		if (res != TEE_SUCCESS)
+			return res;
+	}
+
+	if (dlen < src_len) {
+		res = TEE_ERROR_SHORT_BUFFER;
+		goto out;
+	}
+
+	res = tee_mmu_check_access_rights(to_user_ta_ctx(sess->ctx),
+					  TEE_MEMORY_ACCESS_READ |
+					  TEE_MEMORY_ACCESS_ANY_OWNER,
+					  (uaddr_t)tag, tag_len);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	if (!crypto_ops.authenc.dec_final)
+		return TEE_ERROR_NOT_IMPLEMENTED;
+	tmp_dlen = dlen;
+	res = crypto_ops.authenc.dec_final(cs->ctx, cs->algo, src_data,
+					   src_len, dst_data, &tmp_dlen, tag,
+					   tag_len);
+	dlen = tmp_dlen;
+
+out:
+	if ((res == TEE_SUCCESS || res == TEE_ERROR_SHORT_BUFFER) &&
+	    dst_len != NULL) {
+		TEE_Result res2;
+
+		res2 = tee_svc_copy_to_user(dst_len, &dlen, sizeof(*dst_len));
+		if (res2 != TEE_SUCCESS)
+			return res2;
+	}
+
+	return res;
+}
+
+static int pkcs1_get_salt_len(const TEE_Attribute *params, uint32_t num_params,
+			      size_t default_len)
+{
+	size_t n;
+
+	assert(default_len < INT_MAX);
+
+	for (n = 0; n < num_params; n++) {
+		if (params[n].attributeID == TEE_ATTR_RSA_PSS_SALT_LENGTH) {
+			if (params[n].content.value.a < INT_MAX)
+				return params[n].content.value.a;
+			break;
+		}
+	}
+	/*
+	 * If salt length isn't provided use the default value which is
+	 * the length of the digest.
+	 */
+	return default_len;
+}
+
+TEE_Result syscall_asymm_operate(unsigned long state,
+			const struct utee_attribute *usr_params,
+			size_t num_params, const void *src_data, size_t src_len,
+			void *dst_data, uint64_t *dst_len)
+{
+	TEE_Result res;
+	struct tee_cryp_state *cs;
+	struct tee_ta_session *sess;
+	uint64_t dlen64;
+	size_t dlen;
+	struct tee_obj *o;
+	void *label = NULL;
+	size_t label_len = 0;
+	size_t n;
+	int salt_len;
+	TEE_Attribute *params = NULL;
+	struct user_ta_ctx *utc;
+
+	res = tee_ta_get_current_session(&sess);
+	if (res != TEE_SUCCESS)
+		return res;
+	utc = to_user_ta_ctx(sess->ctx);
+
+	res = tee_svc_cryp_get_state(sess, tee_svc_uref_to_vaddr(state), &cs);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = tee_mmu_check_access_rights(
+		utc,
+		TEE_MEMORY_ACCESS_READ | TEE_MEMORY_ACCESS_ANY_OWNER,
+		(uaddr_t) src_data, src_len);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = tee_svc_copy_from_user(&dlen64, dst_len, sizeof(dlen64));
+	if (res != TEE_SUCCESS)
+		return res;
+	dlen = dlen64;
+
+	res = tee_mmu_check_access_rights(
+		utc,
+		TEE_MEMORY_ACCESS_READ | TEE_MEMORY_ACCESS_WRITE |
+			TEE_MEMORY_ACCESS_ANY_OWNER,
+		(uaddr_t) dst_data, dlen);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	params = malloc(sizeof(TEE_Attribute) * num_params);
+	if (!params)
+		return TEE_ERROR_OUT_OF_MEMORY;
+	res = copy_in_attrs(utc, usr_params, num_params, params);
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	res = tee_obj_get(utc, cs->key1, &o);
+	if (res != TEE_SUCCESS)
+		goto out;
+	if ((o->info.handleFlags & TEE_HANDLE_FLAG_INITIALIZED) == 0) {
+		res = TEE_ERROR_GENERIC;
+		goto out;
+	}
+
+	switch (cs->algo) {
+	case TEE_ALG_RSA_NOPAD:
+		if (cs->mode == TEE_MODE_ENCRYPT) {
+			if (crypto_ops.acipher.rsanopad_encrypt)
+				res = crypto_ops.acipher.rsanopad_encrypt(
+					o->attr, src_data, src_len,
+					dst_data, &dlen);
+			else
+				res = TEE_ERROR_NOT_IMPLEMENTED;
+		} else if (cs->mode == TEE_MODE_DECRYPT) {
+			if (crypto_ops.acipher.rsanopad_decrypt)
+				res = crypto_ops.acipher.rsanopad_decrypt(
+					o->attr, src_data, src_len, dst_data,
+					&dlen);
+			else
+				res = TEE_ERROR_NOT_IMPLEMENTED;
+		} else {
+			/*
+			 * We will panic because "the mode is not compatible
+			 * with the function"
+			 */
+			res = TEE_ERROR_GENERIC;
+		}
+		break;
+
+	case TEE_ALG_RSAES_PKCS1_V1_5:
+	case TEE_ALG_RSAES_PKCS1_OAEP_MGF1_SHA1:
+	case TEE_ALG_RSAES_PKCS1_OAEP_MGF1_SHA224:
+	case TEE_ALG_RSAES_PKCS1_OAEP_MGF1_SHA256:
+	case TEE_ALG_RSAES_PKCS1_OAEP_MGF1_SHA384:
+	case TEE_ALG_RSAES_PKCS1_OAEP_MGF1_SHA512:
+		for (n = 0; n < num_params; n++) {
+			if (params[n].attributeID == TEE_ATTR_RSA_OAEP_LABEL) {
+				label = params[n].content.ref.buffer;
+				label_len = params[n].content.ref.length;
+				break;
+			}
+		}
+
+		if (cs->mode == TEE_MODE_ENCRYPT) {
+			if (crypto_ops.acipher.rsaes_encrypt)
+				res = crypto_ops.acipher.rsaes_encrypt(
+					cs->algo, o->attr, label, label_len,
+					src_data, src_len, dst_data, &dlen);
+			else
+				res = TEE_ERROR_NOT_IMPLEMENTED;
+		} else if (cs->mode == TEE_MODE_DECRYPT) {
+			if (crypto_ops.acipher.rsaes_decrypt)
+				res = crypto_ops.acipher.rsaes_decrypt(
+					cs->algo, o->attr,
+					label, label_len,
+					src_data, src_len, dst_data, &dlen);
+			else
+				res = TEE_ERROR_NOT_IMPLEMENTED;
+		} else {
+			res = TEE_ERROR_BAD_PARAMETERS;
+		}
+		break;
+
+	case TEE_ALG_RSASSA_PKCS1_V1_5_MD5:
+	case TEE_ALG_RSASSA_PKCS1_V1_5_SHA1:
+	case TEE_ALG_RSASSA_PKCS1_V1_5_SHA224:
+	case TEE_ALG_RSASSA_PKCS1_V1_5_SHA256:
+	case TEE_ALG_RSASSA_PKCS1_V1_5_SHA384:
+	case TEE_ALG_RSASSA_PKCS1_V1_5_SHA512:
+	case TEE_ALG_RSASSA_PKCS1_PSS_MGF1_SHA1:
+	case TEE_ALG_RSASSA_PKCS1_PSS_MGF1_SHA224:
+	case TEE_ALG_RSASSA_PKCS1_PSS_MGF1_SHA256:
+	case TEE_ALG_RSASSA_PKCS1_PSS_MGF1_SHA384:
+	case TEE_ALG_RSASSA_PKCS1_PSS_MGF1_SHA512:
+		if (cs->mode != TEE_MODE_SIGN) {
+			res = TEE_ERROR_BAD_PARAMETERS;
+			break;
+		}
+		salt_len = pkcs1_get_salt_len(params, num_params, src_len);
+		if (!crypto_ops.acipher.rsassa_sign) {
+			res = TEE_ERROR_NOT_IMPLEMENTED;
+			break;
+		}
+		res = crypto_ops.acipher.rsassa_sign(cs->algo, o->attr,
+						     salt_len, src_data,
+						     src_len, dst_data, &dlen);
+		break;
+
+	case TEE_ALG_DSA_SHA1:
+	case TEE_ALG_DSA_SHA224:
+	case TEE_ALG_DSA_SHA256:
+		if (!crypto_ops.acipher.dsa_sign) {
+			res = TEE_ERROR_NOT_IMPLEMENTED;
+			break;
+		}
+		res = crypto_ops.acipher.dsa_sign(cs->algo, o->attr, src_data,
+						  src_len, dst_data, &dlen);
+		break;
+	case TEE_ALG_ECDSA_P192:
+	case TEE_ALG_ECDSA_P224:
+	case TEE_ALG_ECDSA_P256:
+	case TEE_ALG_ECDSA_P384:
+	case TEE_ALG_ECDSA_P521:
+		if (!crypto_ops.acipher.ecc_sign) {
+			res = TEE_ERROR_NOT_IMPLEMENTED;
+			break;
+		}
+		res = crypto_ops.acipher.ecc_sign(cs->algo, o->attr, src_data,
+						  src_len, dst_data, &dlen);
+		break;
+
+	default:
+		res = TEE_ERROR_BAD_PARAMETERS;
+		break;
+	}
+
+out:
+	free(params);
+
+	if (res == TEE_SUCCESS || res == TEE_ERROR_SHORT_BUFFER) {
+		TEE_Result res2;
+
+		dlen64 = dlen;
+		res2 = tee_svc_copy_to_user(dst_len, &dlen64, sizeof(*dst_len));
+		if (res2 != TEE_SUCCESS)
+			return res2;
+	}
+
+	return res;
+}
+
+TEE_Result syscall_asymm_verify(unsigned long state,
+			const struct utee_attribute *usr_params,
+			size_t num_params, const void *data, size_t data_len,
+			const void *sig, size_t sig_len)
+{
+	TEE_Result res;
+	struct tee_cryp_state *cs;
+	struct tee_ta_session *sess;
+	struct tee_obj *o;
+	size_t hash_size;
+	int salt_len;
+	TEE_Attribute *params = NULL;
+	uint32_t hash_algo;
+	struct user_ta_ctx *utc;
+
+	res = tee_ta_get_current_session(&sess);
+	if (res != TEE_SUCCESS)
+		return res;
+	utc = to_user_ta_ctx(sess->ctx);
+
+	res = tee_svc_cryp_get_state(sess, tee_svc_uref_to_vaddr(state), &cs);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	if (cs->mode != TEE_MODE_VERIFY)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	res = tee_mmu_check_access_rights(utc,
+					  TEE_MEMORY_ACCESS_READ |
+					  TEE_MEMORY_ACCESS_ANY_OWNER,
+					  (uaddr_t)data, data_len);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = tee_mmu_check_access_rights(utc,
+					  TEE_MEMORY_ACCESS_READ |
+					  TEE_MEMORY_ACCESS_ANY_OWNER,
+					  (uaddr_t)sig, sig_len);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	params = malloc(sizeof(TEE_Attribute) * num_params);
+	if (!params)
+		return TEE_ERROR_OUT_OF_MEMORY;
+	res = copy_in_attrs(utc, usr_params, num_params, params);
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	res = tee_obj_get(utc, cs->key1, &o);
+	if (res != TEE_SUCCESS)
+		goto out;
+	if ((o->info.handleFlags & TEE_HANDLE_FLAG_INITIALIZED) == 0) {
+		res = TEE_ERROR_BAD_PARAMETERS;
+		goto out;
+	}
+
+	if (TEE_ALG_GET_MAIN_ALG(cs->algo) == TEE_MAIN_ALGO_ECDSA)
+		hash_algo = TEE_ALG_SHA1;
+	else
+		hash_algo = TEE_DIGEST_HASH_TO_ALGO(cs->algo);
+
+	res = tee_hash_get_digest_size(hash_algo, &hash_size);
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	if (TEE_ALG_GET_MAIN_ALG(cs->algo) == TEE_MAIN_ALGO_DSA) {
+		/*
+		 * Depending on the DSA algorithm (NIST), the digital signature
+		 * output size may be truncated to the size of a key pair
+		 * (Q prime size). Q prime size must be less or equal than the
+		 * hash output length of the hash algorithm involved.
+		 */
+		if (data_len > hash_size) {
+			res = TEE_ERROR_BAD_PARAMETERS;
+			goto out;
+		}
+	} else {
+		if (data_len != hash_size) {
+			res = TEE_ERROR_BAD_PARAMETERS;
+			goto out;
+		}
+	}
+
+	switch (TEE_ALG_GET_MAIN_ALG(cs->algo)) {
+	case TEE_MAIN_ALGO_RSA:
+		salt_len = pkcs1_get_salt_len(params, num_params, hash_size);
+		if (!crypto_ops.acipher.rsassa_verify) {
+			res = TEE_ERROR_NOT_IMPLEMENTED;
+			break;
+		}
+		res = crypto_ops.acipher.rsassa_verify(cs->algo, o->attr,
+						       salt_len, data,
+						       data_len, sig, sig_len);
+		break;
+
+	case TEE_MAIN_ALGO_DSA:
+		if (!crypto_ops.acipher.dsa_verify) {
+			res = TEE_ERROR_NOT_IMPLEMENTED;
+			break;
+		}
+		res = crypto_ops.acipher.dsa_verify(cs->algo, o->attr, data,
+						    data_len, sig, sig_len);
+		break;
+
+	case TEE_MAIN_ALGO_ECDSA:
+		if (!crypto_ops.acipher.ecc_verify) {
+			res = TEE_ERROR_NOT_IMPLEMENTED;
+			break;
+		}
+		res = crypto_ops.acipher.ecc_verify(cs->algo, o->attr, data,
+						    data_len, sig, sig_len);
+		break;
+
+	default:
+		res = TEE_ERROR_NOT_SUPPORTED;
+	}
+
+out:
+	free(params);
+	return res;
+}
diff --git a/core/tee/tee_svc_storage.c b/core/tee/tee_svc_storage.c
new file mode 100644
index 0000000..916ddca
--- /dev/null
+++ b/core/tee/tee_svc_storage.c
@@ -0,0 +1,1208 @@
+/*
+ * Copyright (c) 2014, STMicroelectronics International N.V.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <kernel/mutex.h>
+#include <kernel/tee_misc.h>
+#include <kernel/tee_ta_manager.h>
+#include <mm/tee_mmu.h>
+#include <string.h>
+#include <tee_api_defines_extensions.h>
+#include <tee_api_defines.h>
+#include <tee/tee_fs_defs.h>
+#include <tee/tee_fs.h>
+#include <tee/tee_obj.h>
+#include <tee/tee_pobj.h>
+#include <tee/tee_svc_cryp.h>
+#include <tee/tee_svc.h>
+#include <tee/tee_svc_storage.h>
+#include <trace.h>
+
+/*
+ * Returns the appropriate tee_file_operations for the specified storage ID.
+ * The value TEE_STORAGE_PRIVATE will select the REE FS if available, otherwise
+ * RPMB.
+ */
+static const struct tee_file_operations *file_ops(uint32_t storage_id)
+{
+
+	switch (storage_id) {
+	case TEE_STORAGE_PRIVATE:
+#if defined(CFG_REE_FS)
+		return &ree_fs_ops;
+#elif defined(CFG_RPMB_FS)
+		return &rpmb_fs_ops;
+#elif defined(CFG_SQL_FS)
+		return &sql_fs_ops;
+#else
+#error At least one filesystem must be enabled.
+#endif
+#ifdef CFG_REE_FS
+	case TEE_STORAGE_PRIVATE_REE:
+		return &ree_fs_ops;
+#endif
+#ifdef CFG_RPMB_FS
+	case TEE_STORAGE_PRIVATE_RPMB:
+		return &rpmb_fs_ops;
+#endif
+#ifdef CFG_SQL_FS
+	case TEE_STORAGE_PRIVATE_SQL:
+		return &sql_fs_ops;
+#endif
+	default:
+		return NULL;
+	}
+}
+
+/* SSF (Secure Storage File version 00 */
+#define TEE_SVC_STORAGE_MAGIC 0x53534600
+
+/* Header of GP formated secure storage files */
+struct tee_svc_storage_head {
+	uint32_t magic;
+	uint32_t head_size;
+	uint32_t meta_size;
+	uint32_t ds_size;
+	uint32_t keySize;
+	uint32_t maxKeySize;
+	uint32_t objectUsage;
+	uint32_t objectType;
+	uint32_t have_attrs;
+};
+
+struct tee_storage_enum {
+	TAILQ_ENTRY(tee_storage_enum) link;
+	struct tee_fs_dir *dir;
+	const struct tee_file_operations *fops;
+};
+
+/*
+ * Protect TA storage directory: avoid race conditions between (create
+ * directory + create file) and (remove directory)
+ */
+static struct mutex ta_dir_mutex = MUTEX_INITIALIZER;
+
+static TEE_Result tee_svc_storage_get_enum(struct user_ta_ctx *utc,
+					   uint32_t enum_id,
+					   struct tee_storage_enum **e_out)
+{
+	struct tee_storage_enum *e;
+
+	TAILQ_FOREACH(e, &utc->storage_enums, link) {
+		if (enum_id == (vaddr_t)e) {
+			*e_out = e;
+			return TEE_SUCCESS;
+		}
+	}
+	return TEE_ERROR_BAD_PARAMETERS;
+}
+
+static TEE_Result tee_svc_close_enum(struct user_ta_ctx *utc,
+				     struct tee_storage_enum *e)
+{
+	if (e == NULL || utc == NULL)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	TAILQ_REMOVE(&utc->storage_enums, e, link);
+
+	if (e->fops)
+		e->fops->closedir(e->dir);
+
+	e->dir = NULL;
+	e->fops = NULL;
+
+	free(e);
+
+	return TEE_SUCCESS;
+}
+
+/* "/TA_uuid/object_id" or "/TA_uuid/.object_id" */
+char *tee_svc_storage_create_filename(struct tee_ta_session *sess,
+				      void *object_id,
+				      uint32_t object_id_len,
+				      bool transient)
+{
+	uint8_t *file;
+	uint32_t pos = 0;
+	uint32_t hslen = 1 /* Leading slash */
+			+ TEE_B2HS_HSBUF_SIZE(sizeof(TEE_UUID) + object_id_len)
+			+ 1; /* Intermediate slash */
+
+	/* +1 for the '.' (temporary persistent object) */
+	if (transient)
+		hslen++;
+
+	file = malloc(hslen);
+	if (!file)
+		return NULL;
+
+	file[pos++] = '/';
+	pos += tee_b2hs((uint8_t *)&sess->ctx->uuid, &file[pos],
+			sizeof(TEE_UUID), hslen);
+	file[pos++] = '/';
+
+	if (transient)
+		file[pos++] = '.';
+
+	tee_b2hs(object_id, file + pos, object_id_len, hslen - pos);
+
+	return (char *)file;
+}
+
+/* "/TA_uuid" */
+char *tee_svc_storage_create_dirname(struct tee_ta_session *sess)
+{
+	uint8_t *dir;
+	uint32_t hslen = TEE_B2HS_HSBUF_SIZE(sizeof(TEE_UUID)) + 1;
+
+	dir = malloc(hslen);
+	if (!dir)
+		return NULL;
+
+	dir[0] = '/';
+	tee_b2hs((uint8_t *)&sess->ctx->uuid, dir + 1, sizeof(TEE_UUID),
+		 hslen);
+
+	return (char *)dir;
+}
+
+static TEE_Result tee_svc_storage_remove_corrupt_obj(
+					struct tee_ta_session *sess,
+					struct tee_obj *o)
+{
+	TEE_Result res;
+	char *file = NULL;
+	const struct tee_file_operations *fops = o->pobj->fops;
+
+	file = tee_svc_storage_create_filename(sess,
+					       o->pobj->obj_id,
+					       o->pobj->obj_id_len,
+					       false);
+	if (file == NULL) {
+		res = TEE_ERROR_OUT_OF_MEMORY;
+		goto exit;
+	}
+
+	tee_obj_close(to_user_ta_ctx(sess->ctx), o);
+	fops->remove(file);
+	free(file);
+
+	res = TEE_SUCCESS;
+
+exit:
+	return res;
+}
+
+static TEE_Result tee_svc_storage_read_head(struct tee_ta_session *sess,
+				     struct tee_obj *o)
+{
+	TEE_Result res = TEE_SUCCESS;
+	size_t bytes;
+	struct tee_svc_storage_head head;
+	char *file = NULL;
+	const struct tee_file_operations *fops;
+	void *attr = NULL;
+
+	if (o == NULL || o->pobj == NULL)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	fops = o->pobj->fops;
+
+	file = tee_svc_storage_create_filename(sess,
+					       o->pobj->obj_id,
+					       o->pobj->obj_id_len,
+					       false);
+	if (file == NULL) {
+		res = TEE_ERROR_OUT_OF_MEMORY;
+		goto exit;
+	}
+
+	assert(!o->fh);
+	res = fops->open(file, &o->fh);
+	if (res != TEE_SUCCESS)
+		goto exit;
+
+	/* read head */
+	bytes = sizeof(struct tee_svc_storage_head);
+	res = fops->read(o->fh, &head, &bytes);
+	if (res != TEE_SUCCESS) {
+		if (res == TEE_ERROR_CORRUPT_OBJECT)
+			EMSG("Head corrupt\n");
+		goto exit;
+	}
+
+	if (bytes != sizeof(struct tee_svc_storage_head)) {
+		res = TEE_ERROR_BAD_FORMAT;
+		goto exit;
+	}
+
+	res = tee_obj_set_type(o, head.objectType, head.maxKeySize);
+	if (res != TEE_SUCCESS)
+		goto exit;
+
+	if (head.meta_size) {
+		attr = malloc(head.meta_size);
+		if (!attr) {
+			res = TEE_ERROR_OUT_OF_MEMORY;
+			goto exit;
+		}
+
+		/* read meta */
+		bytes = head.meta_size;
+		res = fops->read(o->fh, attr, &bytes);
+		if (res != TEE_SUCCESS || bytes != head.meta_size) {
+			res = TEE_ERROR_CORRUPT_OBJECT;
+			goto exit;
+		}
+	}
+
+	res = tee_obj_attr_from_binary(o, attr, head.meta_size);
+	if (res != TEE_SUCCESS)
+		goto exit;
+
+	o->info.dataSize = head.ds_size;
+	o->info.keySize = head.keySize;
+	o->info.objectUsage = head.objectUsage;
+	o->info.objectType = head.objectType;
+	o->have_attrs = head.have_attrs;
+
+exit:
+	free(attr);
+	free(file);
+
+	return res;
+}
+
+static TEE_Result tee_svc_storage_update_head(struct tee_obj *o,
+					uint32_t ds_size)
+{
+	TEE_Result res;
+	const struct tee_file_operations *fops;
+	int32_t old_off;
+
+	fops = o->pobj->fops;
+
+	/* save original offset */
+	res = fops->seek(o->fh, 0, TEE_DATA_SEEK_CUR, &old_off);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	/* update head.ds_size */
+	res = fops->seek(o->fh, offsetof(struct tee_svc_storage_head,
+			ds_size), TEE_DATA_SEEK_SET, NULL);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = fops->write(o->fh, &ds_size, sizeof(uint32_t));
+	if (res != TEE_SUCCESS)
+		return res;
+
+	/* restore original offset */
+	res = fops->seek(o->fh, old_off, TEE_DATA_SEEK_SET, NULL);
+	return res;
+}
+
+static TEE_Result tee_svc_storage_init_file(struct tee_ta_session *sess,
+					    struct tee_obj *o,
+					    struct tee_obj *attr_o, void *data,
+					    uint32_t len)
+{
+	TEE_Result res = TEE_SUCCESS;
+	struct tee_svc_storage_head head;
+	char *tmpfile = NULL;
+	const struct tee_file_operations *fops;
+	void *attr = NULL;
+	size_t attr_size = 0;
+
+	if (o == NULL || o->pobj == NULL)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	fops = o->pobj->fops;
+
+	/* create temporary persistent object filename */
+	tmpfile = tee_svc_storage_create_filename(sess,
+						   o->pobj->obj_id,
+						   o->pobj->obj_id_len,
+						   true);
+
+	if (tmpfile == NULL) {
+		res = TEE_ERROR_OUT_OF_MEMORY;
+		goto exit;
+	}
+
+	mutex_lock(&ta_dir_mutex);
+	res = fops->create(tmpfile, &o->fh);
+	mutex_unlock(&ta_dir_mutex);
+	if (res != TEE_SUCCESS)
+		goto exit;
+
+	if (attr_o) {
+		res = tee_obj_set_type(o, attr_o->info.objectType,
+				       attr_o->info.maxKeySize);
+		if (res != TEE_SUCCESS)
+			goto exit;
+		res = tee_obj_attr_copy_from(o, attr_o);
+		if (res != TEE_SUCCESS)
+			goto exit;
+		o->have_attrs = attr_o->have_attrs;
+		o->info.objectUsage = attr_o->info.objectUsage;
+		o->info.keySize = attr_o->info.keySize;
+		res = tee_obj_attr_to_binary(o, NULL, &attr_size);
+		if (res != TEE_SUCCESS)
+			goto exit;
+		if (attr_size) {
+			attr = malloc(attr_size);
+			if (!attr) {
+				res = TEE_ERROR_OUT_OF_MEMORY;
+				goto exit;
+			}
+			res = tee_obj_attr_to_binary(o, attr, &attr_size);
+			if (res != TEE_SUCCESS)
+				goto exit;
+		}
+	} else {
+		res = tee_obj_set_type(o, TEE_TYPE_DATA, 0);
+		if (res != TEE_SUCCESS)
+			goto exit;
+	}
+
+	/* write head */
+	head.magic = TEE_SVC_STORAGE_MAGIC;
+	head.head_size = sizeof(struct tee_svc_storage_head);
+	head.meta_size = attr_size;
+	head.ds_size = len;
+	head.keySize = o->info.keySize;
+	head.maxKeySize = o->info.maxKeySize;
+	head.objectUsage = o->info.objectUsage;
+	head.objectType = o->info.objectType;
+	head.have_attrs = o->have_attrs;
+
+	/* write head */
+	res = fops->write(o->fh, &head, sizeof(struct tee_svc_storage_head));
+	if (res != TEE_SUCCESS)
+		goto exit;
+
+	/* write meta */
+	res = fops->write(o->fh, attr, attr_size);
+	if (res != TEE_SUCCESS)
+		goto exit;
+
+	/* write init data */
+	o->info.dataSize = len;
+
+	/* write data to fs if needed */
+	if (data && len)
+		res = fops->write(o->fh, data, len);
+
+exit:
+	free(attr);
+	free(tmpfile);
+	fops->close(&o->fh);
+
+	return res;
+}
+
+TEE_Result syscall_storage_obj_open(unsigned long storage_id, void *object_id,
+			size_t object_id_len, unsigned long flags,
+			uint32_t *obj)
+{
+	TEE_Result res;
+	struct tee_ta_session *sess;
+	struct tee_obj *o = NULL;
+	char *file = NULL;
+	struct tee_pobj *po = NULL;
+	struct user_ta_ctx *utc;
+	const struct tee_file_operations *fops = file_ops(storage_id);
+	size_t attr_size;
+
+	if (!fops) {
+		res = TEE_ERROR_ITEM_NOT_FOUND;
+		goto exit;
+	}
+
+	if (object_id_len > TEE_OBJECT_ID_MAX_LEN) {
+		res = TEE_ERROR_BAD_PARAMETERS;
+		goto exit;
+	}
+
+	res = tee_ta_get_current_session(&sess);
+	if (res != TEE_SUCCESS)
+		goto err;
+	utc = to_user_ta_ctx(sess->ctx);
+
+	res = tee_mmu_check_access_rights(utc,
+					  TEE_MEMORY_ACCESS_READ |
+					  TEE_MEMORY_ACCESS_ANY_OWNER,
+					  (uaddr_t) object_id,
+					  object_id_len);
+	if (res != TEE_SUCCESS)
+		goto err;
+
+	res = tee_pobj_get((void *)&sess->ctx->uuid, object_id,
+			   object_id_len, flags, fops, &po);
+	if (res != TEE_SUCCESS)
+		goto err;
+
+	o = tee_obj_alloc();
+	if (o == NULL) {
+		tee_pobj_release(po);
+		res = TEE_ERROR_OUT_OF_MEMORY;
+		goto err;
+	}
+
+	o->info.handleFlags =
+	    TEE_HANDLE_FLAG_PERSISTENT | TEE_HANDLE_FLAG_INITIALIZED;
+	o->flags = flags;
+	o->pobj = po;
+	tee_obj_add(utc, o);
+
+	res = tee_svc_storage_read_head(sess, o);
+	if (res != TEE_SUCCESS) {
+		if (res == TEE_ERROR_CORRUPT_OBJECT) {
+			EMSG("Object corrupt");
+			goto err;
+		}
+		goto oclose;
+	}
+
+	res = tee_svc_copy_kaddr_to_uref(obj, o);
+	if (res != TEE_SUCCESS)
+		goto oclose;
+
+	res = tee_obj_attr_to_binary(o, NULL, &attr_size);
+	if (res != TEE_SUCCESS)
+		goto oclose;
+
+	res = fops->seek(o->fh, sizeof(struct tee_svc_storage_head) + attr_size,
+			 TEE_DATA_SEEK_SET, NULL);
+	if (res  != TEE_SUCCESS)
+		goto err;
+
+	goto exit;
+
+oclose:
+	tee_obj_close(utc, o);
+	o = NULL;
+
+err:
+	if (res == TEE_ERROR_NO_DATA || res == TEE_ERROR_BAD_FORMAT)
+		res = TEE_ERROR_CORRUPT_OBJECT;
+	if (res == TEE_ERROR_CORRUPT_OBJECT && o)
+		tee_svc_storage_remove_corrupt_obj(sess, o);
+
+exit:
+	free(file);
+	file = NULL;
+	return res;
+}
+
+TEE_Result syscall_storage_obj_create(unsigned long storage_id, void *object_id,
+			size_t object_id_len, unsigned long flags,
+			unsigned long attr, void *data, size_t len,
+			uint32_t *obj)
+{
+	TEE_Result res;
+	struct tee_ta_session *sess;
+	struct tee_obj *o = NULL;
+	struct tee_obj *attr_o = NULL;
+	char *file = NULL;
+	struct tee_pobj *po = NULL;
+	char *tmpfile = NULL;
+	struct user_ta_ctx *utc;
+	const struct tee_file_operations *fops = file_ops(storage_id);
+	size_t attr_size;
+
+	if (!fops)
+		return TEE_ERROR_ITEM_NOT_FOUND;
+
+	if (object_id_len > TEE_OBJECT_ID_MAX_LEN)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	res = tee_ta_get_current_session(&sess);
+	if (res != TEE_SUCCESS)
+		return res;
+	utc = to_user_ta_ctx(sess->ctx);
+
+	res = tee_mmu_check_access_rights(utc,
+					  TEE_MEMORY_ACCESS_READ |
+					  TEE_MEMORY_ACCESS_ANY_OWNER,
+					  (uaddr_t) object_id,
+					  object_id_len);
+	if (res != TEE_SUCCESS)
+		goto err;
+
+	res = tee_pobj_get((void *)&sess->ctx->uuid, object_id,
+			   object_id_len, flags, fops, &po);
+	if (res != TEE_SUCCESS)
+		goto err;
+
+	/* check rights of the provided buffer */
+	if (data && len) {
+		res = tee_mmu_check_access_rights(utc,
+						  TEE_MEMORY_ACCESS_READ |
+						  TEE_MEMORY_ACCESS_ANY_OWNER,
+						  (uaddr_t) data, len);
+
+		if (res != TEE_SUCCESS)
+			goto err;
+	}
+
+	o = tee_obj_alloc();
+	if (o == NULL) {
+		res = TEE_ERROR_OUT_OF_MEMORY;
+		goto err;
+	}
+
+	o->info.handleFlags =
+	    TEE_HANDLE_FLAG_PERSISTENT | TEE_HANDLE_FLAG_INITIALIZED;
+	o->flags = flags;
+	o->pobj = po;
+
+	if (attr != TEE_HANDLE_NULL) {
+		res = tee_obj_get(utc, tee_svc_uref_to_vaddr(attr),
+				  &attr_o);
+		if (res != TEE_SUCCESS)
+			goto err;
+	}
+
+	res = tee_svc_storage_init_file(sess, o, attr_o, data, len);
+	if (res != TEE_SUCCESS)
+		goto err;
+
+	/* create persistent object filename */
+	file = tee_svc_storage_create_filename(sess, object_id,
+					       object_id_len, false);
+	if (file == NULL) {
+		res = TEE_ERROR_OUT_OF_MEMORY;
+		goto err;
+	}
+
+	/* create temporary persistent object filename */
+	tmpfile = tee_svc_storage_create_filename(sess, object_id,
+						  object_id_len,
+						  true);
+	if (tmpfile == NULL) {
+		res = TEE_ERROR_OUT_OF_MEMORY;
+		goto err;
+	}
+
+	/* rename temporary persistent object filename */
+	res = fops->rename(tmpfile, file, !!(flags & TEE_DATA_FLAG_OVERWRITE));
+	if (res != TEE_SUCCESS)
+		goto rmfile;
+
+	res = fops->open(file, &o->fh);
+	if (res != TEE_SUCCESS)
+		goto err;
+
+	tee_obj_add(utc, o);
+
+	res = tee_svc_copy_kaddr_to_uref(obj, o);
+	if (res != TEE_SUCCESS)
+		goto oclose;
+
+	res = tee_obj_attr_to_binary(o, NULL, &attr_size);
+	if (res != TEE_SUCCESS)
+		goto oclose;
+
+	res = fops->seek(o->fh, sizeof(struct tee_svc_storage_head) + attr_size,
+			 TEE_DATA_SEEK_SET, NULL);
+	if (res != TEE_SUCCESS)
+		goto oclose;
+
+	goto exit;
+
+oclose:
+	tee_obj_close(utc, o);
+	goto exit;
+
+rmfile:
+	fops->remove(tmpfile);
+
+err:
+	if (res == TEE_ERROR_NO_DATA || res == TEE_ERROR_BAD_FORMAT)
+		res = TEE_ERROR_CORRUPT_OBJECT;
+	if (res == TEE_ERROR_CORRUPT_OBJECT && file)
+		fops->remove(file);
+	if (o)
+		fops->close(&o->fh);
+	if (po)
+		tee_pobj_release(po);
+	free(o);
+
+exit:
+	free(file);
+	free(tmpfile);
+
+	return res;
+}
+
+TEE_Result syscall_storage_obj_del(unsigned long obj)
+{
+	TEE_Result res;
+	struct tee_ta_session *sess;
+	struct tee_obj *o;
+	char *file;
+	struct user_ta_ctx *utc;
+	const struct tee_file_operations *fops;
+
+	res = tee_ta_get_current_session(&sess);
+	if (res != TEE_SUCCESS)
+		return res;
+	utc = to_user_ta_ctx(sess->ctx);
+
+	res = tee_obj_get(utc, tee_svc_uref_to_vaddr(obj), &o);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	if (!(o->flags & TEE_DATA_FLAG_ACCESS_WRITE_META))
+		return TEE_ERROR_ACCESS_CONFLICT;
+
+	if (o->pobj == NULL || o->pobj->obj_id == NULL)
+		return TEE_ERROR_BAD_STATE;
+
+	file = tee_svc_storage_create_filename(sess, o->pobj->obj_id,
+						o->pobj->obj_id_len, false);
+	if (file == NULL)
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	fops = o->pobj->fops;
+	tee_obj_close(utc, o);
+
+	res = fops->remove(file);
+	free(file);
+	return res;
+}
+
+TEE_Result syscall_storage_obj_rename(unsigned long obj, void *object_id,
+			size_t object_id_len)
+{
+	TEE_Result res;
+	struct tee_ta_session *sess;
+	struct tee_obj *o;
+	struct tee_pobj *po = NULL;
+	char *new_file = NULL;
+	char *old_file = NULL;
+	struct user_ta_ctx *utc;
+	const struct tee_file_operations *fops;
+
+	if (object_id_len > TEE_OBJECT_ID_MAX_LEN)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	res = tee_ta_get_current_session(&sess);
+	if (res != TEE_SUCCESS)
+		return res;
+	utc = to_user_ta_ctx(sess->ctx);
+
+	res = tee_obj_get(utc, tee_svc_uref_to_vaddr(obj), &o);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	if (!(o->info.handleFlags & TEE_HANDLE_FLAG_PERSISTENT)) {
+		res = TEE_ERROR_BAD_STATE;
+		goto exit;
+	}
+
+	if (!(o->flags & TEE_DATA_FLAG_ACCESS_WRITE_META)) {
+		res = TEE_ERROR_BAD_STATE;
+		goto exit;
+	}
+
+	if (o->pobj == NULL || o->pobj->obj_id == NULL) {
+		res = TEE_ERROR_BAD_STATE;
+		goto exit;
+	}
+
+	res = tee_mmu_check_access_rights(utc,
+					TEE_MEMORY_ACCESS_READ |
+					TEE_MEMORY_ACCESS_ANY_OWNER,
+					(uaddr_t) object_id, object_id_len);
+	if (res != TEE_SUCCESS)
+		goto exit;
+
+	/* get new ds name */
+	new_file = tee_svc_storage_create_filename(sess, object_id,
+						   object_id_len, false);
+	if (new_file == NULL) {
+		res = TEE_ERROR_OUT_OF_MEMORY;
+		goto exit;
+	}
+
+	old_file = tee_svc_storage_create_filename(sess, o->pobj->obj_id,
+						   o->pobj->obj_id_len, false);
+	if (old_file == NULL) {
+		res = TEE_ERROR_OUT_OF_MEMORY;
+		goto exit;
+	}
+
+	/* reserve dest name */
+	fops = o->pobj->fops;
+	res = tee_pobj_get((void *)&sess->ctx->uuid, object_id,
+			   object_id_len, TEE_DATA_FLAG_ACCESS_WRITE_META,
+			   fops, &po);
+	if (res != TEE_SUCCESS)
+		goto exit;
+
+	/* move */
+	res = fops->rename(old_file, new_file, false /* no overwrite */);
+	if (res == TEE_ERROR_GENERIC)
+		goto exit;
+
+	res = tee_pobj_rename(o->pobj, object_id, object_id_len);
+
+exit:
+	tee_pobj_release(po);
+
+	free(new_file);
+	free(old_file);
+
+	return res;
+}
+
+TEE_Result syscall_storage_alloc_enum(uint32_t *obj_enum)
+{
+	struct tee_storage_enum *e;
+	struct tee_ta_session *sess;
+	TEE_Result res;
+	struct user_ta_ctx *utc;
+
+	if (obj_enum == NULL)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	res = tee_ta_get_current_session(&sess);
+	if (res != TEE_SUCCESS)
+		return res;
+	utc = to_user_ta_ctx(sess->ctx);
+
+	e = malloc(sizeof(struct tee_storage_enum));
+	if (e == NULL)
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	e->dir = NULL;
+	e->fops = NULL;
+	TAILQ_INSERT_TAIL(&utc->storage_enums, e, link);
+
+	return tee_svc_copy_kaddr_to_uref(obj_enum, e);
+}
+
+TEE_Result syscall_storage_free_enum(unsigned long obj_enum)
+{
+	struct tee_storage_enum *e;
+	TEE_Result res;
+	struct tee_ta_session *sess;
+	struct user_ta_ctx *utc;
+
+	res = tee_ta_get_current_session(&sess);
+	if (res != TEE_SUCCESS)
+		return res;
+	utc = to_user_ta_ctx(sess->ctx);
+
+	res = tee_svc_storage_get_enum(utc,
+			tee_svc_uref_to_vaddr(obj_enum), &e);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	return tee_svc_close_enum(utc, e);
+}
+
+TEE_Result syscall_storage_reset_enum(unsigned long obj_enum)
+{
+	struct tee_storage_enum *e;
+	TEE_Result res;
+	struct tee_ta_session *sess;
+
+	res = tee_ta_get_current_session(&sess);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = tee_svc_storage_get_enum(to_user_ta_ctx(sess->ctx),
+			tee_svc_uref_to_vaddr(obj_enum), &e);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	e->fops->closedir(e->dir);
+	e->fops = NULL;
+	e->dir = NULL;
+
+	return TEE_SUCCESS;
+}
+
+static TEE_Result tee_svc_storage_set_enum(char *d_name,
+			const struct tee_file_operations *fops,
+			struct tee_obj *o)
+{
+	TEE_Result res;
+	uint32_t blen;
+	uint32_t hslen;
+
+	o->info.handleFlags =
+	    TEE_HANDLE_FLAG_PERSISTENT | TEE_HANDLE_FLAG_INITIALIZED;
+	o->info.objectUsage = TEE_USAGE_DEFAULT;
+
+	hslen = strlen(d_name);
+	blen = TEE_HS2B_BBUF_SIZE(hslen);
+	o->pobj->obj_id = malloc(blen);
+	if (!o->pobj->obj_id) {
+		res = TEE_ERROR_OUT_OF_MEMORY;
+		goto exit;
+	}
+	tee_hs2b((uint8_t *)d_name, o->pobj->obj_id, hslen, blen);
+	o->pobj->obj_id_len = blen;
+	o->pobj->fops = fops;
+
+	res = TEE_SUCCESS;
+
+exit:
+	return res;
+
+}
+
+TEE_Result syscall_storage_start_enum(unsigned long obj_enum,
+				      unsigned long storage_id)
+{
+	struct tee_storage_enum *e;
+	char *dir;
+	TEE_Result res;
+	struct tee_ta_session *sess;
+	const struct tee_file_operations *fops = file_ops(storage_id);
+
+	res = tee_ta_get_current_session(&sess);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = tee_svc_storage_get_enum(to_user_ta_ctx(sess->ctx),
+			tee_svc_uref_to_vaddr(obj_enum), &e);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	if (!fops)
+		return TEE_ERROR_ITEM_NOT_FOUND;
+
+	e->fops = fops;
+	dir = tee_svc_storage_create_dirname(sess);
+	if (dir == NULL) {
+		res = TEE_ERROR_OUT_OF_MEMORY;
+		goto exit;
+	}
+
+	assert(!e->dir);
+	res = fops->opendir(dir, &e->dir);
+	free(dir);
+exit:
+	return res;
+}
+
+TEE_Result syscall_storage_next_enum(unsigned long obj_enum,
+			TEE_ObjectInfo *info, void *obj_id, uint64_t *len)
+{
+	struct tee_storage_enum *e;
+	struct tee_fs_dirent *d;
+	TEE_Result res = TEE_SUCCESS;
+	struct tee_ta_session *sess;
+	struct tee_obj *o = NULL;
+	uint64_t l;
+	struct user_ta_ctx *utc;
+
+	res = tee_ta_get_current_session(&sess);
+	if (res != TEE_SUCCESS)
+		goto exit;
+	utc = to_user_ta_ctx(sess->ctx);
+
+	res = tee_svc_storage_get_enum(utc,
+			tee_svc_uref_to_vaddr(obj_enum), &e);
+	if (res != TEE_SUCCESS)
+		goto exit;
+
+	/* check rights of the provided buffers */
+	res = tee_mmu_check_access_rights(utc,
+					TEE_MEMORY_ACCESS_WRITE |
+					TEE_MEMORY_ACCESS_ANY_OWNER,
+					(uaddr_t) info,
+					sizeof(TEE_ObjectInfo));
+	if (res != TEE_SUCCESS)
+		goto exit;
+
+	res = tee_mmu_check_access_rights(utc,
+					TEE_MEMORY_ACCESS_WRITE |
+					TEE_MEMORY_ACCESS_ANY_OWNER,
+					(uaddr_t) obj_id,
+					TEE_OBJECT_ID_MAX_LEN);
+	if (res != TEE_SUCCESS)
+		goto exit;
+
+	if (!e->fops) {
+		res = TEE_ERROR_ITEM_NOT_FOUND;
+		goto exit;
+	}
+
+	res = e->fops->readdir(e->dir, &d);
+	if (res != TEE_SUCCESS)
+		goto exit;
+
+	o = tee_obj_alloc();
+	if (o == NULL) {
+		res = TEE_ERROR_OUT_OF_MEMORY;
+		goto exit;
+	}
+	o->flags = TEE_DATA_FLAG_SHARE_READ;
+
+	o->pobj = calloc(1, sizeof(struct tee_pobj));
+	if (!o->pobj) {
+		res = TEE_ERROR_OUT_OF_MEMORY;
+		goto exit;
+	}
+
+	res = tee_svc_storage_set_enum(d->d_name, e->fops, o);
+	if (res != TEE_SUCCESS)
+		goto exit;
+
+	res = tee_svc_storage_read_head(sess, o);
+	if (res != TEE_SUCCESS)
+		goto exit;
+
+	memcpy(info, &o->info, sizeof(TEE_ObjectInfo));
+	memcpy(obj_id, o->pobj->obj_id, o->pobj->obj_id_len);
+
+	l = o->pobj->obj_id_len;
+	res = tee_svc_copy_to_user(len, &l, sizeof(*len));
+
+exit:
+	if (o) {
+		if (o->pobj) {
+			o->pobj->fops->close(&o->fh);
+			free(o->pobj->obj_id);
+		}
+		free(o->pobj);
+		tee_obj_free(o);
+	}
+
+	return res;
+}
+
+TEE_Result syscall_storage_obj_read(unsigned long obj, void *data, size_t len,
+			uint64_t *count)
+{
+	TEE_Result res;
+	struct tee_ta_session *sess;
+	struct tee_obj *o;
+	uint64_t u_count;
+	struct user_ta_ctx *utc;
+	size_t bytes;
+
+	res = tee_ta_get_current_session(&sess);
+	if (res != TEE_SUCCESS)
+		goto exit;
+	utc = to_user_ta_ctx(sess->ctx);
+
+	res = tee_obj_get(utc, tee_svc_uref_to_vaddr(obj), &o);
+	if (res != TEE_SUCCESS)
+		goto exit;
+
+	if (!(o->info.handleFlags & TEE_HANDLE_FLAG_PERSISTENT)) {
+		res = TEE_ERROR_BAD_STATE;
+		goto exit;
+	}
+
+	if (!(o->flags & TEE_DATA_FLAG_ACCESS_READ)) {
+		res = TEE_ERROR_ACCESS_CONFLICT;
+		goto exit;
+	}
+
+	/* check rights of the provided buffer */
+	res = tee_mmu_check_access_rights(utc,
+					TEE_MEMORY_ACCESS_WRITE |
+					TEE_MEMORY_ACCESS_ANY_OWNER,
+					(uaddr_t) data, len);
+	if (res != TEE_SUCCESS)
+		goto exit;
+
+	bytes = len;
+	res = o->pobj->fops->read(o->fh, data, &bytes);
+	if (res != TEE_SUCCESS) {
+		EMSG("Error code=%x\n", (uint32_t)res);
+		if (res == TEE_ERROR_CORRUPT_OBJECT) {
+			EMSG("Object corrupt\n");
+			tee_svc_storage_remove_corrupt_obj(sess, o);
+		}
+		goto exit;
+	}
+
+	o->info.dataPosition += bytes;
+
+	u_count = bytes;
+	res = tee_svc_copy_to_user(count, &u_count, sizeof(*count));
+exit:
+	return res;
+}
+
+TEE_Result syscall_storage_obj_write(unsigned long obj, void *data, size_t len)
+{
+	TEE_Result res;
+	struct tee_ta_session *sess;
+	struct tee_obj *o;
+	struct user_ta_ctx *utc;
+
+	res = tee_ta_get_current_session(&sess);
+	if (res != TEE_SUCCESS)
+		goto exit;
+	utc = to_user_ta_ctx(sess->ctx);
+
+	res = tee_obj_get(utc, tee_svc_uref_to_vaddr(obj), &o);
+	if (res != TEE_SUCCESS)
+		goto exit;
+
+	if (!(o->info.handleFlags & TEE_HANDLE_FLAG_PERSISTENT)) {
+		res = TEE_ERROR_BAD_STATE;
+		goto exit;
+	}
+
+	if (!(o->flags & TEE_DATA_FLAG_ACCESS_WRITE)) {
+		res = TEE_ERROR_ACCESS_CONFLICT;
+		goto exit;
+	}
+
+	/* check rights of the provided buffer */
+	res = tee_mmu_check_access_rights(utc,
+					TEE_MEMORY_ACCESS_READ |
+					TEE_MEMORY_ACCESS_ANY_OWNER,
+					(uaddr_t) data, len);
+	if (res != TEE_SUCCESS)
+		goto exit;
+
+	res = o->pobj->fops->write(o->fh, data, len);
+	if (res != TEE_SUCCESS)
+		goto exit;
+
+	o->info.dataPosition += len;
+	if (o->info.dataPosition > o->info.dataSize) {
+		res = tee_svc_storage_update_head(o, o->info.dataPosition);
+		if (res != TEE_SUCCESS)
+			goto exit;
+		o->info.dataSize = o->info.dataPosition;
+	}
+
+exit:
+	return res;
+}
+
+TEE_Result syscall_storage_obj_trunc(unsigned long obj, size_t len)
+{
+	TEE_Result res;
+	struct tee_ta_session *sess;
+	struct tee_obj *o;
+	size_t off;
+	size_t attr_size;
+
+	res = tee_ta_get_current_session(&sess);
+	if (res != TEE_SUCCESS)
+		goto exit;
+
+	res = tee_obj_get(to_user_ta_ctx(sess->ctx),
+			  tee_svc_uref_to_vaddr(obj), &o);
+	if (res != TEE_SUCCESS)
+		goto exit;
+
+	if (!(o->info.handleFlags & TEE_HANDLE_FLAG_PERSISTENT)) {
+		res = TEE_ERROR_BAD_STATE;
+		goto exit;
+	}
+
+	if (!(o->flags & TEE_DATA_FLAG_ACCESS_WRITE)) {
+		res = TEE_ERROR_ACCESS_CONFLICT;
+		goto exit;
+	}
+
+	res = tee_obj_attr_to_binary(o, NULL, &attr_size);
+	if (res != TEE_SUCCESS)
+		goto exit;
+
+	off = sizeof(struct tee_svc_storage_head) + attr_size;
+	res = o->pobj->fops->truncate(o->fh, len + off);
+	if (res != TEE_SUCCESS) {
+		if (res == TEE_ERROR_CORRUPT_OBJECT) {
+			EMSG("Object corrupt\n");
+			res = tee_svc_storage_remove_corrupt_obj(sess, o);
+			if (res != TEE_SUCCESS)
+				goto exit;
+			res = TEE_ERROR_CORRUPT_OBJECT;
+			goto exit;
+		} else
+			res = TEE_ERROR_GENERIC;
+	}
+
+exit:
+	return res;
+}
+
+TEE_Result syscall_storage_obj_seek(unsigned long obj, int32_t offset,
+				    unsigned long whence)
+{
+	TEE_Result res;
+	struct tee_ta_session *sess;
+	struct tee_obj *o;
+	int32_t off;
+	size_t attr_size;
+
+	res = tee_ta_get_current_session(&sess);
+	if (res != TEE_SUCCESS)
+		goto exit;
+
+	res = tee_obj_get(to_user_ta_ctx(sess->ctx),
+			  tee_svc_uref_to_vaddr(obj), &o);
+	if (res != TEE_SUCCESS)
+		goto exit;
+
+	if (!(o->info.handleFlags & TEE_HANDLE_FLAG_PERSISTENT)) {
+		res = TEE_ERROR_BAD_STATE;
+		goto exit;
+	}
+
+	res = tee_obj_attr_to_binary(o, NULL, &attr_size);
+	if (res != TEE_SUCCESS)
+		goto exit;
+
+	off = offset;
+	if (whence == TEE_DATA_SEEK_SET)
+		off += sizeof(struct tee_svc_storage_head) + attr_size;
+
+	res = o->pobj->fops->seek(o->fh, off, whence, &off);
+	if (res != TEE_SUCCESS)
+		goto exit;
+	o->info.dataPosition = off - (sizeof(struct tee_svc_storage_head) +
+				      attr_size);
+
+exit:
+	return res;
+}
+
+void tee_svc_storage_close_all_enum(struct user_ta_ctx *utc)
+{
+	struct tee_storage_enum_head *eh = &utc->storage_enums;
+
+	/* disregard return value */
+	while (!TAILQ_EMPTY(eh))
+		tee_svc_close_enum(utc, TAILQ_FIRST(eh));
+}
diff --git a/core/tee/tee_time_generic.c b/core/tee/tee_time_generic.c
new file mode 100644
index 0000000..0b983b0
--- /dev/null
+++ b/core/tee/tee_time_generic.c
@@ -0,0 +1,159 @@
+/*
+ * Copyright (c) 2014, STMicroelectronics International N.V.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <kernel/panic.h>
+#include <kernel/tee_time.h>
+#include <string.h>
+#include <stdlib.h>
+#include <trace.h>
+#include <utee_defines.h>
+
+struct tee_ta_time_offs {
+	TEE_UUID uuid;
+	TEE_Time offs;
+	bool positive;
+};
+
+static struct tee_ta_time_offs *tee_time_offs;
+static size_t tee_time_num_offs;
+
+static TEE_Result tee_time_ta_get_offs(const TEE_UUID *uuid,
+				       const TEE_Time **offs, bool *positive)
+{
+	size_t n;
+
+	for (n = 0; n < tee_time_num_offs; n++) {
+		if (memcmp(uuid, &tee_time_offs[n].uuid, sizeof(TEE_UUID))
+				== 0) {
+			*offs = &tee_time_offs[n].offs;
+			*positive = tee_time_offs[n].positive;
+			return TEE_SUCCESS;
+		}
+	}
+	return TEE_ERROR_TIME_NOT_SET;
+}
+
+static TEE_Result tee_time_ta_set_offs(const TEE_UUID *uuid,
+				       const TEE_Time *offs, bool positive)
+{
+	size_t n;
+	struct tee_ta_time_offs *o;
+
+	for (n = 0; n < tee_time_num_offs; n++) {
+		if (memcmp(uuid, &tee_time_offs[n].uuid, sizeof(TEE_UUID))
+				== 0) {
+			tee_time_offs[n].offs = *offs;
+			tee_time_offs[n].positive = positive;
+			return TEE_SUCCESS;
+		}
+	}
+
+	n = tee_time_num_offs + 1;
+	o = realloc(tee_time_offs, n * sizeof(struct tee_ta_time_offs));
+	if (!o)
+		return TEE_ERROR_OUT_OF_MEMORY;
+	tee_time_offs = o;
+	tee_time_offs[tee_time_num_offs].uuid = *uuid;
+	tee_time_offs[tee_time_num_offs].offs = *offs;
+	tee_time_offs[tee_time_num_offs].positive = positive;
+	tee_time_num_offs = n;
+	return TEE_SUCCESS;
+}
+
+TEE_Result tee_time_get_ta_time(const TEE_UUID *uuid, TEE_Time *time)
+{
+	TEE_Result res;
+	const TEE_Time *offs;
+	bool positive;
+	TEE_Time t;
+	TEE_Time t2;
+
+	res = tee_time_ta_get_offs(uuid, &offs, &positive);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = tee_time_get_sys_time(&t);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	if (positive) {
+		TEE_TIME_ADD(t, *offs, t2);
+
+		/* Detect wrapping, the wrapped time should be returned. */
+		if (TEE_TIME_LT(t2, t))
+			res = TEE_ERROR_OVERFLOW;
+	} else {
+		TEE_TIME_SUB(t, *offs, t2);
+
+		/* Detect wrapping, the wrapped time should be returned. */
+		if (TEE_TIME_LE(t, t2))
+			res = TEE_ERROR_OVERFLOW;
+	}
+	*time = t2;
+
+	return res;
+}
+
+TEE_Result tee_time_set_ta_time(const TEE_UUID *uuid, const TEE_Time *time)
+{
+	TEE_Result res;
+	TEE_Time offs;
+	TEE_Time t;
+
+	/* Check that time is normalized. */
+	if (time->millis >= TEE_TIME_MILLIS_BASE)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	res = tee_time_get_sys_time(&t);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	if (TEE_TIME_LT(t, *time)) {
+		TEE_TIME_SUB(*time, t, offs);
+		return tee_time_ta_set_offs(uuid, &offs, true);
+	} else {
+		TEE_TIME_SUB(t, *time, offs);
+		return tee_time_ta_set_offs(uuid, &offs, false);
+	}
+}
+
+void tee_time_busy_wait(uint32_t milliseconds_delay)
+{
+	TEE_Time curr;
+	TEE_Time delta;
+	TEE_Time end;
+
+	if (tee_time_get_sys_time(&curr) != TEE_SUCCESS)
+		panic();
+	delta.seconds = milliseconds_delay / 1000;
+	delta.millis = milliseconds_delay % 1000;
+	TEE_TIME_ADD(curr, delta, end);
+
+	while (TEE_TIME_LT(curr, end))
+		if (tee_time_get_sys_time(&curr) != TEE_SUCCESS)
+			panic();
+}
diff --git a/core/tee/uuid.c b/core/tee/uuid.c
new file mode 100644
index 0000000..3642b1c
--- /dev/null
+++ b/core/tee/uuid.c
@@ -0,0 +1,52 @@
+/*
+ * Copyright (c) 2016, Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <string.h>
+#include <tee/uuid.h>
+#include <util.h>
+
+void tee_uuid_to_octets(uint8_t *d, const TEE_UUID *s)
+{
+	d[0] = s->timeLow >> 24;
+	d[1] = s->timeLow >> 16;
+	d[2] = s->timeLow >> 8;
+	d[3] = s->timeLow;
+	d[4] = s->timeMid >> 8;
+	d[5] = s->timeMid;
+	d[6] = s->timeHiAndVersion >> 8;
+	d[7] = s->timeHiAndVersion;
+	memcpy(d + 8, s->clockSeqAndNode, sizeof(s->clockSeqAndNode));
+}
+
+void tee_uuid_from_octets(TEE_UUID *d, const uint8_t *s)
+{
+	d->timeLow = SHIFT_U32(s[0], 24) | SHIFT_U32(s[1], 16) |
+		     SHIFT_U32(s[2], 8) | s[3];
+	d->timeMid = SHIFT_U32(s[4], 8) | s[5];
+	d->timeHiAndVersion = SHIFT_U32(s[6], 8) | s[7];
+	memcpy(d->clockSeqAndNode, s + 8, sizeof(d->clockSeqAndNode));
+}