diff --git a/hwc/hwc.c b/hwc/hwc.c
index 4ac3817..bc54f12 100644
--- a/hwc/hwc.c
+++ b/hwc/hwc.c
@@ -1860,7 +1860,7 @@
     memset(hwc_dev, 0, sizeof(*hwc_dev));
 
     hwc_dev->base.common.tag = HARDWARE_DEVICE_TAG;
-    hwc_dev->base.common.version = HWC_API_VERSION;
+    hwc_dev->base.common.version = HWC_DEVICE_API_VERSION;
     hwc_dev->base.common.module = (hw_module_t *)module;
     hwc_dev->base.common.close = omap4_hwc_device_close;
     hwc_dev->base.prepare = omap4_hwc_prepare;
@@ -2009,8 +2009,8 @@
     .base = {
         .common = {
             .tag =                  HARDWARE_MODULE_TAG,
-            .version_major =        1,
-            .version_minor =        0,
+            .module_api_version =   HWC_MODULE_API_VERSION,
+            .hal_api_version =      HARDWARE_HAL_API_VERSION,
             .id =                   HWC_HARDWARE_MODULE_ID,
             .name =                 "OMAP 44xx Hardware Composer HAL",
             .author =               "Texas Instruments",
diff --git a/omap4.mk b/omap4.mk
index 0fc74f5..9a7324e 100644
--- a/omap4.mk
+++ b/omap4.mk
@@ -33,5 +33,6 @@
 	libomxcameraadapter \
 	hwcomposer.omap4 \
 	smc_pa_ctrl \
-	tf_daemon
+	tf_daemon \
+	libtf_crypto_sst
 
diff --git a/security/tf_crypto_sst/Android.mk b/security/tf_crypto_sst/Android.mk
new file mode 100644
index 0000000..6d3a9d8
--- /dev/null
+++ b/security/tf_crypto_sst/Android.mk
@@ -0,0 +1,32 @@
+ifeq ($(TARGET_BOARD_PLATFORM),omap4)
+
+LOCAL_PATH := $(call my-dir)
+include $(CLEAR_VARS)
+
+LOCAL_PRELINK_MODULE := false
+LOCAL_ARM_MODE := arm
+
+LOCAL_SRC_FILES := \
+	lib_object.c \
+	lib_mutex_linux.c \
+	sst_stub.c \
+	mtc.c \
+	pkcs11_global.c \
+	pkcs11_object.c \
+	pkcs11_session.c
+
+LOCAL_CFLAGS += -DLINUX
+LOCAL_CFLAGS += -D__ANDROID32__
+
+ifdef S_VERSION_BUILD
+LOCAL_CFLAGS += -DS_VERSION_BUILD=$(S_VERSION_BUILD)
+endif
+
+LOCAL_CFLAGS += -I $(LOCAL_PATH)/../tf_sdk/include/
+
+LOCAL_MODULE:= libtf_crypto_sst
+LOCAL_STATIC_LIBRARIES := libtee_client_api_driver
+LOCAL_MODULE_TAGS := optional
+
+include $(BUILD_SHARED_LIBRARY)
+endif
diff --git a/security/tf_crypto_sst/lib_mutex.h b/security/tf_crypto_sst/lib_mutex.h
new file mode 100644
index 0000000..6f4bed1
--- /dev/null
+++ b/security/tf_crypto_sst/lib_mutex.h
@@ -0,0 +1,97 @@
+/**
+ * Copyright(c) 2011 Trusted Logic.   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 Trusted Logic 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.
+ */
+
+#ifndef __LIB_MUTEX_H__
+#define __LIB_MUTEX_H__
+
+/**
+ * This libray defines a type of simple non-recursive mutex that can
+ * be statically initialized. The interface is very similar to pthread
+ * mutexes.
+ **/
+
+#ifdef WIN32
+
+#include <windows.h>
+#include "s_type.h"
+
+/* Windows API: use a critical section with a state describing
+   whether it has been initialized or not or is being initialized.
+   We use interlocked operations on the state to synchronize between
+   multiple threads competing to initialize the critical section */
+typedef struct
+{
+   volatile uint32_t nInitState;
+   CRITICAL_SECTION sCriticalSection;
+}
+LIB_MUTEX;
+
+#define LIB_MUTEX_WIN32_STATE_UNINITIALIZED 0
+#define LIB_MUTEX_WIN32_STATE_INITIALIZING  1
+#define LIB_MUTEX_WIN32_STATE_INITIALIZED   2
+
+#define LIB_MUTEX_INITIALIZER { 0 }
+
+#endif  /* WIN32 */
+
+#ifdef LINUX
+
+#include <pthread.h>
+#include "s_type.h"
+
+/* Linux: directly use a pthread mutex. */
+typedef pthread_mutex_t LIB_MUTEX;
+#define LIB_MUTEX_INITIALIZER PTHREAD_MUTEX_INITIALIZER
+
+#endif /* LINUX */
+
+/**
+ * Initialize a mutex. Note that thie function cannot fail. If there
+ * is not enough resource to initialize the mutex, the implementation
+ * resort to active-wait
+ **/
+void libMutexInit(LIB_MUTEX* pMutex);
+
+/**
+ * Lock the mutex
+ */
+void libMutexLock(LIB_MUTEX* pMutex);
+
+/**
+ * Unlock the mutex
+ */
+void libMutexUnlock(LIB_MUTEX* pMutex);
+
+/**
+ * Destroy the mutex
+ */
+void libMutexDestroy(LIB_MUTEX* pMutex);
+
+#endif /* __LIB_MUTEX_H__ */
diff --git a/security/tf_crypto_sst/lib_mutex_linux.c b/security/tf_crypto_sst/lib_mutex_linux.c
new file mode 100644
index 0000000..da8c9e0
--- /dev/null
+++ b/security/tf_crypto_sst/lib_mutex_linux.c
@@ -0,0 +1,56 @@
+/**
+ * Copyright(c) 2011 Trusted Logic.   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 Trusted Logic 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.
+ */
+
+/**
+ * Implementation of the mutex library over the Linux API
+ **/
+#include <pthread.h>
+#include "lib_mutex.h"
+
+void libMutexInit(LIB_MUTEX* pMutex)
+{
+   pthread_mutex_init(pMutex, NULL);
+   /* Error ignored. Is that OK? */
+}
+
+void libMutexLock(LIB_MUTEX* pMutex)
+{
+   pthread_mutex_lock(pMutex);
+}
+
+void libMutexUnlock(LIB_MUTEX* pMutex)
+{
+   pthread_mutex_unlock(pMutex);
+}
+
+void libMutexDestroy(LIB_MUTEX* pMutex)
+{
+   pthread_mutex_destroy(pMutex);
+}
diff --git a/security/tf_crypto_sst/lib_object.c b/security/tf_crypto_sst/lib_object.c
new file mode 100644
index 0000000..a07a66f
--- /dev/null
+++ b/security/tf_crypto_sst/lib_object.c
@@ -0,0 +1,418 @@
+/**
+ * Copyright(c) 2011 Trusted Logic.   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 Trusted Logic 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.
+ */
+
+/** Simple implementation of lib_object using doubly-linked lists. This
+   implementation should outperform more sophisticated data structures
+   like blanced binary trees when the tables have fewer than 10 to 20
+   elements.  */
+
+#include <string.h>
+#include "s_type.h"
+
+#include "lib_object.h"
+
+/* Generic node */
+typedef struct LIB_OBJECT_NODE
+{
+   struct LIB_OBJECT_NODE* pPrevious;
+   struct LIB_OBJECT_NODE* pNext;
+   union
+   {
+      uint16_t nHandle;
+
+      S_STORAGE_NAME sStorageName;
+
+      struct
+      {
+         /* Public fields */
+         uint8_t  sFilename[64];
+         uint8_t  nFilenameLength;
+      }
+      f;
+   }
+   key;
+}
+LIB_OBJECT_NODE;
+
+typedef enum
+{
+   LIB_OBJECT_NODE_TYPE_HANDLE16,
+   LIB_OBJECT_NODE_TYPE_STORAGE_NAME,
+   LIB_OBJECT_NODE_TYPE_FILENAME,
+   LIB_OBJECT_NODE_TYPE_UNINDEXED
+}
+LIB_OBJECT_NODE_TYPE;
+
+/* -----------------------------------------------------------------------
+   Search functions
+   -----------------------------------------------------------------------*/
+static bool libObjectKeyEqualNode(
+   LIB_OBJECT_NODE* pNode,
+   uint32_t nKey1,
+   void* pKey2,
+   LIB_OBJECT_NODE_TYPE eNodeType)
+{
+   switch (eNodeType)
+   {
+   default:
+   case LIB_OBJECT_NODE_TYPE_HANDLE16:
+      return
+         nKey1 == pNode->key.nHandle;
+   case LIB_OBJECT_NODE_TYPE_STORAGE_NAME:
+      return
+         memcmp(
+            (S_STORAGE_NAME*)pKey2,
+            &pNode->key.sStorageName,
+            sizeof(S_STORAGE_NAME)) == 0;
+   case LIB_OBJECT_NODE_TYPE_FILENAME:
+   {
+      uint32_t nLength1 = nKey1;
+      uint32_t nLength2 = pNode->key.f.nFilenameLength;
+      return
+         nLength1 == nLength2
+         &&
+         memcmp(
+            pKey2,
+            &pNode->key.f.sFilename,
+            nLength1) == 0;
+   }
+   }
+}
+
+/* Polymorphic search function */
+static LIB_OBJECT_NODE* libObjectSearch(
+   LIB_OBJECT_NODE* pRoot,
+   uint32_t nKey1,
+   void* pKey2,
+   LIB_OBJECT_NODE_TYPE eNodeType)
+{
+   if (pRoot != NULL)
+   {
+      LIB_OBJECT_NODE* pNode = pRoot;
+      do
+      {
+         if (libObjectKeyEqualNode(pNode, nKey1, pKey2, eNodeType))
+         {
+            /* Match found */
+            return pNode;
+         }
+         pNode = pNode->pNext;
+      }
+      while (pNode != pRoot);
+   }
+   return NULL;
+}
+
+LIB_OBJECT_NODE_HANDLE16* libObjectHandle16Search(
+               LIB_OBJECT_TABLE_HANDLE16* pTable,
+               uint32_t nHandle)
+{
+   return (LIB_OBJECT_NODE_HANDLE16*)libObjectSearch(
+      (LIB_OBJECT_NODE*)pTable->pRoot, nHandle, NULL, LIB_OBJECT_NODE_TYPE_HANDLE16);
+}
+
+
+LIB_OBJECT_NODE_STORAGE_NAME* libObjectStorageNameSearch(
+               LIB_OBJECT_TABLE_STORAGE_NAME* pTable,
+               S_STORAGE_NAME* pStorageName)
+{
+   return (LIB_OBJECT_NODE_STORAGE_NAME*)libObjectSearch(
+      (LIB_OBJECT_NODE*)pTable->pRoot, 0, pStorageName, LIB_OBJECT_NODE_TYPE_STORAGE_NAME);
+}
+
+LIB_OBJECT_NODE_FILENAME* libObjectFilenameSearch(
+               LIB_OBJECT_TABLE_FILENAME* pTable,
+               uint8_t* pFilename,
+               uint32_t  nFilenameLength)
+{
+   return (LIB_OBJECT_NODE_FILENAME*)libObjectSearch(
+      (LIB_OBJECT_NODE*)pTable->pRoot, nFilenameLength, pFilename, LIB_OBJECT_NODE_TYPE_FILENAME);
+}
+
+/* -----------------------------------------------------------------------
+   Add functions
+   -----------------------------------------------------------------------*/
+
+/* Polymorphic add function. Add the node at the end of the linked list */
+static bool libObjectAdd(
+   LIB_OBJECT_NODE** ppRoot,
+   LIB_OBJECT_NODE* pNew,
+   LIB_OBJECT_NODE_TYPE eNodeType)
+{
+   LIB_OBJECT_NODE* pRoot;
+   LIB_OBJECT_NODE* pLast;
+   if (*ppRoot == NULL)
+   {
+      *ppRoot = pNew;
+      pNew->pNext = pNew;
+      pNew->pPrevious = pNew;
+      if (eNodeType == LIB_OBJECT_NODE_TYPE_HANDLE16)
+      {
+         pNew->key.nHandle = 1;
+      }
+      return true;
+   }
+   else
+   {
+      pRoot = *ppRoot;
+      pLast = pRoot->pPrevious;
+      if (eNodeType == LIB_OBJECT_NODE_TYPE_HANDLE16)
+      {
+         if (pLast->key.nHandle == LIB_OBJECT_HANDLE16_MAX)
+         {
+            /* We cannot just assign last handle + 1 because it will
+               overflow. So, scan the whole list */
+            goto scan_list;
+         }
+         pNew->key.nHandle = pLast->key.nHandle + 1;
+      }
+      pLast->pNext = pNew;
+      pNew->pPrevious = pLast;
+      pNew->pNext = pRoot;
+      pRoot->pPrevious = pNew;
+      return true;
+   }
+scan_list:
+   {
+      LIB_OBJECT_NODE* pNode = pRoot;
+      uint32_t nFreeHandle = 1;
+      do
+      {
+         if (pNode->key.nHandle > nFreeHandle)
+         {
+            /* nMaxHandle+1 is not allocated. Insert pNew just before pNode */
+            pNew->key.nHandle = nFreeHandle;
+            pNew->pNext = pNode;
+            pNew->pPrevious = pNode->pPrevious;
+            pNode->pPrevious->pNext = pNew;
+            pNode->pPrevious = pNew;
+            if (pNode == pRoot)
+            {
+               /* Special case, pNew is the new root */
+               *ppRoot = pNew;
+            }
+            return true;
+         }
+         pNode = pNode->pNext;
+         nFreeHandle++;
+      }
+      while (pNode != pRoot);
+      /* No free handle */
+      return false;
+   }
+}
+
+bool libObjectHandle16Add(
+               LIB_OBJECT_TABLE_HANDLE16* pTable,
+               LIB_OBJECT_NODE_HANDLE16* pObject)
+{
+   return libObjectAdd(
+      (LIB_OBJECT_NODE**)&pTable->pRoot,
+      (LIB_OBJECT_NODE*)pObject,
+      LIB_OBJECT_NODE_TYPE_HANDLE16);
+}
+
+
+void libObjectStorageNameAdd(
+               LIB_OBJECT_TABLE_STORAGE_NAME* pTable,
+               LIB_OBJECT_NODE_STORAGE_NAME* pObject)
+{
+   libObjectAdd(
+      (LIB_OBJECT_NODE**)&pTable->pRoot,
+      (LIB_OBJECT_NODE*)pObject,
+      LIB_OBJECT_NODE_TYPE_STORAGE_NAME);
+}
+
+
+void libObjectFilenameAdd(
+               LIB_OBJECT_TABLE_FILENAME* pTable,
+               LIB_OBJECT_NODE_FILENAME* pObject)
+{
+   libObjectAdd(
+      (LIB_OBJECT_NODE**)&pTable->pRoot,
+      (LIB_OBJECT_NODE*)pObject,
+      LIB_OBJECT_NODE_TYPE_FILENAME);
+}
+
+
+void libObjectUnindexedAdd(
+         LIB_OBJECT_TABLE_UNINDEXED* pTable,
+         LIB_OBJECT_NODE_UNINDEXED* pObject)
+{
+   libObjectAdd(
+      (LIB_OBJECT_NODE**)&pTable->pRoot,
+      (LIB_OBJECT_NODE*)pObject,
+      LIB_OBJECT_NODE_TYPE_UNINDEXED);
+}
+
+
+/* -----------------------------------------------------------------------
+   Remove functions
+   -----------------------------------------------------------------------*/
+static void libObjectRemove(LIB_OBJECT_NODE** ppRoot, LIB_OBJECT_NODE* pObject)
+{
+   LIB_OBJECT_NODE* pPrevious = pObject->pPrevious;
+   LIB_OBJECT_NODE* pNext = pObject->pNext;
+
+   pPrevious->pNext = pNext;
+   pNext->pPrevious = pPrevious;
+
+   if (pPrevious == pObject)
+   {
+      /* Removed the last object in the table */
+      *ppRoot = NULL;
+   }
+   else if (pObject == *ppRoot)
+   {
+      /* Removed the first object in the list */
+      *ppRoot = pNext;
+   }
+}
+
+static LIB_OBJECT_NODE* libObjectRemoveOne(LIB_OBJECT_NODE** ppRoot)
+{
+   if (*ppRoot == NULL)
+   {
+      return NULL;
+   }
+   else
+   {
+      LIB_OBJECT_NODE* pObject = *ppRoot;
+      libObjectRemove(ppRoot, pObject);
+      return pObject;
+   }
+}
+
+void libObjectHandle16Remove(
+               LIB_OBJECT_TABLE_HANDLE16* pTable,
+               LIB_OBJECT_NODE_HANDLE16* pObject)
+{
+   libObjectRemove((LIB_OBJECT_NODE**)&pTable->pRoot, (LIB_OBJECT_NODE*)pObject);
+   pObject->nHandle = 0;
+}
+
+LIB_OBJECT_NODE_HANDLE16* libObjectHandle16RemoveOne(
+               LIB_OBJECT_TABLE_HANDLE16* pTable)
+{
+   LIB_OBJECT_NODE_HANDLE16* pObject = (LIB_OBJECT_NODE_HANDLE16*)libObjectRemoveOne((LIB_OBJECT_NODE**)&pTable->pRoot);
+   if (pObject != NULL)
+   {
+      pObject->nHandle = 0;
+   }
+   return pObject;
+}
+
+void libObjectStorageNameRemove(
+               LIB_OBJECT_TABLE_STORAGE_NAME* pTable,
+               LIB_OBJECT_NODE_STORAGE_NAME* pObject)
+{
+   libObjectRemove((LIB_OBJECT_NODE**)&pTable->pRoot, (LIB_OBJECT_NODE*)pObject);
+}
+
+LIB_OBJECT_NODE_STORAGE_NAME* libObjectStorageNameRemoveOne(
+               LIB_OBJECT_TABLE_STORAGE_NAME* pTable)
+{
+   return (LIB_OBJECT_NODE_STORAGE_NAME*)libObjectRemoveOne((LIB_OBJECT_NODE**)&pTable->pRoot);
+}
+
+void libObjectFilenameRemove(
+               LIB_OBJECT_TABLE_FILENAME* pTable,
+               LIB_OBJECT_NODE_FILENAME* pObject)
+{
+   libObjectRemove((LIB_OBJECT_NODE**)&pTable->pRoot, (LIB_OBJECT_NODE*)pObject);
+}
+
+LIB_OBJECT_NODE_FILENAME* libObjectFilenameRemoveOne(
+               LIB_OBJECT_TABLE_FILENAME* pTable)
+{
+   return (LIB_OBJECT_NODE_FILENAME*)libObjectRemoveOne((LIB_OBJECT_NODE**)&pTable->pRoot);
+}
+
+void libObjectUnindexedRemove(
+         LIB_OBJECT_TABLE_UNINDEXED* pTable,
+         LIB_OBJECT_NODE_UNINDEXED* pObject)
+{
+   libObjectRemove((LIB_OBJECT_NODE**)&pTable->pRoot, (LIB_OBJECT_NODE*)pObject);
+}
+
+LIB_OBJECT_NODE_UNINDEXED* libObjectUnindexedRemoveOne(LIB_OBJECT_TABLE_UNINDEXED* pTable)
+{
+   return (LIB_OBJECT_NODE_UNINDEXED*)libObjectRemoveOne((LIB_OBJECT_NODE**)&pTable->pRoot);
+}
+
+
+/* -----------------------------------------------------------------------
+   Get-next functions
+   -----------------------------------------------------------------------*/
+
+static LIB_OBJECT_NODE* libObjectNext(LIB_OBJECT_NODE* pRoot, LIB_OBJECT_NODE* pObject)
+{
+   if (pObject == NULL)
+   {
+      return pRoot;
+   }
+   else if (pObject == pRoot->pPrevious)
+   {
+      return NULL;
+   }
+   else
+   {
+      return pObject->pNext;
+   }
+}
+
+
+LIB_OBJECT_NODE_HANDLE16* libObjectHandle16Next(
+               LIB_OBJECT_TABLE_HANDLE16* pTable,
+               LIB_OBJECT_NODE_HANDLE16* pObject)
+{
+   return (LIB_OBJECT_NODE_HANDLE16*)libObjectNext((LIB_OBJECT_NODE*)pTable->pRoot, (LIB_OBJECT_NODE*)pObject);
+}
+
+LIB_OBJECT_NODE_STORAGE_NAME* libObjectStorageNameNext(
+               LIB_OBJECT_TABLE_STORAGE_NAME* pTable,
+               LIB_OBJECT_NODE_STORAGE_NAME* pObject)
+{
+   return (LIB_OBJECT_NODE_STORAGE_NAME*)libObjectNext((LIB_OBJECT_NODE*)pTable->pRoot, (LIB_OBJECT_NODE*)pObject);
+}
+
+LIB_OBJECT_NODE_FILENAME* libObjectFilenameNext(
+               LIB_OBJECT_TABLE_FILENAME* pTable,
+               LIB_OBJECT_NODE_FILENAME* pObject)
+{
+   return (LIB_OBJECT_NODE_FILENAME*)libObjectNext((LIB_OBJECT_NODE*)pTable->pRoot, (LIB_OBJECT_NODE*)pObject);
+}
+
+LIB_OBJECT_NODE_UNINDEXED* libObjectUnindexedNext(
+               LIB_OBJECT_TABLE_UNINDEXED* pTable,
+               LIB_OBJECT_NODE_UNINDEXED* pObject)
+{
+   return (LIB_OBJECT_NODE_UNINDEXED*)libObjectNext((LIB_OBJECT_NODE*)pTable->pRoot, (LIB_OBJECT_NODE*)pObject);
+}
diff --git a/security/tf_crypto_sst/lib_object.h b/security/tf_crypto_sst/lib_object.h
new file mode 100644
index 0000000..5abb834
--- /dev/null
+++ b/security/tf_crypto_sst/lib_object.h
@@ -0,0 +1,348 @@
+/**
+ * Copyright(c) 2011 Trusted Logic.   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 Trusted Logic 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.
+ */
+
+#ifndef   __LIB_OBJECT_H__
+#define   __LIB_OBJECT_H__
+
+#include "s_type.h"
+
+typedef struct
+{
+   /* Type of storage: See S_STORAGE_TYPE_XXX */
+   uint32_t nStorageType;
+
+   /* Login type of the client: See S_LOGIN_XXX */
+   uint32_t nLoginType;
+
+   /* Identifier of the client (secure or non-secure client) */
+   S_UUID sClientUUID;
+}
+S_STORAGE_NAME;
+
+
+/**
+ * This library defines three types of objects and keys:
+ * - objects identified by a 16-bit handle
+ * - objects identified by a S_STORAGE_NAME
+ * - objects identified by a filename, which is a variable-size up-to-64 bytes byte array
+ * - unindexed objects
+ **/
+
+/* -------------------------------------------------------------------------
+   Useful macro to get a structure from a pointer to one of its fields.
+
+   Typical usage:
+   typedef struct
+   {
+      LIB_OBJECT_NODE_HANDLE16  sNodeInHandleTable;
+      LIB_OBJECT_NODE_UNINDEXED sNodeInList;
+   }
+   CONTEXT;
+
+   LIB_OBJECT_CONTAINER_OF(libObjectUnindexedNext(pList, pObject), CONTEXT, sNodeInList)
+
+
+   -------------------------------------------------------------------------*/
+#define LIB_OBJECT_CONTAINER_OF(ptr, type, member) (((type*)(((char*)(ptr)) - offsetof(type, member))))
+
+
+/* -------------------------------------------------------------------------
+   Table of objects indexed by 16-bit handles
+   -------------------------------------------------------------------------*/
+
+#define LIB_OBJECT_HANDLE16_MAX ((uint16_t)0xFFFF)
+
+/**
+ * NODE of an object in a table indexed by 16-bit handles
+ **/
+typedef struct
+{
+   /* Implementation-defined fields */
+   uint32_t _l[2];
+
+   /* Public field */
+   uint16_t   nHandle;
+}
+LIB_OBJECT_NODE_HANDLE16;
+
+/**
+ * A table of objects indexed by 16-bit handles
+ **/
+typedef struct
+{
+   LIB_OBJECT_NODE_HANDLE16* pRoot;
+}
+LIB_OBJECT_TABLE_HANDLE16;
+
+/**
+ * Add an object in a handle table. This function also
+ * assigns a new handle value to the object. The handle
+ * is guaranteed to be unique among all the objects
+ * in the table and non-zero.
+ *
+ * Returns false if the maximum number of handles has been reached
+ *    (i.e., there are already 65535 objects in the table)
+ **/
+bool libObjectHandle16Add(
+               LIB_OBJECT_TABLE_HANDLE16* pTable,
+               LIB_OBJECT_NODE_HANDLE16* pObject);
+
+/**
+ * Search an object by its handle. Return NULL if the
+ * object is not found
+ **/
+LIB_OBJECT_NODE_HANDLE16* libObjectHandle16Search(
+               LIB_OBJECT_TABLE_HANDLE16* pTable,
+               uint32_t nHandle);
+
+/**
+ * Remove an object from a handle table.
+ *
+ * The object must be part of the table
+ **/
+void libObjectHandle16Remove(
+               LIB_OBJECT_TABLE_HANDLE16* pTable,
+               LIB_OBJECT_NODE_HANDLE16* pObject);
+
+/**
+ * Remove one object from the table. This is useful when
+ * you want to destroy all the objects in the table.
+ *
+ * Returns NULL if the table is empty
+ **/
+LIB_OBJECT_NODE_HANDLE16* libObjectHandle16RemoveOne(
+               LIB_OBJECT_TABLE_HANDLE16* pTable);
+
+/**
+ * Get the object following pObject in the handle table.
+ * If pObject is NULL, return the first object in the list
+ * Return NULL if the object is the last in the table
+ **/
+LIB_OBJECT_NODE_HANDLE16* libObjectHandle16Next(
+               LIB_OBJECT_TABLE_HANDLE16* pTable,
+               LIB_OBJECT_NODE_HANDLE16* pObject);
+
+/* -------------------------------------------------------------------------
+   Table of objects indexed by storage name
+   -------------------------------------------------------------------------*/
+
+/**
+ * NODE of an object in a table indexed by storage name
+ **/
+typedef struct
+{
+   /* Implementation-defined fields */
+   uint32_t _l[2];
+
+   /* Public fields */
+   S_STORAGE_NAME sStorageName;
+}
+LIB_OBJECT_NODE_STORAGE_NAME;
+
+/**
+ * A table of objects indexed by storage name
+ **/
+typedef struct
+{
+   LIB_OBJECT_NODE_STORAGE_NAME* pRoot;
+}
+LIB_OBJECT_TABLE_STORAGE_NAME;
+
+/**
+ * Add an object in a storage name table.
+ *
+ * The object must not be part of the table yet. The caller
+ * must have set pObject->sStorageName with the storage name
+ **/
+void libObjectStorageNameAdd(
+               LIB_OBJECT_TABLE_STORAGE_NAME* pTable,
+               LIB_OBJECT_NODE_STORAGE_NAME* pObject);
+
+/**
+ * Search an object by its storage name. Return NULL if the
+ * object is not found
+ **/
+LIB_OBJECT_NODE_STORAGE_NAME* libObjectStorageNameSearch(
+               LIB_OBJECT_TABLE_STORAGE_NAME* pTable,
+               S_STORAGE_NAME* pStorageName);
+
+/**
+ * Remove an object from a storage name table.
+ *
+ * The object must be part of the table
+ **/
+void libObjectStorageNameRemove(
+               LIB_OBJECT_TABLE_STORAGE_NAME* pTable,
+               LIB_OBJECT_NODE_STORAGE_NAME* pObject);
+
+/**
+ * Remove one object from the table. This is useful when
+ * you want to destroy all the objects in the table
+ * Returns NULL if the table is empty
+ **/
+LIB_OBJECT_NODE_STORAGE_NAME* libObjectStorageNameRemoveOne(
+               LIB_OBJECT_TABLE_STORAGE_NAME* pTable);
+
+/**
+ * Get the object following pObject in the storage name table.
+ * If pObject is NULL, return the first object
+ * Return NULL if the object is the last in the table
+ **/
+LIB_OBJECT_NODE_STORAGE_NAME* libObjectStorageNameNext(
+               LIB_OBJECT_TABLE_STORAGE_NAME* pTable,
+               LIB_OBJECT_NODE_STORAGE_NAME* pObject);
+
+/* -------------------------------------------------------------------------
+   Table of objects indexed by filenames
+   -------------------------------------------------------------------------*/
+
+/**
+ * NODE of an object in a table indexed by filenames (varsize up-to-64 bytes)
+ **/
+typedef struct
+{
+   /* Implementation-defined fields */
+   uint32_t _l[2];
+
+   /* Public fields */
+   uint8_t  sFilename[64];
+   uint8_t  nFilenameLength;
+}
+LIB_OBJECT_NODE_FILENAME;
+
+/**
+ * A table of objects indexed by filenames
+ **/
+typedef struct
+{
+   LIB_OBJECT_NODE_FILENAME* pRoot;
+}
+LIB_OBJECT_TABLE_FILENAME;
+
+/**
+ * Add an object in a filename table.
+ *
+ * The object must not be part of the table yet. The caller
+ * must have set pObject->sFilename and pObject->nFilenameLength
+ * with the object filename
+ **/
+void libObjectFilenameAdd(
+               LIB_OBJECT_TABLE_FILENAME* pTable,
+               LIB_OBJECT_NODE_FILENAME* pObject);
+
+/**
+ * Search an object by its filename. Return NULL if the
+ * object is not found
+ **/
+LIB_OBJECT_NODE_FILENAME* libObjectFilenameSearch(
+               LIB_OBJECT_TABLE_FILENAME* pTable,
+               uint8_t* pFilename,
+               uint32_t  nFilenameLength);
+
+/**
+ * Remove an object from a filename table.
+ *
+ * The object must be part of the table
+ **/
+void libObjectFilenameRemove(
+               LIB_OBJECT_TABLE_FILENAME* pTable,
+               LIB_OBJECT_NODE_FILENAME* pObject);
+
+/**
+ * Remove one element from the table and return it. This is useful when
+ * you want to destroy all the objects in the table
+ * Returns NULL if the table is empty
+ **/
+LIB_OBJECT_NODE_FILENAME* libObjectFilenameRemoveOne(
+               LIB_OBJECT_TABLE_FILENAME* pTable);
+
+/**
+ * Get the object following pObject in the filename table.
+ * If pObject is NULL, return the first object
+ * Return NULL if the object is the last in the table
+ **/
+LIB_OBJECT_NODE_FILENAME* libObjectFilenameNext(
+               LIB_OBJECT_TABLE_FILENAME* pTable,
+               LIB_OBJECT_NODE_FILENAME* pObject);
+
+/* -------------------------------------------------------------------------
+   Unindexed table of objects
+   -------------------------------------------------------------------------*/
+/**
+ * NODE of an unindexed object
+ **/
+typedef struct
+{
+   /* Implementation-defined fields */
+   uint32_t _l[2];
+}
+LIB_OBJECT_NODE_UNINDEXED;
+
+/**
+ * A table of unindexed objects
+ **/
+typedef struct
+{
+   LIB_OBJECT_NODE_UNINDEXED* pRoot;
+}
+LIB_OBJECT_TABLE_UNINDEXED;
+
+
+/**
+ * Add an object in an unindexed table. The object must not be part of the table yet.
+ **/
+void libObjectUnindexedAdd(
+         LIB_OBJECT_TABLE_UNINDEXED* pTable,
+         LIB_OBJECT_NODE_UNINDEXED* pObject);
+
+/**
+ * Remove an object from an unindexed table. The object must be part of the table.
+ **/
+void libObjectUnindexedRemove(
+         LIB_OBJECT_TABLE_UNINDEXED* pTable,
+         LIB_OBJECT_NODE_UNINDEXED* pObject);
+
+/**
+ * Remove one object in the table. This is useful when you want to destroy all objects
+ * in the table.
+ * Returns NULL if the table is empty
+ **/
+LIB_OBJECT_NODE_UNINDEXED* libObjectUnindexedRemoveOne(LIB_OBJECT_TABLE_UNINDEXED* pTable);
+
+/**
+ * Get the object following pObject in the table.
+ * If pObject is NULL, return the first object
+ * Return NULL if the object is the last in the table
+ **/
+LIB_OBJECT_NODE_UNINDEXED* libObjectUnindexedNext(
+               LIB_OBJECT_TABLE_UNINDEXED* pTable,
+               LIB_OBJECT_NODE_UNINDEXED* pObject);
+
+#endif /* __LIB_OBJECT_H__ */
diff --git a/security/tf_crypto_sst/mtc.c b/security/tf_crypto_sst/mtc.c
new file mode 100644
index 0000000..2be4870
--- /dev/null
+++ b/security/tf_crypto_sst/mtc.c
@@ -0,0 +1,274 @@
+/**
+ * Copyright(c) 2011 Trusted Logic.   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 Trusted Logic 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 <stdlib.h>
+#include <string.h>
+
+#define MTC_EXPORTS
+#include "mtc.h"
+
+/* Included for the TEE management */
+#include "pkcs11_internal.h"
+
+
+/*------------------------------------------------------------------------------
+   Defines
+------------------------------------------------------------------------------*/
+
+/**
+ * The magic word.
+ */
+#define MTC_SESSION_MAGIC  ( (uint32_t)0x4D544300 )   /* "MTC\0" */
+
+/**
+ * The MTC session context
+ */
+typedef struct
+{
+   /* Magic word, must be set to {MTC_SESSION_MAGIC}. */
+   uint32_t    nMagicWord;
+
+   /* MTC Identifier */
+   uint32_t nCounterIdentifier;
+
+   /* TEEC session and cryptoki session */
+   TEEC_Session sSession;
+   uint32_t     hCryptoSession;
+
+} MTC_SESSION_CONTEXT;
+
+
+static bool g_bMTCInitialized = false;
+
+
+/*------------------------------------------------------------------------------
+   Static functions
+------------------------------------------------------------------------------*/
+
+static S_RESULT static_getMonotonicCounter(S_HANDLE hCounter,
+                                           S_MONOTONIC_COUNTER_VALUE* psValue,
+                                           bool bIncrement)
+{
+   TEEC_Result          nError;
+   TEEC_Operation       sOperation;
+   MTC_SESSION_CONTEXT* pSession = NULL;
+   uint32_t             nCommandID;
+
+   if (!g_bMTCInitialized)
+   {
+      return S_ERROR_BAD_STATE;
+   }
+
+   pSession = (MTC_SESSION_CONTEXT *)hCounter;
+   if ((pSession == NULL) || (pSession->nMagicWord != MTC_SESSION_MAGIC))
+   {
+      return S_ERROR_BAD_PARAMETERS;
+   }
+
+   if (bIncrement)
+   {
+      nCommandID = SERVICE_SYSTEM_PKCS11_INCREMENT_MTC_COMMAND_ID;
+   }
+   else
+   {
+      nCommandID = SERVICE_SYSTEM_PKCS11_GET_MTC_COMMAND_ID;
+   }
+
+   sOperation.paramTypes = TEEC_PARAM_TYPES(TEEC_VALUE_INOUT, TEEC_NONE, TEEC_NONE, TEEC_NONE);
+   sOperation.params[0].value.a = pSession->nCounterIdentifier;
+   sOperation.params[0].value.b = 0;
+   nError = TEEC_InvokeCommand(&pSession->sSession,
+                            (pSession->hCryptoSession << 16 ) |
+                              (nCommandID & 0x00007FFF),
+                            &sOperation,
+                            NULL);
+
+   psValue->nLow  = sOperation.params[0].value.a;
+   psValue->nHigh = sOperation.params[0].value.b;
+
+   return nError;
+}
+
+/*------------------------------------------------------------------------------
+   API
+------------------------------------------------------------------------------*/
+
+MTC_EXPORT S_RESULT SMonotonicCounterInit(void)
+{
+   TEEC_Result nTeeError;
+
+   stubMutexLock();
+   if (g_bMTCInitialized)
+   {
+      nTeeError = TEEC_SUCCESS;
+   }
+   else
+   {
+      nTeeError = stubInitializeContext();
+      if (nTeeError == TEEC_SUCCESS)
+      {
+         g_bMTCInitialized = true;
+      }
+   }
+   stubMutexUnlock();
+
+   return nTeeError;
+}
+
+MTC_EXPORT void SMonotonicCounterTerminate(void)
+{
+   stubMutexLock();
+   if (g_bMTCInitialized)
+   {
+      stubFinalizeContext();
+      g_bMTCInitialized = false;
+   }
+   stubMutexUnlock();
+}
+
+MTC_EXPORT S_RESULT SMonotonicCounterOpen(
+                 uint32_t nCounterIdentifier,
+                 OUT S_HANDLE* phCounter)
+{
+   TEEC_Result                nError;
+   TEEC_Operation             sOperation;
+   MTC_SESSION_CONTEXT*       pSession = NULL;
+   S_MONOTONIC_COUNTER_VALUE  nCounterValue;
+
+   if (phCounter == NULL)
+   {
+      return S_ERROR_BAD_PARAMETERS;
+   }
+
+   *phCounter = S_HANDLE_NULL;
+
+   if (!g_bMTCInitialized)
+   {
+      return S_ERROR_BAD_STATE;
+   }
+
+   if (nCounterIdentifier != S_MONOTONIC_COUNTER_GLOBAL)
+   {
+      return S_ERROR_ITEM_NOT_FOUND;
+   }
+
+   pSession = (MTC_SESSION_CONTEXT*)malloc(sizeof(MTC_SESSION_CONTEXT));
+   if (pSession == NULL)
+   {
+      return S_ERROR_OUT_OF_MEMORY;
+   }
+   memset(pSession, 0, sizeof(MTC_SESSION_CONTEXT));
+   pSession->nMagicWord = MTC_SESSION_MAGIC;
+
+   /* Open a TEE session with the system service */
+   nError = TEEC_OpenSession(&g_sContext,
+                             &pSession->sSession,
+                             &SERVICE_UUID,
+                             TEEC_LOGIN_PUBLIC,
+                             NULL,
+                             NULL, /* No operation parameters */
+                             NULL);
+   if (nError != TEEC_SUCCESS)
+   {
+      goto error;
+   }
+
+   /* Open a cryptoki session */
+   sOperation.paramTypes = TEEC_PARAM_TYPES(TEEC_VALUE_INOUT, TEEC_NONE, TEEC_NONE, TEEC_NONE);
+   sOperation.params[0].value.a = CKV_TOKEN_SYSTEM_SHARED;
+   sOperation.params[0].value.b = CKF_RW_SESSION | CKF_SERIAL_SESSION;
+   nError = TEEC_InvokeCommand(&pSession->sSession,
+                               SERVICE_SYSTEM_PKCS11_C_OPEN_SESSION_COMMAND_ID & 0x00007FFF,
+                               &sOperation,
+                               NULL);
+   if (nError != TEEC_SUCCESS)
+   {
+      TEEC_CloseSession(&pSession->sSession);
+      goto error;
+   }
+
+   pSession->hCryptoSession = sOperation.params[0].value.a;
+   pSession->nCounterIdentifier = nCounterIdentifier;
+
+   nError = SMonotonicCounterGet((S_HANDLE)pSession, &nCounterValue);
+   if (nError != TEEC_SUCCESS)
+   {
+      SMonotonicCounterClose((S_HANDLE)pSession);
+      return nError;
+   }
+
+   *phCounter = (S_HANDLE)pSession;
+
+   return TEEC_SUCCESS;
+
+error:
+   free(pSession);
+   return nError;
+}
+
+MTC_EXPORT void SMonotonicCounterClose(S_HANDLE hCounter)
+{
+   MTC_SESSION_CONTEXT* pSession;
+
+   if (!g_bMTCInitialized)
+   {
+      return;
+   }
+
+   pSession = (MTC_SESSION_CONTEXT *)hCounter;
+   if ((pSession == NULL) || (pSession->nMagicWord != MTC_SESSION_MAGIC))
+   {
+      return;
+   }
+
+   (void)TEEC_InvokeCommand(&pSession->sSession,
+                            (pSession->hCryptoSession << 16 ) |
+                              (SERVICE_SYSTEM_PKCS11_C_CLOSE_SESSION_COMMAND_ID & 0x00007FFF),
+                            NULL, /* No operation parameters */
+                            NULL);
+
+   TEEC_CloseSession(&pSession->sSession);
+   free(pSession);
+}
+
+MTC_EXPORT S_RESULT SMonotonicCounterGet(
+                 S_HANDLE hCounter,
+                 S_MONOTONIC_COUNTER_VALUE* psCurrentValue)
+{
+   return static_getMonotonicCounter(hCounter, psCurrentValue, false);
+}
+
+MTC_EXPORT S_RESULT SMonotonicCounterIncrement(
+                 S_HANDLE hCounter,
+                 S_MONOTONIC_COUNTER_VALUE* psNewValue)
+{
+   return static_getMonotonicCounter(hCounter, psNewValue, true);
+}
diff --git a/security/tf_crypto_sst/pkcs11_global.c b/security/tf_crypto_sst/pkcs11_global.c
new file mode 100644
index 0000000..f7c491d
--- /dev/null
+++ b/security/tf_crypto_sst/pkcs11_global.c
@@ -0,0 +1,275 @@
+/**
+ * Copyright(c) 2011 Trusted Logic.   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 Trusted Logic 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.
+ */
+
+/*
+ * Implementation Notes:
+ *
+ * This API is NOT thread-safe. Indeed this Cryptoki implementation
+ * only supports option 1 defined in PKCS#11, section 6.5.2:
+ * "The application can specify that it will not be accessing the library concurrently
+ * from multiple threads, and so the library need not worry about performing any type
+ * of locking for the sake of thread-safety."
+ */
+
+#include "pkcs11_internal.h"
+
+/* ------------------------------------------------------------------------
+    System Service UUID
+------------------------------------------------------------------------- */
+const TEEC_UUID SERVICE_UUID = SERVICE_SYSTEM_UUID;
+
+/* ------------------------------------------------------------------------
+    Definition of the global TEE Context
+------------------------------------------------------------------------- */
+TEEC_Context g_sContext;
+/* A mutex that protects the access to the global context and to the
+   g_bContextRefCounter flag */
+LIB_MUTEX g_sContextMutex = LIB_MUTEX_INITIALIZER;
+/* Whether the context has already been initialized or not */
+uint32_t  g_nContextRefCounter = 0;
+
+bool g_bCryptokiInitialized = false;
+
+/* ------------------------------------------------------------------------
+   Internal global TEE context management
+------------------------------------------------------------------------- */
+
+void stubMutexLock(void)
+{
+   libMutexLock(&g_sContextMutex);
+}
+
+void stubMutexUnlock(void)
+{
+   libMutexUnlock(&g_sContextMutex);
+}
+
+/* This API must be protected by stubMutexLock/Unlock */
+TEEC_Result stubInitializeContext(void)
+{
+   TEEC_Result nTeeError;
+
+   if (g_nContextRefCounter)
+   {
+      g_nContextRefCounter ++;
+      return TEEC_SUCCESS;
+   }
+
+   nTeeError = TEEC_InitializeContext(NULL, &g_sContext);
+   if (nTeeError == TEEC_SUCCESS)
+   {
+      g_nContextRefCounter = 1;
+   }
+
+   return nTeeError;
+}
+
+/* This API must be protected by stubMutexLock/Unlock */
+void stubFinalizeContext(void)
+{
+   if (g_nContextRefCounter > 0)
+   {
+      g_nContextRefCounter --;
+   }
+
+   if (g_nContextRefCounter == 0)
+   {
+      TEEC_FinalizeContext(&g_sContext);
+      memset(&g_sContext, 0, sizeof(TEEC_Context));
+   }
+}
+
+
+/* ------------------------------------------------------------------------
+                          Internal monitor management
+------------------------------------------------------------------------- */
+/**
+* Check that hSession is a valid primary session,
+* or a valid secondary session attached to a valid primary session.
+*
+* input:
+*   S_HANDLE hSession: the session handle to check
+* output:
+*   bool* pBoolIsPrimarySession: a boolean set to true if the session is primary,
+*             set to false if the session if the session is secondary
+*   returned boolean: set to true iff :
+*              - either hSession is a valid primary session
+*              - or hSession is a valid secondary session attached to a valid primary session
+**/
+bool ckInternalSessionIsOpenedEx(S_HANDLE hSession, bool* pBoolIsPrimarySession)
+{
+   PPKCS11_SESSION_CONTEXT_HEADER   pHeader = (PPKCS11_SESSION_CONTEXT_HEADER)hSession;
+   PPKCS11_PRIMARY_SESSION_CONTEXT  pSession = NULL;
+
+   if ((pHeader == NULL) || (pHeader->nMagicWord != PKCS11_SESSION_MAGIC))
+   {
+      return FALSE;
+   }
+   if (pHeader->nSessionTag == PKCS11_PRIMARY_SESSION_TAG) /* primary session */
+   {
+      pSession = (PPKCS11_PRIMARY_SESSION_CONTEXT)pHeader;
+
+      *pBoolIsPrimarySession = true;
+
+      /* check that primary session is valid */
+      return (pSession->hCryptoSession != CK_INVALID_HANDLE);
+   }
+   else if (pHeader->nSessionTag == PKCS11_SECONDARY_SESSION_TAG) /*secondary session */
+   {
+      PPKCS11_SECONDARY_SESSION_CONTEXT pSecSession = (PPKCS11_SECONDARY_SESSION_CONTEXT)pHeader;
+
+      *pBoolIsPrimarySession = false;
+
+      /* check that primary session is still valid */
+      pSession = pSecSession->pPrimarySession;
+      if (  (pSession == NULL) ||
+            (pSession->sHeader.nMagicWord != PKCS11_SESSION_MAGIC) ||
+            (pSession->sHeader.nSessionTag != PKCS11_PRIMARY_SESSION_TAG))
+      {
+         return FALSE;
+      }
+
+      if (pSession->hCryptoSession == CK_INVALID_HANDLE)
+      {
+         return FALSE;
+      }
+
+      /* check that secondary session is valid */
+      return (pSecSession->hSecondaryCryptoSession != CK_INVALID_HANDLE);
+   }
+   else
+   {
+     return FALSE;
+   }
+}
+
+/* ------------------------------------------------------------------------
+                          Internal error management
+------------------------------------------------------------------------- */
+
+CK_RV ckInternalTeeErrorToCKError(TEEC_Result nError)
+{
+   switch (nError)
+   {
+      case TEEC_SUCCESS:
+         return CKR_OK;
+
+      case TEEC_ERROR_BAD_PARAMETERS:
+      case TEEC_ERROR_BAD_FORMAT:
+         return CKR_ARGUMENTS_BAD;
+      case TEEC_ERROR_OUT_OF_MEMORY:
+         return CKR_HOST_MEMORY;
+      case TEEC_ERROR_ACCESS_DENIED:
+         return CKR_TOKEN_NOT_PRESENT;
+      default:
+         return CKR_DEVICE_ERROR;
+   }
+}
+
+/* ------------------------------------------------------------------------
+                          Public Functions
+------------------------------------------------------------------------- */
+CK_RV PKCS11_EXPORT C_Initialize(CK_VOID_PTR pInitArgs)
+{
+   CK_RV       nErrorCode;
+   TEEC_Result nTeeError;
+
+   if (pInitArgs != NULL_PTR)
+   {
+      return CKR_ARGUMENTS_BAD;
+   }
+
+   stubMutexLock();
+   if (g_bCryptokiInitialized)
+   {
+      nErrorCode = CKR_CRYPTOKI_ALREADY_INITIALIZED;
+   }
+   else
+   {
+      nTeeError = stubInitializeContext();
+      if (nTeeError == TEEC_SUCCESS)
+      {
+         g_bCryptokiInitialized = true;
+      }
+      nErrorCode = ckInternalTeeErrorToCKError(nTeeError);
+   }
+   stubMutexUnlock();
+
+   return nErrorCode;
+}
+
+CK_RV PKCS11_EXPORT C_Finalize(CK_VOID_PTR pReserved)
+{
+   CK_RV nErrorCode;
+
+   if (pReserved != NULL_PTR)
+   {
+      return CKR_ARGUMENTS_BAD;
+   }
+
+   stubMutexLock();
+   if (g_bCryptokiInitialized)
+   {
+      stubFinalizeContext();
+      g_bCryptokiInitialized = false;
+      nErrorCode = CKR_OK;
+   }
+   else
+   {
+      nErrorCode = CKR_CRYPTOKI_NOT_INITIALIZED;
+   }
+   stubMutexUnlock();
+
+   return nErrorCode;
+}
+
+static const CK_INFO sImplementationInfo =
+{
+   {2, 20},         /* cryptokiVersion, spec 2.20 */
+   "Trusted Logic", /* manufacturerID */
+   0,               /* flags */
+   "PKCS#11",       /* libraryDescription */
+   {3, 0}           /* libraryVersion */
+};
+
+CK_RV PKCS11_EXPORT C_GetInfo(CK_INFO_PTR pInfo)
+{
+   if (!g_bCryptokiInitialized)
+   {
+      return CKR_CRYPTOKI_NOT_INITIALIZED;
+   }
+   if (pInfo == NULL_PTR)
+   {
+      return CKR_ARGUMENTS_BAD;
+   }
+
+   memcpy(pInfo, &sImplementationInfo, sizeof(CK_INFO));
+   return CKR_OK;
+}
diff --git a/security/tf_crypto_sst/pkcs11_internal.h b/security/tf_crypto_sst/pkcs11_internal.h
new file mode 100644
index 0000000..14ecf83
--- /dev/null
+++ b/security/tf_crypto_sst/pkcs11_internal.h
@@ -0,0 +1,137 @@
+/**
+ * Copyright(c) 2011 Trusted Logic.   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 Trusted Logic 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.
+ */
+
+#ifndef __PKCS11_INTERNAL_H__
+#define __PKCS11_INTERNAL_H__
+
+#define CRYPTOKI_EXPORTS
+#include "cryptoki.h"
+#include "service_system_protocol.h"
+
+#include "lib_object.h"
+#include "lib_mutex.h"
+#include "tee_client_api.h"
+
+#include <stdlib.h>
+#include <string.h>
+
+
+/**
+ * The magic word.
+ */
+#define PKCS11_SESSION_MAGIC  ( (uint32_t)0x45EF683B )
+
+/**
+ * Computes required size to fit in a 4-bytes aligned buffer (at the end)
+ * If the size is a multiple of 4, just returns the size
+ * Otherwise, return the size so that the (end of the buffer)+1 is 4-bytes aligned.
+ */
+#define PKCS11_GET_SIZE_WITH_ALIGNMENT(a)  (uint32_t)(((uint32_t)a+3) & ~3)
+
+
+/**
+ * The System Service UUID used by the library
+ */
+extern const TEEC_UUID SERVICE_UUID;
+
+/**
+ * g_sContext: the global TEEC context used by the library
+ */
+extern TEEC_Context  g_sContext;
+
+void stubMutexLock(void);
+void stubMutexUnlock(void);
+TEEC_Result stubInitializeContext(void);
+void stubFinalizeContext(void);
+
+/** Whether the cryptoki library is initialized or not */
+extern bool g_bCryptokiInitialized;
+
+CK_RV ckInternalTeeErrorToCKError(TEEC_Result nError);
+
+#define PKCS11_PRIMARY_SESSION_TAG    1
+#define PKCS11_SECONDARY_SESSION_TAG  2
+
+typedef struct
+{
+   /*
+    * Magic word, must be set to {PKCS11_SESSION_MAGIC}.
+    */
+   uint32_t    nMagicWord;
+
+   /* nSessionTag must be set to {PKCS11_PRIMARY_SESSION_TAG} for primary session
+   *                          to {PKCS11_SECONDARY_SESSION_TAG} for secondary session */
+   uint32_t    nSessionTag;
+
+}PKCS11_SESSION_CONTEXT_HEADER, * PPKCS11_SESSION_CONTEXT_HEADER;
+
+/**
+ * The PKCS11 Primary session context
+ */
+typedef struct
+{
+   /* sHeader must be the first field of this structure */
+   PKCS11_SESSION_CONTEXT_HEADER sHeader;
+
+   /* TEEC session used for this cryptoki primary session.
+      Each primary session has its own TEEC session */
+   TEEC_Session sSession;
+   uint32_t     hCryptoSession;
+
+   /* Mutex to protect the table of secondary sessions */
+   LIB_MUTEX sSecondarySessionTableMutex;
+
+   /* Table of secondary sessions */
+   LIB_OBJECT_TABLE_HANDLE16 sSecondarySessionTable;
+
+} PKCS11_PRIMARY_SESSION_CONTEXT, * PPKCS11_PRIMARY_SESSION_CONTEXT;
+
+/**
+ * The PKCS11 Secondary session context
+ */
+typedef struct
+{
+   /* sHeader must be the first field of this structure */
+   PKCS11_SESSION_CONTEXT_HEADER sHeader;
+
+   /* Secondary session handle as returned by pkcs11 */
+   uint32_t       hSecondaryCryptoSession;
+
+   /* A node of the table of secondary sessions */
+   LIB_OBJECT_NODE_HANDLE16 sSecondarySessionNode;
+
+   /* pointer to the primary session */
+   PKCS11_PRIMARY_SESSION_CONTEXT* pPrimarySession;
+
+} PKCS11_SECONDARY_SESSION_CONTEXT, *PPKCS11_SECONDARY_SESSION_CONTEXT;
+
+bool ckInternalSessionIsOpenedEx(S_HANDLE hSession, bool* pBoolIsPrimarySession);
+
+#endif /* __PKCS11_INTERNAL_H__ */
diff --git a/security/tf_crypto_sst/pkcs11_object.c b/security/tf_crypto_sst/pkcs11_object.c
new file mode 100644
index 0000000..53caadc
--- /dev/null
+++ b/security/tf_crypto_sst/pkcs11_object.c
@@ -0,0 +1,1637 @@
+/**
+ * Copyright(c) 2011 Trusted Logic.   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 Trusted Logic 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 "pkcs11_internal.h"
+
+/* ------------------------------------------------------------------------
+Internal Functions
+------------------------------------------------------------------------- */
+/**
+* Checks the following pre-conditions:
+* - cryptoki is initialized,
+* - hSession is valid (primary and/or secondary),
+* - the user is logged in.
+*
+* And updates handle values:
+*  IN/OUT : phSession
+*           IN  = Cryptoki external handle
+*           OUT = TFAPI handle = primary cryptoki session handle
+*  OUT    : phSecSession16Msb
+*           OUT = 0 for a primary session or
+*                 the secondary cryptoki session handle in the 16 MSB bits
+*/
+static CK_RV static_checkPreConditionsAndUpdateHandles(
+         CK_SESSION_HANDLE*   phSession,
+         uint32_t*            phCommandIDAndSession,
+         PPKCS11_PRIMARY_SESSION_CONTEXT* ppSession)
+{
+   bool  bIsPrimarySession;
+
+   /* Check Cryptoki is initialized */
+   if (!g_bCryptokiInitialized)
+   {
+      return CKR_CRYPTOKI_NOT_INITIALIZED;
+   }
+
+   if (phSession == NULL)
+   {
+      return CKR_SESSION_HANDLE_INVALID;
+   }
+
+   /* Check that the session is valid */
+   if (!ckInternalSessionIsOpenedEx(*phSession, &bIsPrimarySession))
+   {
+      return CKR_SESSION_HANDLE_INVALID;
+   }
+   /* previous check is fine, then update session handles */
+   if (bIsPrimarySession)
+   {
+      PPKCS11_PRIMARY_SESSION_CONTEXT pSession =
+         (PPKCS11_PRIMARY_SESSION_CONTEXT)(*phSession);
+
+      *phSession = pSession->hCryptoSession;
+      *phCommandIDAndSession = (pSession->hCryptoSession<<16)|(*phCommandIDAndSession&0x00007FFF);
+      *ppSession = pSession;
+   }
+   else
+   {
+      PPKCS11_SECONDARY_SESSION_CONTEXT pSecSession =
+         (PPKCS11_SECONDARY_SESSION_CONTEXT)(*phSession);
+
+      *phSession = pSecSession->pPrimarySession->hCryptoSession;
+      *phCommandIDAndSession = (pSecSession->hSecondaryCryptoSession<<16)|(1<<15)|(*phCommandIDAndSession&0x00007FFF);
+      *ppSession = pSecSession->pPrimarySession;
+   }
+
+   return CKR_OK;
+}
+
+/******************************************/
+/* The buffer must be freed by the caller */
+/******************************************/
+static CK_RV static_encodeTwoTemplates(
+   uint8_t**         ppBuffer,
+   uint32_t *        pBufferSize,
+   const uint32_t    nParamIndex,
+   CK_ATTRIBUTE*     pTemplate1,
+   CK_ULONG          ulCount1,
+   CK_ATTRIBUTE*     pTemplate2,
+   CK_ULONG          ulCount2)
+{
+   INPUT_TEMPLATE_ITEM  sItem;
+
+   uint32_t i;
+   uint32_t nDataOffset    = 0;
+   uint32_t nBufferIndex   = 0;
+   uint32_t nBufferSize    = 0;
+   uint8_t* pBuffer = NULL;
+   CK_RV    nErrorCode = CKR_OK;
+
+   if (ulCount1 == 0)
+   {
+      /* Nothing to do */
+      return CKR_OK;
+   }
+   if (pTemplate1 == NULL)
+   {
+      /* Nothing to do */
+      return CKR_OK;
+   }
+
+   /* First compute the total required buffer size that
+    * will contain the full templates (for the template 1 AND 2)
+    */
+   nBufferSize =  4 +                                    /* Nb Attributes */
+                  sizeof(INPUT_TEMPLATE_ITEM)*ulCount1;  /* The attributes items */
+   if (pTemplate2 != NULL)
+   {
+      nBufferSize += 4 +                                    /* Nb Attributes */
+                     sizeof(INPUT_TEMPLATE_ITEM)*ulCount2;  /* The attributes items */
+   }
+
+   /* First data (attribute values) on either template 1 or 2 will just be after the last item */
+   nDataOffset = nBufferSize;
+
+   for (i = 0; i < ulCount1; i++)
+   {
+      /* Each value will be aligned on 4 bytes.
+         This computation includes the spare bytes. */
+      nBufferSize += PKCS11_GET_SIZE_WITH_ALIGNMENT(pTemplate1[i].ulValueLen);
+   }
+   if (pTemplate2 != NULL)
+   {
+      for (i = 0; i < ulCount2; i++)
+      {
+         /* Each value will be aligned on 4 bytes.
+            This computation includes the spare bytes. */
+         nBufferSize += PKCS11_GET_SIZE_WITH_ALIGNMENT(pTemplate2[i].ulValueLen);
+      }
+   }
+
+   pBuffer = (uint8_t*)malloc(nBufferSize);
+   if (pBuffer == NULL)
+   {
+      /* Not enough memory */
+      return CKR_DEVICE_MEMORY;
+   }
+
+   memset(pBuffer, 0, nBufferSize);
+
+   /*
+    * First template
+    */
+   *(uint32_t*)(pBuffer + nBufferIndex) = ulCount1;
+   nBufferIndex += 4;
+   for (i = 0; i < ulCount1; i++)
+   {
+      sItem.attributeType     = (uint32_t)pTemplate1[i].type;
+      /* dataOffset = 0 means NULL buffer */
+      sItem.dataOffset        = ((pTemplate1[i].pValue == NULL) ? 0 : nDataOffset);
+      sItem.dataParamIndex    = nParamIndex; /* The parameter where we store the data (0 to 3) */
+      sItem.dataValueLen      = (uint32_t)pTemplate1[i].ulValueLen;
+      /* Copy the item */
+      memcpy(pBuffer + nBufferIndex, &sItem, sizeof(INPUT_TEMPLATE_ITEM));
+      nBufferIndex += sizeof(INPUT_TEMPLATE_ITEM);
+      if (pTemplate1[i].pValue != NULL)
+      {
+         /* Copy the data */
+         memcpy(pBuffer + nDataOffset, (uint8_t*)pTemplate1[i].pValue, (uint32_t)pTemplate1[i].ulValueLen);
+         /* Next data will be stored just after the previous one but aligned on 4 bytes */
+         nDataOffset += PKCS11_GET_SIZE_WITH_ALIGNMENT(pTemplate1[i].ulValueLen);
+         if ((nDataOffset & 0xC0000000) != 0)
+         {
+            /* We whould never go in this case, that means the dataOffset will not be able to store the offset correctly */
+            nErrorCode = CKR_DEVICE_ERROR;
+            goto error;
+         }
+      }
+   }
+
+   /*
+    * Second template
+    */
+   if (pTemplate2 != NULL)
+   {
+      *(uint32_t*)(pBuffer + nBufferIndex) = ulCount2;
+      nBufferIndex += 4;
+      for (i = 0; i < ulCount2; i++)
+      {
+         sItem.attributeType     = (uint32_t)pTemplate2[i].type;
+         /* dataOffset = 0 means NULL buffer */
+         sItem.dataOffset        = ((pTemplate2[i].pValue == NULL) ? 0 : nDataOffset);
+         sItem.dataParamIndex    = nParamIndex; /* The parameter where we store the data (0..3) */
+         sItem.dataValueLen      = (uint32_t)pTemplate2[i].ulValueLen;
+         /* Copy the item */
+         memcpy(pBuffer + nBufferIndex, &sItem, sizeof(INPUT_TEMPLATE_ITEM));
+         nBufferIndex += sizeof(INPUT_TEMPLATE_ITEM);
+         if (pTemplate2[i].pValue != NULL)
+         {
+            /* Copy the data */
+            memcpy(pBuffer + nDataOffset, (uint8_t*)pTemplate2[i].pValue, (uint32_t)pTemplate2[i].ulValueLen);
+            /* Next data will be stored just after the previous one but aligned on 4 bytes */
+            nDataOffset += PKCS11_GET_SIZE_WITH_ALIGNMENT(pTemplate2[i].ulValueLen);
+            if ((nDataOffset & 0xC0000000) != 0)
+            {
+               /* We whould never go in this case, that means the dataOffset will not be able to store the offset correctly */
+               nErrorCode = CKR_DEVICE_ERROR;
+               goto error;
+            }
+         }
+      }
+   }
+
+   *ppBuffer      = pBuffer;
+   *pBufferSize   = nBufferSize;
+
+   return CKR_OK;
+
+error:
+   free(pBuffer);
+   return nErrorCode;
+}
+
+/******************************************/
+/* The buffer must be freed by the caller */
+/******************************************/
+static CK_RV static_encodeTemplate(
+   uint8_t**         ppBuffer,
+   uint32_t*         pBufferSize,
+   const uint32_t    nParamIndex,
+   CK_ATTRIBUTE*     pTemplate,
+   CK_ULONG          ulCount)
+{
+   return static_encodeTwoTemplates(ppBuffer, pBufferSize, nParamIndex, pTemplate, ulCount, NULL, 0);
+}
+/* ----------------------------------------------------------------------- */
+
+static CK_RV static_C_CallInit(
+   uint32_t            nCommandID,
+   CK_SESSION_HANDLE   hSession,
+   const CK_MECHANISM* pMechanism,
+   CK_OBJECT_HANDLE    hKey)
+{
+   TEEC_Result    teeErr;
+   uint32_t       nErrorOrigin;
+   TEEC_Operation sOperation;
+   CK_RV          nErrorCode = CKR_OK;
+   uint32_t       nCommandIDAndSession = nCommandID;
+   uint32_t       nParamType2 = TEEC_NONE;
+   PPKCS11_PRIMARY_SESSION_CONTEXT pSession;
+
+   nErrorCode = static_checkPreConditionsAndUpdateHandles(&hSession, &nCommandIDAndSession, &pSession);
+   if (nErrorCode != CKR_OK)
+   {
+      return nErrorCode;
+   }
+   if (pMechanism == NULL)
+   {
+      return CKR_ARGUMENTS_BAD;
+   }
+
+   memset(&sOperation, 0, sizeof(TEEC_Operation));
+   sOperation.params[0].value.a = (uint32_t)pMechanism->mechanism;
+   if (nCommandID != SERVICE_SYSTEM_PKCS11_C_DIGESTINIT_COMMAND_ID)
+   {
+      sOperation.params[0].value.b = (uint32_t)hKey;
+
+   }
+   sOperation.params[1].tmpref.buffer = (uint8_t*)pMechanism->pParameter;
+   sOperation.params[1].tmpref.size   = (uint32_t)pMechanism->ulParameterLen;
+
+   /* Specific case of RSA OAEP */
+   if (((nCommandID == SERVICE_SYSTEM_PKCS11_C_ENCRYPTINIT_COMMAND_ID)
+      ||(nCommandID == SERVICE_SYSTEM_PKCS11_C_DECRYPTINIT_COMMAND_ID))
+      && (pMechanism->mechanism == CKM_RSA_PKCS_OAEP)
+      && (pMechanism->pParameter != NULL))
+   {
+      /* Add the source buffer of the RSA OAEP mechanism parameters */
+      nParamType2 = TEEC_MEMREF_TEMP_INPUT;
+      sOperation.params[2].tmpref.buffer = (uint8_t*)((CK_RSA_PKCS_OAEP_PARAMS_PTR)(pMechanism->pParameter))->pSourceData;
+      sOperation.params[2].tmpref.size   = (uint32_t) ((CK_RSA_PKCS_OAEP_PARAMS_PTR)(pMechanism->pParameter))->ulSourceDataLen;
+   }
+   sOperation.paramTypes = TEEC_PARAM_TYPES(TEEC_VALUE_INPUT, TEEC_MEMREF_TEMP_INPUT, nParamType2, TEEC_NONE);
+   teeErr = TEEC_InvokeCommand(   &pSession->sSession,
+                                  nCommandIDAndSession,        /* commandID */
+                                  &sOperation,                 /* IN OUT operation */
+                                  &nErrorOrigin                /* OUT returnOrigin, optional */
+                                 );
+   nErrorCode = (nErrorOrigin == TEEC_ORIGIN_TRUSTED_APP ?
+                  teeErr :
+                  ckInternalTeeErrorToCKError(teeErr));
+
+   return nErrorCode;
+}
+
+/* ----------------------------------------------------------------------- */
+/**
+* If bSend, the pData buffer is sent to the service.
+* If bResult, a buffer is received, the convention described in
+* PKCS11 Section 11.2 applies for pResult and pulResultLen.
+* Specific function used for single operation
+*/
+static CK_RV static_C_CallForSingle(
+   uint32_t             nCommandID,
+   CK_SESSION_HANDLE    hSession,
+   const CK_BYTE*       pData,
+   CK_ULONG             ulDataLen,
+   CK_BYTE*             pResult,
+   CK_ULONG*            pulResultLen,
+   bool                 bSend,
+   bool                 bReceive)
+{
+   TEEC_Result       teeErr;
+   uint32_t          nErrorOrigin;
+   TEEC_Operation    sOperation;
+   CK_RV             nErrorCode = CKR_OK;
+   uint32_t          nCommandIDAndSession = nCommandID;
+   uint32_t          nParamType0 = TEEC_NONE;
+   uint32_t          nParamType1 = TEEC_NONE;
+   PPKCS11_PRIMARY_SESSION_CONTEXT pSession;
+
+   nErrorCode = static_checkPreConditionsAndUpdateHandles(&hSession, &nCommandIDAndSession, &pSession);
+   if (nErrorCode != CKR_OK)
+   {
+      return nErrorCode;
+   }
+
+   memset(&sOperation, 0, sizeof(TEEC_Operation));
+
+   if (bSend)
+   {
+      nParamType0 = TEEC_MEMREF_TEMP_INPUT;
+      sOperation.params[0].tmpref.buffer = (uint8_t*)pData;
+      sOperation.params[0].tmpref.size   = (uint32_t)ulDataLen;
+   }
+
+   if (bReceive)
+   {
+      if (pulResultLen == NULL)
+      {
+         /* The P11 API Spec states that, in this case, the operation must be
+            aborted and the error code CKR_ARGUMENTS_BAD must be returned. We
+            achieve this result by sending an invalid parameter type */
+         nParamType1 = TEEC_NONE;
+      }
+      else if (pResult == NULL)
+      {
+         /* If pResult is NULL, the caller only wants the buffer length.
+            Send a NULL output memref */
+         nParamType1 = TEEC_MEMREF_TEMP_OUTPUT;
+         sOperation.params[1].tmpref.buffer = (uint8_t*)NULL;
+      }
+      else
+      {
+         /* send the result buffer information */
+         nParamType1 = TEEC_MEMREF_TEMP_OUTPUT;
+         sOperation.params[1].tmpref.buffer = (uint8_t*)pResult;
+         sOperation.params[1].tmpref.size   = (uint32_t)*pulResultLen;
+      }
+   }
+
+   sOperation.paramTypes = TEEC_PARAM_TYPES(nParamType0, nParamType1, TEEC_NONE, TEEC_NONE);
+   teeErr = TEEC_InvokeCommand(&pSession->sSession,
+                               nCommandIDAndSession,     /* commandID */
+                               &sOperation,              /* IN OUT operation */
+                               &nErrorOrigin             /* OUT returnOrigin, optional */
+                              );
+   if (teeErr != TEEC_SUCCESS)
+   {
+      nErrorCode = (nErrorOrigin == TEEC_ORIGIN_TRUSTED_APP ?
+                     teeErr :
+                     ckInternalTeeErrorToCKError(teeErr));
+      goto end;
+   }
+
+   /* Success */
+   nErrorCode = CKR_OK;
+
+ end:
+   if (bReceive)
+   {
+      if ((nErrorCode == CKR_OK) || (nErrorCode == CKR_BUFFER_TOO_SMALL))
+      {
+         /* The service has returned the actual result */
+         /* The data is already in pResult, we get the returned length */
+         *pulResultLen = sOperation.params[1].tmpref.size;
+      }
+   }
+
+   return nErrorCode;
+}
+
+/* ----------------------------------------------------------------------- */
+/**
+* If bSend, the pData buffer is sent to the service.
+* If bResult, a buffer is received, the convention described in
+* PKCS11 Section 11.2 applies for pResult and pulResultLen.
+* Specific function only used for update operations
+*/
+static CK_RV static_C_CallUpdate(
+   uint32_t             nCommandID,
+   CK_SESSION_HANDLE    hSession,
+   const CK_BYTE*       pData,
+   CK_ULONG             ulDataLen,
+   CK_BYTE*             pResult,
+   CK_ULONG*            pulResultLen,
+   bool                 bSend,
+   bool                 bReceive)
+{
+   TEEC_Result       teeErr;
+   uint32_t          nErrorOrigin;
+   TEEC_Operation    sOperation;
+   CK_RV             nErrorCode = CKR_OK;
+   uint32_t          nResultLen = 0;
+   uint32_t          nCommandIDAndSession = nCommandID;
+   uint32_t          nParamType0 = TEEC_NONE;
+   uint32_t          nParamType1 = TEEC_NONE;
+   PPKCS11_PRIMARY_SESSION_CONTEXT pSession;
+
+   nErrorCode = static_checkPreConditionsAndUpdateHandles(&hSession, &nCommandIDAndSession, &pSession);
+   if (nErrorCode != CKR_OK)
+   {
+      return nErrorCode;
+   }
+
+   if (pulResultLen != NULL)
+   {
+      nResultLen = *pulResultLen;
+   }
+
+   memset(&sOperation, 0, sizeof(TEEC_Operation));
+
+   if (bSend)
+   {
+      nParamType0 = TEEC_MEMREF_TEMP_INPUT;
+      sOperation.params[0].tmpref.buffer = (void*)pData;
+      sOperation.params[0].tmpref.size   = ulDataLen;
+   }
+
+   if (bReceive)
+   {
+      if (pulResultLen == NULL)
+      {
+         /* The P11 API Spec states that, in this case, the operation must be
+            aborted and the error code CKR_ARGUMENTS_BAD must be returned. We
+            achieve this result by setting an invalid parameter type */
+         nParamType1 = TEEC_NONE;
+      }
+      else if (pResult == NULL)
+      {
+         /* If pResult is NULL, the caller only wants the output buffer length.
+            Pass a NULL output ref */
+         nParamType1 = TEEC_MEMREF_TEMP_OUTPUT;
+         sOperation.params[1].tmpref.buffer = NULL;
+      }
+      else
+      {
+         /* send the result buffer information */
+         nParamType1 = TEEC_MEMREF_TEMP_OUTPUT;
+         sOperation.params[1].tmpref.buffer = pResult;
+         sOperation.params[1].tmpref.size   = (uint32_t)*pulResultLen;
+      }
+   }
+
+   sOperation.paramTypes = TEEC_PARAM_TYPES(nParamType0, nParamType1, TEEC_NONE, TEEC_NONE);
+   teeErr = TEEC_InvokeCommand(   &pSession->sSession,
+                                  nCommandIDAndSession,        /* commandID */
+                                  &sOperation,                 /* IN OUT operation */
+                                  &nErrorOrigin                /* OUT returnOrigin, optional */
+                                 );
+   if (teeErr != TEEC_SUCCESS)
+   {
+      nErrorCode = (nErrorOrigin == TEEC_ORIGIN_TRUSTED_APP ?
+                     teeErr :
+                     ckInternalTeeErrorToCKError(teeErr));
+      goto end;
+   }
+
+   /* Success */
+   nErrorCode = CKR_OK;
+
+ end:
+   if (bReceive)
+   {
+      if ((nErrorCode == CKR_OK) || (nErrorCode == CKR_BUFFER_TOO_SMALL))
+      {
+         /* The service has returned the actual result */
+         /* The data is already in pResult, we get the returned length */
+         *pulResultLen = sOperation.params[1].tmpref.size;
+      }
+   }
+
+   return nErrorCode;
+}
+
+/* Splits the buffer pData in chunks of nChunkSize size
+ * and calls static_C_CallUpdate for each chunk
+ */
+static CK_RV static_C_CallSplitUpdate(
+                           uint32_t           nCommandID,
+                           CK_SESSION_HANDLE  hSession,
+                           const CK_BYTE*     pData,
+                           CK_ULONG           ulDataLen,
+                           CK_BYTE*           pResult,
+                           CK_ULONG*          pulResultLen,
+                           bool               bSend,
+                           bool               bReceive,
+                           uint32_t           nChunkSize)
+{
+   CK_RV nErrorCode;
+   CK_ULONG nPartDataLen;
+   CK_ULONG nPartResultLen = 0;
+   CK_ULONG ulResultLen = 0;
+   bool bIsSymOperation = false;
+
+   if (pulResultLen != NULL)
+   {
+      ulResultLen = *pulResultLen;
+      /* Check wether the operation is a symetrical or asymetrical */
+      if (*pulResultLen == ulDataLen)
+      {
+         bIsSymOperation = true;
+      }
+      *pulResultLen = 0;
+   }
+
+   while (ulDataLen > 0)
+   {
+      nPartDataLen = (ulDataLen <= nChunkSize ?
+                        ulDataLen : nChunkSize);
+      if (bIsSymOperation)
+      {
+         /* update the result only if it is a symetric operation */
+         nPartResultLen = (ulResultLen <= nChunkSize ?
+                               ulResultLen : nChunkSize);
+      }
+      else
+      {
+         nPartResultLen = ulResultLen;
+      }
+
+      nErrorCode =  static_C_CallUpdate(
+                                 nCommandID,
+                                 hSession,
+                                 pData,
+                                 nPartDataLen,
+                                 pResult,
+                                 &nPartResultLen,
+                                 bSend,
+                                 bReceive);
+      if (nErrorCode != CKR_OK)
+      {
+         return nErrorCode;
+      }
+
+      ulDataLen -= nPartDataLen;
+      pData += nPartDataLen;
+
+      if (pResult != NULL)
+      {
+         ulResultLen -= nPartResultLen;
+         pResult += nPartResultLen;
+      }
+
+      if ((pulResultLen != NULL) && (bIsSymOperation))
+      {
+         *pulResultLen += nPartResultLen;
+      }
+   }
+   return CKR_OK;
+}
+
+/* Decides whether to split or not the inout/output buffer into chunks
+*/
+static CK_RV static_C_Call_CallForUpdate(
+                           uint32_t           nCommandID,
+                           CK_SESSION_HANDLE  hSession,
+                           const CK_BYTE*     pData,
+                           CK_ULONG           ulDataLen,
+                           CK_BYTE*           pResult,
+                           CK_ULONG*          pulResultLen,
+                           bool               bSend,
+                           bool               bReceive)
+{
+   CK_RV                   nErrorCode;
+   uint32_t                nChunkSize;
+
+   TEEC_ImplementationLimits  limits;
+
+   if (!g_bCryptokiInitialized)
+   {
+      return CKR_CRYPTOKI_NOT_INITIALIZED;
+   }
+
+   TEEC_GetImplementationLimits(&limits);
+
+   /* We can split the buffer in chunks of fixed size.
+      No matter of the start address of the buffer,
+      a safe size would be TotalNumberOfPages - 1
+   */
+   nChunkSize = limits.tmprefMaxSize - limits.pageSize;
+
+   if (ulDataLen > nChunkSize)
+   {
+      /* inoutMaxSize = 0  means unlimited size */
+       nErrorCode = static_C_CallSplitUpdate(nCommandID,
+                                 hSession,
+                                 pData,
+                                 ulDataLen,
+                                 pResult,
+                                 pulResultLen,
+                                 bSend,
+                                 bReceive,
+                                 nChunkSize);
+   }
+   else
+   {
+      nErrorCode = static_C_CallUpdate(nCommandID,
+                                 hSession,
+                                 pData,
+                                 ulDataLen,
+                                 pResult,
+                                 pulResultLen,
+                                 bSend,
+                                 bReceive);
+   }
+   return nErrorCode;
+
+}
+
+/* ------------------------------------------------------------------------
+Public Functions
+------------------------------------------------------------------------- */
+
+CK_RV PKCS11_EXPORT C_CreateObject(
+   CK_SESSION_HANDLE    hSession,    /* the session's handle */
+   const CK_ATTRIBUTE*  pTemplate,   /* the object's template */
+   CK_ULONG             ulCount,     /* attributes in template */
+   CK_OBJECT_HANDLE*    phObject)    /* receives new object's handle. */
+{
+   TEEC_Result          teeErr;
+   uint32_t             nErrorOrigin;
+   TEEC_Operation       sOperation;
+   CK_RV                nErrorCode = CKR_OK;
+   PPKCS11_PRIMARY_SESSION_CONTEXT pSession;
+   uint32_t             nCommandIDAndSession = SERVICE_SYSTEM_PKCS11_C_CREATEOBJECT_COMMAND_ID;
+   uint8_t*             pBuffer = NULL;
+   uint32_t             nBufferSize = 0;
+
+   if ( pTemplate == NULL || phObject == NULL )
+   {
+      return CKR_ARGUMENTS_BAD;
+   }
+
+   nErrorCode = static_checkPreConditionsAndUpdateHandles(&hSession, &nCommandIDAndSession, &pSession);
+   if (nErrorCode != CKR_OK)
+   {
+      return nErrorCode;
+   }
+
+   nErrorCode = static_encodeTemplate(&pBuffer, &nBufferSize, 0, (CK_ATTRIBUTE*)pTemplate, ulCount); /* Sets the template on the param 0 */
+   if (nErrorCode != CKR_OK)
+   {
+      return nErrorCode;
+   }
+
+   memset(&sOperation, 0, sizeof(TEEC_Operation));
+   sOperation.paramTypes = TEEC_PARAM_TYPES(TEEC_MEMREF_TEMP_INPUT, TEEC_VALUE_OUTPUT, TEEC_NONE, TEEC_NONE);
+   sOperation.params[0].tmpref.buffer = pBuffer;
+   sOperation.params[0].tmpref.size   = nBufferSize;
+   teeErr = TEEC_InvokeCommand(   &pSession->sSession,
+                                  nCommandIDAndSession,        /* commandID */
+                                  &sOperation,                 /* IN OUT operation */
+                                  &nErrorOrigin                /* OUT returnOrigin, optional */
+                                 );
+   free(pBuffer);
+
+   if (teeErr != TEEC_SUCCESS)
+   {
+      nErrorCode = (nErrorOrigin == TEEC_ORIGIN_TRUSTED_APP ?
+                     teeErr :
+                     ckInternalTeeErrorToCKError(teeErr));
+      goto end;
+   }
+
+   *phObject = sOperation.params[1].value.a;
+
+   /* Success */
+   nErrorCode = CKR_OK;
+
+end:
+   return nErrorCode;
+}
+
+CK_RV PKCS11_EXPORT C_DestroyObject(
+   CK_SESSION_HANDLE hSession,  /* the session's handle */
+   CK_OBJECT_HANDLE  hObject)   /* the object's handle */
+{
+   TEEC_Result    teeErr;
+   uint32_t       nErrorOrigin;
+   TEEC_Operation sOperation;
+   CK_RV       nErrorCode = CKR_OK;
+   uint32_t    nCommandIDAndSession = SERVICE_SYSTEM_PKCS11_C_DESTROYOBJECT_COMMAND_ID;
+   PPKCS11_PRIMARY_SESSION_CONTEXT pSession;
+
+   nErrorCode = static_checkPreConditionsAndUpdateHandles(&hSession, &nCommandIDAndSession, &pSession);
+   if (nErrorCode != CKR_OK)
+   {
+      return nErrorCode;
+   }
+
+   memset(&sOperation, 0, sizeof(TEEC_Operation));
+   sOperation.paramTypes = TEEC_PARAM_TYPES(TEEC_VALUE_INPUT, TEEC_NONE, TEEC_NONE, TEEC_NONE);
+   sOperation.params[0].value.a = (uint32_t)hObject;
+   teeErr = TEEC_InvokeCommand(   &pSession->sSession,
+                                  nCommandIDAndSession,        /* commandID */
+                                  &sOperation,                 /* IN OUT operation */
+                                  &nErrorOrigin                /* OUT returnOrigin, optional */
+                                 );
+   nErrorCode = (nErrorOrigin == TEEC_ORIGIN_TRUSTED_APP ?
+                  teeErr :
+                  ckInternalTeeErrorToCKError(teeErr));
+   return nErrorCode;
+}
+
+CK_RV PKCS11_EXPORT C_GetAttributeValue(
+   CK_SESSION_HANDLE hSession,   /* the session's handle */
+   CK_OBJECT_HANDLE  hObject,    /* the object's handle */
+   CK_ATTRIBUTE*     pTemplate,  /* specifies attributes, gets values */
+   CK_ULONG          ulCount)    /* attributes in template */
+{
+   TEEC_Result       teeErr;
+   uint32_t          nErrorOrigin;
+   TEEC_Operation    sOperation;
+   CK_RV             nErrorCode = CKR_OK;
+   CK_RV             nFinalErrorCode = CKR_OK;
+   uint32_t          i = 0;
+   uint32_t          nCommandIDAndSession = SERVICE_SYSTEM_PKCS11_C_GETATTRIBUTEVALUE_COMMAND_ID;
+   PPKCS11_PRIMARY_SESSION_CONTEXT pSession;
+
+   nErrorCode = static_checkPreConditionsAndUpdateHandles(&hSession, &nCommandIDAndSession, &pSession);
+   if (nErrorCode != CKR_OK)
+   {
+      return nErrorCode;
+   }
+
+   if (pTemplate == NULL)
+   {
+      return CKR_ARGUMENTS_BAD;
+   }
+
+   if (ulCount == 0)
+   {
+      return CKR_OK;
+   }
+
+   for (i = 0; i < ulCount; i++)
+   {
+      memset(&sOperation, 0, sizeof(TEEC_Operation));
+      sOperation.paramTypes = TEEC_PARAM_TYPES(TEEC_VALUE_INPUT, TEEC_MEMREF_TEMP_OUTPUT, TEEC_NONE, TEEC_NONE);
+      sOperation.params[0].value.a = (uint32_t)hObject;
+      sOperation.params[0].value.b = (uint32_t)pTemplate[i].type;
+      sOperation.params[1].tmpref.buffer = pTemplate[i].pValue;
+      sOperation.params[1].tmpref.size   = pTemplate[i].ulValueLen;
+      teeErr = TEEC_InvokeCommand(   &pSession->sSession,
+                                     nCommandIDAndSession,        /* commandID */
+                                     &sOperation,                 /* IN OUT operation */
+                                     &nErrorOrigin                /* OUT returnOrigin, optional */
+                                    );
+      nErrorCode = (nErrorOrigin == TEEC_ORIGIN_TRUSTED_APP ?
+                     teeErr :
+                     ckInternalTeeErrorToCKError(teeErr));
+      if (nErrorCode != CKR_OK)
+      {
+         if (  (nErrorCode == CKR_ATTRIBUTE_SENSITIVE) ||
+               (nErrorCode == CKR_ATTRIBUTE_TYPE_INVALID) ||
+               (nErrorCode == CKR_BUFFER_TOO_SMALL))
+         {
+            nFinalErrorCode = nErrorCode;
+         }
+         else
+         {
+            /* Not some of the special error codes: this is fatal */
+            return nErrorCode;
+         }
+      }
+
+      pTemplate[i].ulValueLen = sOperation.params[1].tmpref.size;
+   }
+
+   return nFinalErrorCode;
+}
+
+CK_RV PKCS11_EXPORT C_FindObjectsInit(
+   CK_SESSION_HANDLE    hSession,   /* the session's handle */
+   const CK_ATTRIBUTE*  pTemplate,  /* attribute values to match */
+   CK_ULONG             ulCount)    /* attributes in search template */
+{
+   TEEC_Result    teeErr;
+   uint32_t       nErrorOrigin;
+   TEEC_Operation sOperation;
+   CK_RV       nErrorCode = CKR_OK;
+   PPKCS11_PRIMARY_SESSION_CONTEXT pSession;
+   uint32_t    nCommandIDAndSession = SERVICE_SYSTEM_PKCS11_C_FINDOBJECTSINIT_COMMAND_ID;
+   uint8_t*    pBuffer     = NULL;
+   uint32_t    nBufferSize = 0;
+
+   nErrorCode = static_checkPreConditionsAndUpdateHandles(&hSession, &nCommandIDAndSession, &pSession);
+   if (nErrorCode != CKR_OK)
+   {
+      return nErrorCode;
+   }
+
+   nErrorCode = static_encodeTemplate(&pBuffer, &nBufferSize, 0, (CK_ATTRIBUTE*)pTemplate, ulCount); /* Sets the template on the param 0 */
+   if (nErrorCode != CKR_OK)
+   {
+      return nErrorCode;
+   }
+
+   memset(&sOperation, 0, sizeof(TEEC_Operation));
+   sOperation.paramTypes = TEEC_PARAM_TYPES(TEEC_MEMREF_TEMP_INPUT, TEEC_NONE, TEEC_NONE, TEEC_NONE);
+   sOperation.params[0].tmpref.buffer = pBuffer;
+   sOperation.params[0].tmpref.size   = nBufferSize;
+   teeErr = TEEC_InvokeCommand(   &pSession->sSession,
+                                  nCommandIDAndSession,        /* commandID */
+                                  &sOperation,                 /* IN OUT operation */
+                                  &nErrorOrigin                /* OUT returnOrigin, optional */
+                                 );
+   free(pBuffer);
+
+   nErrorCode = (nErrorOrigin == TEEC_ORIGIN_TRUSTED_APP ?
+                  teeErr :
+                  ckInternalTeeErrorToCKError(teeErr));
+   return nErrorCode;
+}
+
+
+CK_RV PKCS11_EXPORT C_FindObjects(
+   CK_SESSION_HANDLE hSession,          /* the session's handle */
+   CK_OBJECT_HANDLE* phObject,          /* receives object handle array */
+   CK_ULONG          ulMaxObjectCount,  /* max handles to be returned */
+   CK_ULONG*         pulObjectCount)    /* actual number returned */
+{
+   TEEC_Result       teeErr;
+   uint32_t          nErrorOrigin;
+   TEEC_Operation    sOperation;
+   CK_RV             nErrorCode = CKR_OK;
+   PPKCS11_PRIMARY_SESSION_CONTEXT pSession;
+   uint32_t          nCommandIDAndSession = SERVICE_SYSTEM_PKCS11_C_FINDOBJECTS_COMMAND_ID;
+
+   if ( (phObject == NULL) || (pulObjectCount == NULL))
+   {
+      return CKR_ARGUMENTS_BAD;
+   }
+
+   *pulObjectCount = 0;
+
+   nErrorCode = static_checkPreConditionsAndUpdateHandles(&hSession, &nCommandIDAndSession, &pSession);
+   if (nErrorCode != CKR_OK)
+   {
+      return nErrorCode;
+   }
+
+   memset(&sOperation, 0, sizeof(TEEC_Operation));
+   sOperation.paramTypes = TEEC_PARAM_TYPES(TEEC_MEMREF_TEMP_OUTPUT, TEEC_NONE, TEEC_NONE, TEEC_NONE);
+   sOperation.params[0].tmpref.buffer = (uint8_t*)phObject;
+   sOperation.params[0].tmpref.size   = (uint32_t)ulMaxObjectCount * sizeof(uint32_t);
+
+   teeErr = TEEC_InvokeCommand(   &pSession->sSession,
+                                  nCommandIDAndSession,        /* commandID */
+                                  &sOperation,                 /* IN OUT operation */
+                                  &nErrorOrigin                /* OUT returnOrigin, optional */
+                                 );
+
+   if (teeErr != TEEC_SUCCESS)
+   {
+      nErrorCode = (nErrorOrigin == TEEC_ORIGIN_TRUSTED_APP ?
+                     teeErr :
+                     ckInternalTeeErrorToCKError(teeErr));
+      return nErrorCode;
+   }
+
+   *pulObjectCount = sOperation.params[0].tmpref.size / sizeof(uint32_t);
+
+   return CKR_OK;
+}
+
+CK_RV PKCS11_EXPORT C_FindObjectsFinal(CK_SESSION_HANDLE hSession) /* the session's handle */
+{
+   TEEC_Result    teeErr;
+   uint32_t       nErrorOrigin;
+   TEEC_Operation sOperation;
+   CK_RV          nErrorCode = CKR_OK;
+   uint32_t       nCommandIDAndSession = SERVICE_SYSTEM_PKCS11_C_FINDOBJECTSFINAL_COMMAND_ID;
+   PPKCS11_PRIMARY_SESSION_CONTEXT pSession;
+
+   nErrorCode = static_checkPreConditionsAndUpdateHandles(&hSession, &nCommandIDAndSession, &pSession);
+   if (nErrorCode != CKR_OK)
+   {
+      return nErrorCode;
+   }
+
+   memset(&sOperation, 0, sizeof(TEEC_Operation));
+   teeErr = TEEC_InvokeCommand(   &pSession->sSession,
+                                  nCommandIDAndSession,        /* commandID */
+                                  &sOperation,                 /* IN OUT operation */
+                                  &nErrorOrigin                /* OUT returnOrigin, optional */
+                                 );
+
+   nErrorCode = (nErrorOrigin == TEEC_ORIGIN_TRUSTED_APP ?
+                  teeErr :
+                  ckInternalTeeErrorToCKError(teeErr));
+   return nErrorCode;
+}
+
+
+CK_RV PKCS11_EXPORT C_DigestInit(
+   CK_SESSION_HANDLE   hSession,   /* the session's handle */
+   const CK_MECHANISM* pMechanism) /* the digesting mechanism */
+{
+   return static_C_CallInit(
+      SERVICE_SYSTEM_PKCS11_C_DIGESTINIT_COMMAND_ID,
+      hSession,
+      pMechanism,
+      CK_INVALID_HANDLE);
+}
+
+CK_RV PKCS11_EXPORT C_Digest(
+   CK_SESSION_HANDLE hSession,     /* the session's handle */
+   const CK_BYTE*    pData,        /* data to be digested */
+   CK_ULONG          ulDataLen,    /* bytes of data to be digested */
+   CK_BYTE*          pDigest,      /* receives the message digest */
+   CK_ULONG*         pulDigestLen) /* receives byte length of digest */
+{
+   return static_C_CallForSingle(
+      SERVICE_SYSTEM_PKCS11_C_DIGEST_COMMAND_ID,
+      hSession,
+      pData,
+      ulDataLen,
+      pDigest,
+      pulDigestLen,
+      TRUE,
+      TRUE);
+}
+
+CK_RV PKCS11_EXPORT C_DigestUpdate(
+   CK_SESSION_HANDLE hSession,  /* the session's handle */
+   const CK_BYTE*    pPart,     /* data to be digested */
+   CK_ULONG          ulPartLen) /* bytes of data to be digested */
+{
+   return static_C_Call_CallForUpdate(
+      SERVICE_SYSTEM_PKCS11_C_DIGESTUPDATE_COMMAND_ID,
+      hSession,
+      pPart,
+      ulPartLen,
+      NULL,
+      NULL,
+      TRUE,
+      FALSE);
+}
+
+CK_RV PKCS11_EXPORT C_DigestFinal(
+   CK_SESSION_HANDLE hSession,  /* the session's handle */
+   CK_BYTE*       pDigest,      /* receives the message digest */
+   CK_ULONG*      pulDigestLen) /* receives byte count of digest */
+{
+   return static_C_Call_CallForUpdate(
+      SERVICE_SYSTEM_PKCS11_C_DIGESTFINAL_COMMAND_ID,
+      hSession,
+      NULL,
+      0,
+      pDigest,
+      pulDigestLen,
+      FALSE,
+      TRUE);
+}
+
+
+CK_RV PKCS11_EXPORT C_SignInit(
+   CK_SESSION_HANDLE    hSession,    /* the session's handle */
+   const CK_MECHANISM*  pMechanism,  /* the signature mechanism */
+   CK_OBJECT_HANDLE     hKey)        /* handle of the signature key */
+{
+   return static_C_CallInit(
+      SERVICE_SYSTEM_PKCS11_C_SIGNINIT_COMMAND_ID,
+      hSession,
+      pMechanism,
+      hKey);
+}
+
+CK_RV PKCS11_EXPORT C_Sign(
+   CK_SESSION_HANDLE hSession,        /* the session's handle */
+   const CK_BYTE*    pData,           /* the data (digest) to be signed */
+   CK_ULONG          ulDataLen,       /* count of bytes to be signed */
+   CK_BYTE*          pSignature,      /* receives the signature */
+   CK_ULONG*         pulSignatureLen) /* receives byte count of signature */
+{
+   return static_C_CallForSingle(
+      SERVICE_SYSTEM_PKCS11_C_SIGN_COMMAND_ID,
+      hSession,
+      pData,
+      ulDataLen,
+      pSignature,
+      pulSignatureLen,
+      TRUE,
+      TRUE);
+}
+
+CK_RV PKCS11_EXPORT C_SignUpdate(
+   CK_SESSION_HANDLE hSession,  /* the session's handle */
+   const CK_BYTE*    pPart,     /* the data (digest) to be signed */
+   CK_ULONG          ulPartLen) /* count of bytes to be signed */
+{
+   return static_C_Call_CallForUpdate(
+      SERVICE_SYSTEM_PKCS11_C_SIGNUPDATE_COMMAND_ID,
+      hSession,
+      pPart,
+      ulPartLen,
+      NULL,
+      NULL,
+      TRUE,
+      FALSE);
+}
+
+CK_RV PKCS11_EXPORT C_SignFinal(
+   CK_SESSION_HANDLE hSession,     /* the session's handle */
+   CK_BYTE*       pSignature,      /* receives the signature */
+   CK_ULONG*      pulSignatureLen) /* receives byte count of signature */
+{
+   return static_C_Call_CallForUpdate(
+      SERVICE_SYSTEM_PKCS11_C_SIGNFINAL_COMMAND_ID,
+      hSession,
+      NULL,
+      0,
+      pSignature,
+      pulSignatureLen,
+      FALSE,
+      TRUE);
+}
+
+CK_RV PKCS11_EXPORT C_EncryptInit(
+   CK_SESSION_HANDLE   hSession,    /* the session's handle */
+   const CK_MECHANISM* pMechanism,  /* the encryption mechanism */
+   CK_OBJECT_HANDLE    hKey)        /* handle of encryption key */
+{
+   return static_C_CallInit(
+      SERVICE_SYSTEM_PKCS11_C_ENCRYPTINIT_COMMAND_ID,
+      hSession,
+      pMechanism,
+      hKey);
+}
+
+CK_RV PKCS11_EXPORT C_Encrypt(
+   CK_SESSION_HANDLE hSession,            /* the session's handle */
+   const CK_BYTE*    pData,               /* the plaintext data */
+   CK_ULONG          ulDataLen,           /* bytes of plaintext data */
+   CK_BYTE*          pEncryptedData,      /* receives encrypted data */
+   CK_ULONG*         pulEncryptedDataLen) /* receives encrypted byte count */
+{
+
+   return static_C_CallForSingle(
+      SERVICE_SYSTEM_PKCS11_C_ENCRYPT_COMMAND_ID,
+      hSession,
+      pData,
+      ulDataLen,
+      pEncryptedData,
+      pulEncryptedDataLen,
+      TRUE,
+      TRUE);
+}
+
+
+
+CK_RV PKCS11_EXPORT C_EncryptUpdate(
+   CK_SESSION_HANDLE hSession,           /* the session's handle */
+   const CK_BYTE*    pPart,              /* the plaintext data */
+   CK_ULONG          ulPartLen,          /* bytes of plaintext data */
+   CK_BYTE*          pEncryptedPart,     /* receives encrypted data */
+   CK_ULONG*         pulEncryptedPartLen)/* receives encrypted byte count */
+{
+   return static_C_Call_CallForUpdate(
+      SERVICE_SYSTEM_PKCS11_C_ENCRYPTUPDATE_COMMAND_ID,
+      hSession,
+      pPart,
+      ulPartLen,
+      pEncryptedPart,
+      pulEncryptedPartLen,
+      TRUE,
+      TRUE);
+}
+
+CK_RV PKCS11_EXPORT C_EncryptFinal(
+   CK_SESSION_HANDLE hSession,             /* the session's handle */
+   CK_BYTE*       pLastEncryptedPart,      /* receives encrypted last part */
+   CK_ULONG*      pulLastEncryptedPartLen) /* receives byte count */
+{
+   return static_C_Call_CallForUpdate(
+      SERVICE_SYSTEM_PKCS11_C_ENCRYPTFINAL_COMMAND_ID,
+      hSession,
+      NULL,
+      0,
+      pLastEncryptedPart,
+      pulLastEncryptedPartLen,
+      FALSE,
+      TRUE);
+}
+
+CK_RV PKCS11_EXPORT C_DecryptInit(
+   CK_SESSION_HANDLE   hSession,    /* the session's handle */
+   const CK_MECHANISM* pMechanism,  /* the decryption mechanism */
+   CK_OBJECT_HANDLE    hKey)        /* handle of the decryption key */
+{
+   return static_C_CallInit(
+      SERVICE_SYSTEM_PKCS11_C_DECRYPTINIT_COMMAND_ID,
+      hSession,
+      pMechanism,
+      hKey);
+}
+
+CK_RV PKCS11_EXPORT C_Decrypt(
+   CK_SESSION_HANDLE hSession,           /* the session's handle */
+   const CK_BYTE*    pEncryptedData,     /* input encrypted data */
+   CK_ULONG          ulEncryptedDataLen, /* count of bytes of input */
+   CK_BYTE*          pData,              /* receives decrypted output */
+   CK_ULONG*         pulDataLen)         /* receives decrypted byte count */
+{
+
+   return static_C_CallForSingle(
+      SERVICE_SYSTEM_PKCS11_C_DECRYPT_COMMAND_ID,
+      hSession,
+      pEncryptedData,
+      ulEncryptedDataLen,
+      pData,
+      pulDataLen,
+      TRUE,
+      TRUE);
+}
+
+CK_RV PKCS11_EXPORT C_DecryptUpdate(
+   CK_SESSION_HANDLE hSession,            /* the session's handle */
+   const CK_BYTE*    pEncryptedPart,      /* input encrypted data */
+   CK_ULONG          ulEncryptedPartLen,  /* count of bytes of input */
+   CK_BYTE*          pPart,               /* receives decrypted output */
+   CK_ULONG*         pulPartLen)          /* receives decrypted byte count */
+{
+   return static_C_Call_CallForUpdate(
+      SERVICE_SYSTEM_PKCS11_C_DECRYPTUPDATE_COMMAND_ID,
+      hSession,
+      pEncryptedPart,
+      ulEncryptedPartLen,
+      pPart,
+      pulPartLen,
+      TRUE,
+      TRUE);
+}
+
+CK_RV PKCS11_EXPORT C_DecryptFinal(
+   CK_SESSION_HANDLE hSession,    /* the session's handle */
+   CK_BYTE*       pLastPart,      /* receives decrypted output */
+   CK_ULONG*      pulLastPartLen) /* receives decrypted byte count */
+{
+   return static_C_Call_CallForUpdate(
+      SERVICE_SYSTEM_PKCS11_C_DECRYPTFINAL_COMMAND_ID,
+      hSession,
+      NULL,
+      0,
+      pLastPart,
+      pulLastPartLen,
+      FALSE,
+      TRUE);
+}
+
+
+CK_RV PKCS11_EXPORT C_GenerateKey(
+   CK_SESSION_HANDLE    hSession,    /* the session's handle */
+   const CK_MECHANISM*  pMechanism,  /* the key generation mechanism */
+   const CK_ATTRIBUTE*  pTemplate,   /* template for the new key */
+   CK_ULONG             ulCount,     /* number of attributes in template */
+   CK_OBJECT_HANDLE*    phKey)       /* receives handle of new key */
+{
+   TEEC_Result    teeErr;
+   uint32_t       nErrorOrigin;
+   TEEC_Operation sOperation;
+   CK_RV       nErrorCode = CKR_OK;
+   uint32_t    nCommandIDAndSession = SERVICE_SYSTEM_PKCS11_C_GENERATEKEY_COMMAND_ID;
+   uint8_t*    pBuffer     = NULL;
+   uint32_t    nBufferSize = 0;
+   PPKCS11_PRIMARY_SESSION_CONTEXT pSession;
+
+   if ((pMechanism == NULL) || (phKey == NULL) || (pTemplate == NULL))
+   {
+      return CKR_ARGUMENTS_BAD;
+   }
+
+   nErrorCode = static_checkPreConditionsAndUpdateHandles(&hSession, &nCommandIDAndSession, &pSession);
+   if (nErrorCode != CKR_OK)
+   {
+      return nErrorCode;
+   }
+
+   nErrorCode = static_encodeTemplate(&pBuffer, &nBufferSize, 2, (CK_ATTRIBUTE*)pTemplate, ulCount);
+   if (nErrorCode != CKR_OK)
+   {
+      return nErrorCode;
+   }
+
+   memset(&sOperation, 0, sizeof(TEEC_Operation));
+   sOperation.params[0].value.a = (uint32_t)pMechanism->mechanism;
+   sOperation.params[0].value.b = 0;
+   sOperation.params[1].tmpref.buffer = pMechanism->pParameter;
+   sOperation.params[1].tmpref.size = (uint32_t)pMechanism->ulParameterLen;
+   sOperation.params[2].tmpref.buffer = pBuffer;
+   sOperation.params[2].tmpref.size = nBufferSize;
+   sOperation.paramTypes = TEEC_PARAM_TYPES(TEEC_VALUE_INOUT, TEEC_MEMREF_TEMP_INPUT, TEEC_MEMREF_TEMP_INPUT, TEEC_NONE);
+   teeErr = TEEC_InvokeCommand(   &pSession->sSession,
+                                  nCommandIDAndSession,        /* commandID */
+                                  &sOperation,                 /* IN OUT operation */
+                                  &nErrorOrigin                /* OUT returnOrigin, optional */
+                                 );
+   free(pBuffer);
+
+   if (teeErr != TEEC_SUCCESS)
+   {
+      nErrorCode = (nErrorOrigin == TEEC_ORIGIN_TRUSTED_APP ?
+                     teeErr :
+                     ckInternalTeeErrorToCKError(teeErr));
+      return nErrorCode;
+   }
+
+   *phKey = sOperation.params[0].value.a;
+
+   return CKR_OK;
+}
+
+CK_RV PKCS11_EXPORT C_GenerateKeyPair(
+   CK_SESSION_HANDLE    hSession,                    /* the session's handle */
+   const CK_MECHANISM*  pMechanism,                  /* the key gen. mech. */
+   const CK_ATTRIBUTE*  pPublicKeyTemplate,          /* pub. attr. template */
+   CK_ULONG             ulPublicKeyAttributeCount,   /* # of pub. attrs. */
+   const CK_ATTRIBUTE*  pPrivateKeyTemplate,         /* priv. attr. template */
+   CK_ULONG             ulPrivateKeyAttributeCount,  /* # of priv. attrs. */
+   CK_OBJECT_HANDLE*    phPublicKey,                 /* gets pub. key handle */
+   CK_OBJECT_HANDLE*    phPrivateKey)                /* gets priv. key handle */
+{
+   TEEC_Result    teeErr;
+   uint32_t       nErrorOrigin;
+   TEEC_Operation sOperation;
+   CK_RV       nErrorCode = CKR_OK;
+   uint32_t    nCommandIDAndSession = SERVICE_SYSTEM_PKCS11_C_GENERATEKEYPAIR_COMMAND_ID;
+   uint8_t*    pBuffer     = NULL;
+   uint32_t    nBufferSize = 0;
+   PPKCS11_PRIMARY_SESSION_CONTEXT pSession;
+
+   if (  (pMechanism == NULL) ||
+         (pPublicKeyTemplate == NULL) || (pPrivateKeyTemplate == NULL) ||
+         (phPublicKey== NULL) || (phPrivateKey== NULL))
+   {
+      return CKR_ARGUMENTS_BAD;
+   }
+
+   nErrorCode = static_checkPreConditionsAndUpdateHandles(&hSession, &nCommandIDAndSession, &pSession);
+   if (nErrorCode != CKR_OK)
+   {
+      return nErrorCode;
+   }
+
+   nErrorCode = static_encodeTwoTemplates(&pBuffer, &nBufferSize, 2, (CK_ATTRIBUTE*)pPublicKeyTemplate, ulPublicKeyAttributeCount, (CK_ATTRIBUTE*)pPrivateKeyTemplate, ulPrivateKeyAttributeCount);
+   if (nErrorCode != CKR_OK)
+   {
+      return nErrorCode;
+   }
+
+   memset(&sOperation, 0, sizeof(TEEC_Operation));
+   sOperation.params[0].value.a = (uint32_t)pMechanism->mechanism;
+   sOperation.params[0].value.b = 0;
+   sOperation.params[1].tmpref.buffer = (uint8_t*)pMechanism->pParameter;
+   sOperation.params[1].tmpref.size = (uint32_t)pMechanism->ulParameterLen;
+   sOperation.params[2].tmpref.buffer = pBuffer;
+   sOperation.params[2].tmpref.size = nBufferSize;
+   sOperation.paramTypes = TEEC_PARAM_TYPES(TEEC_VALUE_INOUT, TEEC_MEMREF_TEMP_INPUT, TEEC_MEMREF_TEMP_INPUT, TEEC_NONE);
+   teeErr = TEEC_InvokeCommand(   &pSession->sSession,
+                                  nCommandIDAndSession,        /* commandID */
+                                  &sOperation,                 /* IN OUT operation */
+                                  &nErrorOrigin                /* OUT returnOrigin, optional */
+                                 );
+   free(pBuffer);
+
+   if (teeErr != TEEC_SUCCESS)
+   {
+      nErrorCode = (nErrorOrigin == TEEC_ORIGIN_TRUSTED_APP ?
+                     teeErr :
+                     ckInternalTeeErrorToCKError(teeErr));
+      return nErrorCode;
+   }
+
+   *phPublicKey  = sOperation.params[0].value.a;
+   *phPrivateKey = sOperation.params[0].value.b;
+
+   return CKR_OK;
+}
+
+CK_RV PKCS11_EXPORT C_DeriveKey(
+   CK_SESSION_HANDLE    hSession,          /* session's handle */
+   const CK_MECHANISM*  pMechanism,        /* key deriv. mech. */
+   CK_OBJECT_HANDLE     hBaseKey,          /* base key */
+   const CK_ATTRIBUTE*  pTemplate,         /* new key template */
+   CK_ULONG             ulAttributeCount,  /* template length */
+   CK_OBJECT_HANDLE*    phKey)             /* gets new handle */
+{
+   TEEC_Result    teeErr;
+   uint32_t       nErrorOrigin;
+   TEEC_Operation sOperation;
+   CK_RV       nErrorCode = CKR_OK;
+   uint32_t    nCommandIDAndSession = SERVICE_SYSTEM_PKCS11_C_DERIVEKEY_COMMAND_ID;
+   uint8_t*    pBuffer     = NULL;
+   uint32_t    nBufferSize = 0;
+   PPKCS11_PRIMARY_SESSION_CONTEXT pSession;
+
+   if ((pMechanism == NULL) || (pTemplate == NULL) || (phKey == NULL))
+   {
+      return CKR_ARGUMENTS_BAD;
+   }
+
+   nErrorCode = static_checkPreConditionsAndUpdateHandles(&hSession, &nCommandIDAndSession, &pSession);
+   if (nErrorCode != CKR_OK)
+   {
+      return nErrorCode;
+   }
+
+   nErrorCode = static_encodeTemplate(&pBuffer, &nBufferSize, 2, (CK_ATTRIBUTE*)pTemplate, ulAttributeCount);
+   if (nErrorCode != CKR_OK)
+   {
+      return nErrorCode;
+   }
+
+   memset(&sOperation, 0, sizeof(TEEC_Operation));
+   sOperation.params[0].value.a = (uint32_t)pMechanism->mechanism;
+   sOperation.params[0].value.b = (uint32_t)hBaseKey;
+   sOperation.params[1].tmpref.buffer = (uint8_t*)pMechanism->pParameter;
+   sOperation.params[1].tmpref.size = (uint32_t)pMechanism->ulParameterLen;
+   sOperation.params[2].tmpref.buffer = pBuffer;
+   sOperation.params[2].tmpref.size = nBufferSize;
+   sOperation.paramTypes = TEEC_PARAM_TYPES(TEEC_VALUE_INOUT, TEEC_MEMREF_TEMP_INPUT, TEEC_MEMREF_TEMP_INPUT, TEEC_NONE);
+   teeErr = TEEC_InvokeCommand(   &pSession->sSession,
+                                  nCommandIDAndSession,        /* commandID */
+                                  &sOperation,                 /* IN OUT operation */
+                                  &nErrorOrigin                /* OUT returnOrigin, optional */
+                                 );
+   free(pBuffer);
+
+   if (teeErr != TEEC_SUCCESS)
+   {
+      nErrorCode = (nErrorOrigin == TEEC_ORIGIN_TRUSTED_APP ?
+                     teeErr :
+                     ckInternalTeeErrorToCKError(teeErr));
+      return nErrorCode;
+   }
+
+   *phKey = sOperation.params[0].value.a;
+
+   return CKR_OK;
+}
+
+CK_RV PKCS11_EXPORT C_SeedRandom(
+   CK_SESSION_HANDLE hSession,  /* the session's handle */
+   const CK_BYTE*    pSeed,     /* the seed material */
+   CK_ULONG          ulSeedLen) /* count of bytes of seed material */
+{
+   TEEC_Result    teeErr;
+   uint32_t       nErrorOrigin;
+   TEEC_Operation sOperation;
+   CK_RV       nErrorCode = CKR_OK;
+   uint32_t    nCommandIDAndSession = SERVICE_SYSTEM_PKCS11_C_SEEDRANDOM_COMMAND_ID;
+   PPKCS11_PRIMARY_SESSION_CONTEXT pSession;
+
+   nErrorCode = static_checkPreConditionsAndUpdateHandles(&hSession, &nCommandIDAndSession, &pSession);
+   if (nErrorCode != CKR_OK)
+   {
+      return nErrorCode;
+   }
+   memset(&sOperation, 0, sizeof(TEEC_Operation));
+   sOperation.paramTypes = TEEC_PARAM_TYPES(TEEC_MEMREF_TEMP_INPUT, TEEC_NONE, TEEC_NONE, TEEC_NONE);
+   sOperation.params[0].tmpref.buffer = (uint8_t*)pSeed;
+   sOperation.params[0].tmpref.size   = (uint32_t)ulSeedLen;
+   teeErr = TEEC_InvokeCommand(   &pSession->sSession,
+                                  nCommandIDAndSession,        /* commandID */
+                                  &sOperation,                 /* IN OUT operation */
+                                  &nErrorOrigin                /* OUT returnOrigin, optional */
+                                 );
+
+   nErrorCode = (nErrorOrigin == TEEC_ORIGIN_TRUSTED_APP ?
+                  teeErr :
+                  ckInternalTeeErrorToCKError(teeErr));
+   return nErrorCode;
+}
+
+CK_RV PKCS11_EXPORT C_GenerateRandom(
+   CK_SESSION_HANDLE hSession,    /* the session's handle */
+   CK_BYTE*          pRandomData,  /* receives the random data */
+   CK_ULONG          ulRandomLen) /* number of bytes to be generated */
+{
+   TEEC_Result    teeErr;
+   uint32_t       nErrorOrigin;
+   TEEC_Operation sOperation;
+   CK_RV       nErrorCode = CKR_OK;
+   uint32_t    nCommandIDAndSession = SERVICE_SYSTEM_PKCS11_C_GENERATERANDOM_COMMAND_ID;
+   PPKCS11_PRIMARY_SESSION_CONTEXT pSession;
+
+   nErrorCode = static_checkPreConditionsAndUpdateHandles(&hSession, &nCommandIDAndSession, &pSession);
+   if (nErrorCode != CKR_OK)
+   {
+      return nErrorCode;
+   }
+
+   do
+   {
+      CK_ULONG nArrayLength;
+      nArrayLength = 1024;
+      if (ulRandomLen < nArrayLength)
+      {
+         nArrayLength = ulRandomLen;
+      }
+      memset(&sOperation, 0, sizeof(TEEC_Operation));
+      sOperation.paramTypes = TEEC_PARAM_TYPES(TEEC_MEMREF_TEMP_OUTPUT, TEEC_NONE, TEEC_NONE, TEEC_NONE);
+      sOperation.params[0].tmpref.buffer = (uint8_t*)pRandomData;
+      sOperation.params[0].tmpref.size   = (uint32_t)nArrayLength;
+      teeErr = TEEC_InvokeCommand(   &pSession->sSession,
+                                     nCommandIDAndSession,        /* commandID */
+                                     &sOperation,                 /* IN OUT operation */
+                                     &nErrorOrigin                /* OUT returnOrigin, optional */
+                                    );
+      if (teeErr != TEEC_SUCCESS)
+      {
+         nErrorCode = (nErrorOrigin == TEEC_ORIGIN_TRUSTED_APP ?
+                        teeErr :
+                        ckInternalTeeErrorToCKError(teeErr));
+         return nErrorCode;
+      }
+
+      ulRandomLen -= nArrayLength;
+      pRandomData += nArrayLength;
+      if (ulRandomLen == 0)
+      {
+         break;
+      }
+   }
+   while(1);
+
+   return CKR_OK;
+}
+
+CK_RV PKCS11_EXPORT C_VerifyInit(
+   CK_SESSION_HANDLE   hSession,    /* the session's handle */
+   const CK_MECHANISM* pMechanism,  /* the verification mechanism */
+   CK_OBJECT_HANDLE    hKey)        /* handle of the verification key */
+{
+   return static_C_CallInit(
+      SERVICE_SYSTEM_PKCS11_C_VERIFYINIT_COMMAND_ID,
+      hSession,
+      pMechanism,
+      hKey);
+}
+
+CK_RV PKCS11_EXPORT C_Verify(
+   CK_SESSION_HANDLE hSession,       /* the session's handle */
+   const CK_BYTE*    pData,          /* plaintext data (digest) to compare */
+   CK_ULONG          ulDataLen,      /* length of data (digest) in bytes */
+   CK_BYTE*          pSignature,     /* the signature to be verified */
+   CK_ULONG          ulSignatureLen) /* count of bytes of signature */
+{
+   TEEC_Result    teeErr;
+   uint32_t       nErrorOrigin;
+   TEEC_Operation sOperation;
+   CK_RV       nErrorCode = CKR_OK;
+   uint32_t    nCommandIDAndSession = SERVICE_SYSTEM_PKCS11_C_VERIFY_COMMAND_ID;
+   PPKCS11_PRIMARY_SESSION_CONTEXT pSession;
+
+   nErrorCode = static_checkPreConditionsAndUpdateHandles(&hSession, &nCommandIDAndSession, &pSession);
+   if (nErrorCode != CKR_OK)
+   {
+      return nErrorCode;
+   }
+
+   memset(&sOperation, 0, sizeof(TEEC_Operation));
+   sOperation.params[0].tmpref.buffer = (uint8_t*)pData;
+   sOperation.params[0].tmpref.size   = (uint32_t)ulDataLen;
+   sOperation.params[1].tmpref.buffer = (uint8_t*)pSignature;
+   sOperation.params[1].tmpref.size   = (uint32_t)ulSignatureLen;
+   sOperation.paramTypes = TEEC_PARAM_TYPES(TEEC_MEMREF_TEMP_INPUT, TEEC_MEMREF_TEMP_INPUT, TEEC_NONE, TEEC_NONE);
+   teeErr = TEEC_InvokeCommand(   &pSession->sSession,
+                                  nCommandIDAndSession,        /* commandID */
+                                  &sOperation,                 /* IN OUT operation */
+                                  &nErrorOrigin                /* OUT returnOrigin, optional */
+                                 );
+   nErrorCode = (nErrorOrigin == TEEC_ORIGIN_TRUSTED_APP ?
+                  teeErr :
+                  ckInternalTeeErrorToCKError(teeErr));
+   return nErrorCode;
+}
+
+CK_RV PKCS11_EXPORT C_VerifyUpdate(
+   CK_SESSION_HANDLE hSession,  /* the session's handle */
+   const CK_BYTE*    pPart,     /* plaintext data (digest) to compare */
+   CK_ULONG          ulPartLen) /* length of data (digest) in bytes */
+{
+   return static_C_Call_CallForUpdate(
+      SERVICE_SYSTEM_PKCS11_C_VERIFYUPDATE_COMMAND_ID,
+      hSession,
+      pPart,
+      ulPartLen,
+      NULL,
+      NULL,
+      TRUE,
+      FALSE);
+}
+
+CK_RV PKCS11_EXPORT C_VerifyFinal(
+   CK_SESSION_HANDLE hSession,       /* the session's handle */
+   const CK_BYTE*    pSignature,     /* the signature to be verified */
+   CK_ULONG          ulSignatureLen) /* count of bytes of signature */
+{
+   return static_C_Call_CallForUpdate(
+      SERVICE_SYSTEM_PKCS11_C_VERIFYFINAL_COMMAND_ID,
+      hSession,
+      pSignature,
+      ulSignatureLen,
+      NULL,
+      NULL,
+      TRUE,
+      FALSE);
+}
+
+CK_RV PKCS11_EXPORT C_CloseObjectHandle(
+   CK_SESSION_HANDLE hSession,  /* the session's handle */
+   CK_OBJECT_HANDLE  hObject)   /* the object's handle */
+{
+   TEEC_Result    teeErr;
+   uint32_t       nErrorOrigin;
+   TEEC_Operation sOperation;
+   CK_RV       nErrorCode = CKR_OK;
+   uint32_t    nCommandIDAndSession = SERVICE_SYSTEM_PKCS11_C_CLOSEOBJECTHANDLE_COMMAND_ID;
+   PPKCS11_PRIMARY_SESSION_CONTEXT pSession;
+
+   nErrorCode = static_checkPreConditionsAndUpdateHandles(&hSession, &nCommandIDAndSession, &pSession);
+   if (nErrorCode != CKR_OK)
+   {
+      return nErrorCode;
+   }
+   memset(&sOperation, 0, sizeof(TEEC_Operation));
+   sOperation.paramTypes = TEEC_PARAM_TYPES(TEEC_VALUE_INPUT, TEEC_NONE, TEEC_NONE, TEEC_NONE);
+   sOperation.params[0].value.a = (uint32_t)hObject;
+   sOperation.params[0].value.b = 0;
+   teeErr = TEEC_InvokeCommand(   &pSession->sSession,
+                                  nCommandIDAndSession,        /* commandID */
+                                  &sOperation,                 /* IN OUT operation */
+                                  &nErrorOrigin                /* OUT returnOrigin, optional */
+                                 );
+   nErrorCode = (nErrorOrigin == TEEC_ORIGIN_TRUSTED_APP ?
+                  teeErr :
+                  ckInternalTeeErrorToCKError(teeErr));
+   return nErrorCode;
+}
+
+CK_RV PKCS11_EXPORT C_CopyObject(
+         CK_SESSION_HANDLE    hSession,    /* the session's handle */
+         CK_OBJECT_HANDLE     hObject,     /* the source object's handle */
+   const CK_ATTRIBUTE*        pTemplate,   /* the template of the copied object */
+         CK_ULONG             ulCount,     /* the number of attributes of the template*/
+         CK_OBJECT_HANDLE*    phNewObject) /* the copied object's handle */
+{
+   TEEC_Result    teeErr;
+   uint32_t       nErrorOrigin;
+   TEEC_Operation sOperation;
+   CK_RV       nErrorCode = CKR_OK;
+   uint32_t    nCommandIDAndSession = SERVICE_SYSTEM_PKCS11_C_COPYOBJECT_COMMAND_ID;
+   uint8_t*    pBuffer     = NULL;
+   uint32_t    nBufferSize = 0;
+   PPKCS11_PRIMARY_SESSION_CONTEXT pSession;
+
+   if ((pTemplate == NULL) || (phNewObject == NULL))
+   {
+      return CKR_ARGUMENTS_BAD;
+   }
+
+   nErrorCode = static_checkPreConditionsAndUpdateHandles(&hSession, &nCommandIDAndSession, &pSession);
+   if (nErrorCode != CKR_OK)
+   {
+      return nErrorCode;
+   }
+
+   nErrorCode = static_encodeTemplate(&pBuffer, &nBufferSize, 1, (CK_ATTRIBUTE*)pTemplate, ulCount);
+   if (nErrorCode != CKR_OK)
+   {
+      return nErrorCode;
+   }
+
+   memset(&sOperation, 0, sizeof(TEEC_Operation));
+   sOperation.params[0].value.a = (uint32_t)hObject;
+   sOperation.params[0].value.b = 0;
+   sOperation.params[1].tmpref.buffer = pBuffer;
+   sOperation.params[1].tmpref.size   = nBufferSize;
+   sOperation.paramTypes = TEEC_PARAM_TYPES(TEEC_VALUE_INOUT, TEEC_MEMREF_TEMP_INPUT, TEEC_NONE, TEEC_NONE);
+   teeErr = TEEC_InvokeCommand(   &pSession->sSession,
+                                  nCommandIDAndSession,        /* commandID */
+                                  &sOperation,                 /* IN OUT operation */
+                                  &nErrorOrigin                /* OUT returnOrigin, optional */
+                                 );
+   free(pBuffer);
+
+   if (teeErr != TEEC_SUCCESS)
+   {
+      nErrorCode = (nErrorOrigin == TEEC_ORIGIN_TRUSTED_APP ?
+                     teeErr :
+                     ckInternalTeeErrorToCKError(teeErr));
+      return nErrorCode;
+   }
+
+   *phNewObject = sOperation.params[0].value.a;
+
+   return CKR_OK;
+}
diff --git a/security/tf_crypto_sst/pkcs11_session.c b/security/tf_crypto_sst/pkcs11_session.c
new file mode 100644
index 0000000..bc845c7
--- /dev/null
+++ b/security/tf_crypto_sst/pkcs11_session.c
@@ -0,0 +1,420 @@
+/**
+ * Copyright(c) 2011 Trusted Logic.   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 Trusted Logic 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.
+ */
+
+/*
+ * Implementation Notes:
+ *
+ * The PKCS11 session handle is directly mapped on the
+ * Trusted Foundations Software session handle (S_HANDLE).
+ */
+
+#include "pkcs11_internal.h"
+
+
+/* ------------------------------------------------------------------------
+                          Public Functions
+------------------------------------------------------------------------- */
+
+
+CK_RV PKCS11_EXPORT C_OpenSession(CK_SLOT_ID              slotID,        /* the slot's ID */
+                                    CK_FLAGS              flags,         /* defined in CK_SESSION_INFO */
+                                    CK_VOID_PTR           pApplication,  /* pointer passed to callback */
+                                    CK_NOTIFY             Notify,        /* notification callback function */
+                                    CK_SESSION_HANDLE*    phSession)     /* receives new session handle */
+{
+   CK_RV                   nErrorCode = CKR_OK;
+   uint32_t                nErrorOrigin = TEEC_ORIGIN_API;
+   TEEC_Result             nTeeError;
+   TEEC_Operation          sOperation;
+   PPKCS11_PRIMARY_SESSION_CONTEXT   pSession = NULL;
+   PPKCS11_SECONDARY_SESSION_CONTEXT pSecondarySession = NULL;
+   uint32_t                nLoginType;
+   uint32_t                nLoginData = 0;
+   void*                   pLoginData = NULL;
+   bool                    bIsPrimarySession;
+   char*                   pSignatureFile = NULL;
+   uint32_t                nSignatureFileLen = 0;
+   uint8_t                 nParamType3 = TEEC_NONE;
+
+   /* Prevent the compiler from complaining about unused parameters */
+   do{(void)pApplication;}while(0);
+   do{(void)Notify;}while(0);
+
+   if (phSession == NULL)
+   {
+      return CKR_ARGUMENTS_BAD;
+   }
+
+      /* Check Cryptoki is initialized */
+   if (!g_bCryptokiInitialized)
+   {
+      return CKR_CRYPTOKI_NOT_INITIALIZED;
+   }
+
+   if ((flags & CKVF_OPEN_SUB_SESSION) == 0)
+   {
+      *phSession = CK_INVALID_HANDLE;
+
+      /*
+      * Allocate the session context
+      */
+      pSession = (PPKCS11_PRIMARY_SESSION_CONTEXT)malloc(sizeof(PKCS11_PRIMARY_SESSION_CONTEXT));
+      if (pSession == NULL)
+      {
+         return CKR_DEVICE_MEMORY;
+      }
+
+      pSession->sHeader.nMagicWord  = PKCS11_SESSION_MAGIC;
+      pSession->sHeader.nSessionTag = PKCS11_PRIMARY_SESSION_TAG;
+      memset(&pSession->sSession, 0, sizeof(TEEC_Session));
+      pSession->sSecondarySessionTable.pRoot = NULL_PTR;
+
+      /* The structure must be initialized first (in a portable manner)
+         to make it work on Win32 */
+      memset(&pSession->sSecondarySessionTableMutex, 0,
+               sizeof(pSession->sSecondarySessionTableMutex));
+      libMutexInit(&pSession->sSecondarySessionTableMutex);
+
+      switch (slotID)
+      {
+      case CKV_TOKEN_SYSTEM_SHARED:
+      case CKV_TOKEN_USER_SHARED:
+         nLoginType = TEEC_LOGIN_PUBLIC;
+         break;
+
+      case CKV_TOKEN_SYSTEM:
+      case CKV_TOKEN_USER:
+      default:
+         nLoginType = TEEC_LOGIN_AUTHENTICATION;
+         break;
+      }
+
+      /* Group tokens */
+      if ((slotID >= 0x00010000) && (slotID <= 0x0002FFFF))
+      {
+         nLoginType = TEEC_LOGIN_GROUP;
+
+         /* The 16 lower-order bits encode the group identifier */
+         nLoginData = (uint32_t)slotID & 0x0000FFFF;
+         pLoginData = (void*)&nLoginData;
+
+         /* Update the slotID for the system / PKCS11 service */
+         if ((slotID >= 0x00010000) && (slotID <= 0x0001FFFF))
+         {
+            /* System group token */
+            slotID = 3;       /* CKV_TOKEN_SYSTEM_GROUP */
+         }
+         else  /* ((slotID >= 0x00020000) && (slotID <= 0x0002FFFF)) */
+         {
+            /* User group token */
+            slotID = 0x4014;  /* CKV_TOKEN_USER_GROUP */
+         }
+      }
+
+retry:
+      memset(&sOperation, 0, sizeof(TEEC_Operation));
+
+      if (nLoginType == TEEC_LOGIN_AUTHENTICATION)
+      {
+          nTeeError = TEEC_ReadSignatureFile((void **)&pSignatureFile, &nSignatureFileLen);
+          if (nTeeError != TEEC_ERROR_ITEM_NOT_FOUND)
+          {
+              if (nTeeError != TEEC_SUCCESS)
+              {
+                  goto error;
+              }
+
+              sOperation.params[3].tmpref.buffer = pSignatureFile;
+              sOperation.params[3].tmpref.size   = nSignatureFileLen;
+              nParamType3 = TEEC_MEMREF_TEMP_INPUT;
+          }
+          else
+          {
+              /* No signature file found.
+              * Should use LOGIN_APPLICATION for now
+              * Can not use TEEC_LOGIN_AUTHENTICATION as this means that all .exe wil need a signature file
+              * - a bit annoying for when passing the tests
+              */
+              nLoginType = TEEC_LOGIN_USER_APPLICATION;
+          }
+      }
+
+      sOperation.paramTypes = TEEC_PARAM_TYPES(TEEC_NONE, TEEC_NONE, TEEC_NONE, nParamType3);
+      nTeeError = TEEC_OpenSession(&g_sContext,
+                                &pSession->sSession,        /* OUT session */
+                                &SERVICE_UUID,              /* destination UUID */
+                                nLoginType,                 /* connectionMethod */
+                                pLoginData,                 /* connectionData */
+                                &sOperation,                /* IN OUT operation */
+                                NULL                        /* OUT returnOrigin, optional */
+                                );
+      if (nTeeError != TEEC_SUCCESS)
+      {
+         /* No need of the returnOrigin as this is not specific to P11 */
+
+         if (  (nTeeError == TEEC_ERROR_NOT_SUPPORTED) &&
+               (nLoginType == TEEC_LOGIN_AUTHENTICATION))
+         {
+            /* We could not open a session with the login TEEC_LOGIN_AUTHENTICATION */
+            /* If it is not supported by the product, */
+            /* retry with fallback to TEEC_LOGIN_USER_APPLICATION */
+            nLoginType = TEEC_LOGIN_USER_APPLICATION;
+            goto retry;
+         }
+
+         /* The ERROR_ACCESS_DENIED, if returned, will be converted into CKR_TOKEN_NOT_PRESENT
+          * For the External Cryptographic API, this means that the authentication
+          * of the calling application fails.
+          */
+         goto error;
+      }
+
+      memset(&sOperation, 0, sizeof(TEEC_Operation));
+      sOperation.params[0].value.a = slotID;
+      sOperation.params[0].value.b = flags;  /* access flags */
+      sOperation.paramTypes = TEEC_PARAM_TYPES(TEEC_VALUE_INOUT, TEEC_NONE, TEEC_NONE, TEEC_NONE);
+      nTeeError = TEEC_InvokeCommand(&pSession->sSession,
+                                  SERVICE_SYSTEM_PKCS11_C_OPEN_SESSION_COMMAND_ID & 0x00007FFF,
+                                  &sOperation,              /* IN OUT operation */
+                                  &nErrorOrigin             /* OUT returnOrigin, optional */
+                                 );
+      if (nTeeError != TEEC_SUCCESS)
+      {
+         goto error;
+      }
+
+      *phSession = (CK_SESSION_HANDLE)pSession;
+      pSession->hCryptoSession = sOperation.params[0].value.a;
+
+      return CKR_OK;
+   }
+   else
+   {
+      bool bResult;
+
+      /* Check that {*phSession} is a valid primary session handle */
+      if ((!ckInternalSessionIsOpenedEx(*phSession, &bIsPrimarySession)) ||
+         (!bIsPrimarySession))
+      {
+         return CKR_SESSION_HANDLE_INVALID;
+      }
+
+      pSession = (PPKCS11_PRIMARY_SESSION_CONTEXT)(*phSession);
+
+      /* allocate the secondary session context */
+      pSecondarySession = (PKCS11_SECONDARY_SESSION_CONTEXT*)malloc(sizeof(PKCS11_SECONDARY_SESSION_CONTEXT));
+      if (pSecondarySession == NULL)
+      {
+         return CKR_DEVICE_MEMORY;
+      }
+      pSecondarySession->sHeader.nMagicWord  = PKCS11_SESSION_MAGIC;
+      pSecondarySession->sHeader.nSessionTag = PKCS11_SECONDARY_SESSION_TAG;
+      pSecondarySession->pPrimarySession = pSession;
+
+      libMutexLock(&pSession->sSecondarySessionTableMutex);
+      bResult = libObjectHandle16Add(&pSession->sSecondarySessionTable,
+                                    &pSecondarySession->sSecondarySessionNode);
+      libMutexUnlock(&pSession->sSecondarySessionTableMutex);
+      if (bResult == false)
+      {
+         free(pSecondarySession);
+         return CKR_DEVICE_MEMORY;
+      }
+
+      memset(&sOperation, 0, sizeof(TEEC_Operation));
+      sOperation.paramTypes = TEEC_PARAM_TYPES(TEEC_VALUE_INOUT, TEEC_NONE, TEEC_NONE, TEEC_NONE);
+      nTeeError = TEEC_InvokeCommand(&pSession->sSession,
+                                  (pSession->hCryptoSession << 16) |
+                                    (1 << 15) |
+                                    (SERVICE_SYSTEM_PKCS11_C_OPEN_SESSION_COMMAND_ID & 0x00007FFF),
+                                  &sOperation,                 /* IN OUT operation */
+                                  &nErrorOrigin                /* OUT returnOrigin, optional */
+                                 );
+      if (nTeeError != TEEC_SUCCESS)
+      {
+         goto error;
+      }
+
+      *phSession = (CK_SESSION_HANDLE)pSecondarySession;
+      pSecondarySession->hSecondaryCryptoSession = sOperation.params[0].value.a;
+
+      return CKR_OK;
+   }
+
+error:
+   nErrorCode = (nErrorOrigin == TEEC_ORIGIN_TRUSTED_APP ?
+                  nTeeError :
+                  ckInternalTeeErrorToCKError(nTeeError));
+
+   if ((flags & CKVF_OPEN_SUB_SESSION) == 0)
+   {
+      libMutexDestroy(&pSession->sSecondarySessionTableMutex);
+      free(pSession);
+   }
+   else
+   {
+      libMutexLock(&pSession->sSecondarySessionTableMutex);
+      libObjectHandle16Remove(&pSession->sSecondarySessionTable,&pSecondarySession->sSecondarySessionNode);
+      libMutexUnlock(&pSession->sSecondarySessionTableMutex);
+      free(pSecondarySession);
+   }
+
+   return nErrorCode;
+}
+
+CK_RV PKCS11_EXPORT C_CloseSession(CK_SESSION_HANDLE hSession) /* the session's handle */
+{
+   CK_RV                   nErrorCode = CKR_OK;
+   uint32_t                nErrorOrigin = TEEC_ORIGIN_API;
+   TEEC_Result             nTeeError;
+   TEEC_Operation          sOperation;
+   bool                    bIsPrimarySession;
+
+   /* Check Cryptoki is initialized */
+   if (!g_bCryptokiInitialized)
+   {
+      return CKR_CRYPTOKI_NOT_INITIALIZED;
+   }
+
+   if (!ckInternalSessionIsOpenedEx(hSession, &bIsPrimarySession))
+   {
+      return CKR_SESSION_HANDLE_INVALID;
+   }
+
+   if (bIsPrimarySession)
+   {
+      LIB_OBJECT_NODE_HANDLE16*           pObject;
+      PPKCS11_SECONDARY_SESSION_CONTEXT   pSecSession;
+      PPKCS11_PRIMARY_SESSION_CONTEXT     pSession = (PPKCS11_PRIMARY_SESSION_CONTEXT)hSession;
+
+      hSession = pSession->hCryptoSession;
+
+      memset(&sOperation, 0, sizeof(TEEC_Operation));
+      sOperation.paramTypes = TEEC_PARAM_TYPES(TEEC_NONE, TEEC_NONE, TEEC_NONE, TEEC_NONE);
+      nTeeError = TEEC_InvokeCommand(&pSession->sSession,
+                                  (pSession->hCryptoSession << 16 ) |
+                                    (SERVICE_SYSTEM_PKCS11_C_CLOSE_SESSION_COMMAND_ID & 0x00007FFF),
+                                  &sOperation,                 /* IN OUT operation */
+                                  &nErrorOrigin                /* OUT returnOrigin, optional */
+                                 );
+      if (nTeeError != TEEC_SUCCESS)
+      {
+         goto end;
+      }
+
+      TEEC_CloseSession(&pSession->sSession);
+      memset(&pSession->sSession, 0, sizeof(TEEC_Session));
+
+      /* Free all secondary session contexts */
+      libMutexLock(&pSession->sSecondarySessionTableMutex);
+      pObject = libObjectHandle16RemoveOne(&pSession->sSecondarySessionTable);
+      while (pObject != NULL)
+      {
+         /* find all secondary session contexts,
+            and release associated resources */
+
+         pSecSession = LIB_OBJECT_CONTAINER_OF(pObject, //ptr
+                                               PKCS11_SECONDARY_SESSION_CONTEXT,//type
+                                               sSecondarySessionNode);//member
+
+         /* free secondary session context */
+         free(pSecSession);
+
+         pObject = libObjectHandle16RemoveOne(&pSession->sSecondarySessionTable);
+      }
+      libMutexUnlock(&pSession->sSecondarySessionTableMutex);
+
+      libMutexDestroy(&pSession->sSecondarySessionTableMutex);
+
+      /* free primary session context */
+      free(pSession);
+   }
+   else
+   {
+      PPKCS11_SECONDARY_SESSION_CONTEXT pSecSession = (PPKCS11_SECONDARY_SESSION_CONTEXT)hSession;
+      PPKCS11_PRIMARY_SESSION_CONTEXT   pSession;
+
+      uint32_t nCommandID = ( (pSecSession->hSecondaryCryptoSession & 0xFFFF) << 16 ) |
+                              (1 << 15) |
+                              (SERVICE_SYSTEM_PKCS11_C_CLOSE_SESSION_COMMAND_ID & 0x00007FFF);
+
+      /* every pre-check are fine, then, update the local handles */
+      hSession = pSecSession->pPrimarySession->hCryptoSession;
+      pSession = (PPKCS11_PRIMARY_SESSION_CONTEXT)(pSecSession->pPrimarySession);
+
+      memset(&sOperation, 0, sizeof(TEEC_Operation));
+      sOperation.paramTypes = TEEC_PARAM_TYPES(TEEC_NONE, TEEC_NONE, TEEC_NONE, TEEC_NONE);
+      nTeeError = TEEC_InvokeCommand(&pSession->sSession,
+                                  nCommandID,
+                                  &sOperation,                 /* IN OUT operation */
+                                  &nErrorOrigin                /* OUT returnOrigin, optional */
+                                 );
+      if (nTeeError != TEEC_SUCCESS)
+      {
+         goto end;
+      }
+
+      /* remove the object from the table */
+      libMutexLock(&pSession->sSecondarySessionTableMutex);
+      libObjectHandle16Remove(&pSecSession->pPrimarySession->sSecondarySessionTable, &pSecSession->sSecondarySessionNode);
+      libMutexUnlock(&pSession->sSecondarySessionTableMutex);
+
+      /* free secondary session context */
+      free(pSecSession);
+   }
+
+end:
+   nErrorCode = (nErrorOrigin == TEEC_ORIGIN_TRUSTED_APP ?
+                  nTeeError :
+                  ckInternalTeeErrorToCKError(nTeeError));
+   return nErrorCode;
+}
+
+
+CK_RV PKCS11_EXPORT C_Login(CK_SESSION_HANDLE hSession,  /* the session's handle */
+                              CK_USER_TYPE      userType,  /* the user type */
+                              const CK_UTF8CHAR*   pPin,      /* the user's PIN */
+                              CK_ULONG          ulPinLen)  /* the length of the PIN */
+{
+   /* Prevent the compiler from complaining about unused variables */
+   do{(void)hSession;}while(0);
+   do{(void)userType;}while(0);
+   do{(void)pPin;}while(0);
+   do{(void)ulPinLen;}while(0);
+
+   return CKR_OK;
+}
+
+CK_RV PKCS11_EXPORT C_Logout(CK_SESSION_HANDLE hSession) /* the session's handle */
+{
+   do{(void)hSession;}while(0);
+
+   return CKR_OK;
+}
diff --git a/security/tf_crypto_sst/service_system_protocol.h b/security/tf_crypto_sst/service_system_protocol.h
new file mode 100644
index 0000000..560ec40
--- /dev/null
+++ b/security/tf_crypto_sst/service_system_protocol.h
@@ -0,0 +1,116 @@
+/**
+ * Copyright(c) 2011 Trusted Logic.   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 Trusted Logic 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.
+ */
+
+#ifndef __SERVICE_SYSTEM_PROTOCOL_H__
+#define __SERVICE_SYSTEM_PROTOCOL_H__
+
+#include "s_type.h"
+
+/* -----------------------------------------------------------------------------
+      UUID and Name
+----------------------------------------------------------------------------- */
+
+/** Service Identifier */
+/* {56304b83-5c4e-4428-b99e-605c96ae58d6} */
+#define SERVICE_SYSTEM_UUID   { 0x56304b83, 0x5c4e, 0x4428, { 0xb9, 0x9e, 0x60, 0x5c, 0x96, 0xae, 0x58, 0xd6 } }
+
+/** Generic type for error codes in the system service */
+#define SERVICE_SYSTEM_NAME_ASCII  "SYSTEM"
+
+
+/* -----------------------------------------------------------------------------
+                               SST Command identifiers
+----------------------------------------------------------------------------- */
+
+#define SERVICE_SYSTEM_SST_OPEN_COMMAND_ID                0x00000001
+#define SERVICE_SYSTEM_SST_CLOSE_COMMAND_ID               0x00000002
+#define SERVICE_SYSTEM_SST_READ_COMMAND_ID                0x00000003
+#define SERVICE_SYSTEM_SST_WRITE_COMMAND_ID               0x00000004
+#define SERVICE_SYSTEM_SST_SEEK_COMMAND_ID                0x00000005
+#define SERVICE_SYSTEM_SST_GET_OFFSET_AND_SIZE_COMMAND_ID 0x00000007
+#define SERVICE_SYSTEM_SST_GET_SIZE_COMMAND_ID            0x00000008
+#define SERVICE_SYSTEM_SST_CLOSE_DELETE_COMMAND_ID        0x00000009
+#define SERVICE_SYSTEM_SST_TRUNCATE_COMMAND_ID            0x0000000A
+#define SERVICE_SYSTEM_SST_ENUM_START_COMMAND_ID          0x0000000B
+#define SERVICE_SYSTEM_SST_ENUM_GETNEXT_COMMAND_ID        0x0000000C
+#define SERVICE_SYSTEM_SST_ENUM_CLOSE_COMMAND_ID          0x0000000D
+#define SERVICE_SYSTEM_SST_RENAME_COMMAND_ID              0x0000000E
+
+/* -----------------------------------------------------------------------------
+                      PKCS11/MTC Command identifiers
+----------------------------------------------------------------------------- */
+#define SERVICE_SYSTEM_PKCS11_C_CREATEOBJECT_COMMAND_ID      0x00000020
+#define SERVICE_SYSTEM_PKCS11_C_DESTROYOBJECT_COMMAND_ID     0x00000021
+#define SERVICE_SYSTEM_PKCS11_C_GETATTRIBUTEVALUE_COMMAND_ID 0x00000022
+#define SERVICE_SYSTEM_PKCS11_C_FINDOBJECTSINIT_COMMAND_ID   0x00000023
+#define SERVICE_SYSTEM_PKCS11_C_FINDOBJECTS_COMMAND_ID       0x00000024
+#define SERVICE_SYSTEM_PKCS11_C_FINDOBJECTSFINAL_COMMAND_ID  0x00000025
+#define SERVICE_SYSTEM_PKCS11_C_DIGESTINIT_COMMAND_ID        0x00000026
+#define SERVICE_SYSTEM_PKCS11_C_DIGEST_COMMAND_ID            0x00000027
+#define SERVICE_SYSTEM_PKCS11_C_DIGESTUPDATE_COMMAND_ID      0x00000028
+#define SERVICE_SYSTEM_PKCS11_C_DIGESTFINAL_COMMAND_ID       0x00000029
+#define SERVICE_SYSTEM_PKCS11_C_SIGNINIT_COMMAND_ID          0x0000002A
+#define SERVICE_SYSTEM_PKCS11_C_SIGN_COMMAND_ID              0x0000002B
+#define SERVICE_SYSTEM_PKCS11_C_SIGNUPDATE_COMMAND_ID        0x0000002C
+#define SERVICE_SYSTEM_PKCS11_C_SIGNFINAL_COMMAND_ID         0x0000002D
+#define SERVICE_SYSTEM_PKCS11_C_ENCRYPTINIT_COMMAND_ID       0x0000002E
+#define SERVICE_SYSTEM_PKCS11_C_ENCRYPT_COMMAND_ID           0x0000002F
+#define SERVICE_SYSTEM_PKCS11_C_ENCRYPTUPDATE_COMMAND_ID     0x00000030
+#define SERVICE_SYSTEM_PKCS11_C_ENCRYPTFINAL_COMMAND_ID      0x00000031
+#define SERVICE_SYSTEM_PKCS11_C_DECRYPTINIT_COMMAND_ID       0x00000032
+#define SERVICE_SYSTEM_PKCS11_C_DECRYPT_COMMAND_ID           0x00000033
+#define SERVICE_SYSTEM_PKCS11_C_DECRYPTUPDATE_COMMAND_ID     0x00000034
+#define SERVICE_SYSTEM_PKCS11_C_DECRYPTFINAL_COMMAND_ID      0x00000035
+#define SERVICE_SYSTEM_PKCS11_C_GENERATEKEY_COMMAND_ID       0x00000036
+#define SERVICE_SYSTEM_PKCS11_C_GENERATEKEYPAIR_COMMAND_ID   0x00000037
+#define SERVICE_SYSTEM_PKCS11_C_SEEDRANDOM_COMMAND_ID        0x00000038
+#define SERVICE_SYSTEM_PKCS11_C_GENERATERANDOM_COMMAND_ID    0x00000039
+#define SERVICE_SYSTEM_PKCS11_C_VERIFYINIT_COMMAND_ID        0x0000003A
+#define SERVICE_SYSTEM_PKCS11_C_VERIFY_COMMAND_ID            0x0000003B
+#define SERVICE_SYSTEM_PKCS11_C_VERIFYUPDATE_COMMAND_ID      0x0000003C
+#define SERVICE_SYSTEM_PKCS11_C_VERIFYFINAL_COMMAND_ID       0x0000003D
+#define SERVICE_SYSTEM_PKCS11_C_DERIVEKEY_COMMAND_ID         0x0000003E
+#define SERVICE_SYSTEM_PKCS11_C_ABORTOPERATION_COMMAND_ID    0x0000003F
+#define SERVICE_SYSTEM_PKCS11_C_COPYOBJECT_COMMAND_ID        0x00000040
+#define SERVICE_SYSTEM_PKCS11_C_CLOSEOBJECTHANDLE_COMMAND_ID 0x00000041
+#define SERVICE_SYSTEM_PKCS11_C_OPEN_SESSION_COMMAND_ID      0x00000042
+#define SERVICE_SYSTEM_PKCS11_C_CLOSE_SESSION_COMMAND_ID     0x00000043
+#define SERVICE_SYSTEM_PKCS11_INCREMENT_MTC_COMMAND_ID       0x00000044
+#define SERVICE_SYSTEM_PKCS11_GET_MTC_COMMAND_ID             0x00000045
+
+typedef struct
+{
+   uint32_t             attributeType;
+   uint32_t             dataOffset : 30;
+   uint32_t             dataParamIndex  : 2;
+   uint32_t             dataValueLen;
+} INPUT_TEMPLATE_ITEM;
+
+#endif /* __SERVICE_SYSTEM_PROTOCOL_H__ */
diff --git a/security/tf_crypto_sst/sst_stub.c b/security/tf_crypto_sst/sst_stub.c
new file mode 100644
index 0000000..e8a78bc
--- /dev/null
+++ b/security/tf_crypto_sst/sst_stub.c
@@ -0,0 +1,800 @@
+/**
+ * Copyright(c) 2011 Trusted Logic.   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 Trusted Logic 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.
+ */
+
+#ifdef __ANDROID32__
+#include <stddef.h>
+#endif
+
+#include <stdlib.h>
+#include <string.h>
+
+#define SST_EXPORTS
+#define EXCLUDE_SERVICE_SYSTEM_SST_BASIC_TYPES
+#include "sst.h"
+
+/* Included for the TEE management */
+#include "pkcs11_internal.h"
+
+
+static TEEC_Session g_SSTSession;
+static bool g_bSSTInitialized = false;
+
+
+/* ------------------------------------------------------------------------
+            TEEC -> SST error code translation
+  ------------------------------------------------------------------------- */
+static SST_ERROR static_SSTConvertErrorCode(TEEC_Result nError)
+{
+   switch (nError)
+   {
+      case TEEC_SUCCESS:
+         return SST_SUCCESS;
+      case SST_ERROR_BAD_PARAMETERS:
+      case SST_ERROR_ACCESS_DENIED:
+      case SST_ERROR_ACCESS_CONFLICT:
+      case SST_ERROR_CORRUPTED:
+      case SST_ERROR_NO_SPACE:
+      case SST_ERROR_ITEM_NOT_FOUND:
+      case SST_ERROR_OUT_OF_MEMORY:
+      case SST_ERROR_OVERFLOW:
+         return nError;
+      default:
+         return SST_ERROR_GENERIC;
+   }
+}
+
+static TEEC_Session* static_SSTGetSession(void)
+{
+   if (g_bSSTInitialized)
+   {
+      return &g_SSTSession;
+   }
+
+   return NULL;
+}
+
+SST_ERROR SST_EXPORT_API SSTInit(void)
+{
+   TEEC_Result          nTeeError = TEEC_SUCCESS;
+   TEEC_Operation       sOperation;
+   uint8_t              nParamType3 = TEEC_NONE;
+   void*                pSignatureFile = NULL;
+   uint32_t             nSignatureFileLen = 0;
+   uint32_t             nLoginType;
+
+   stubMutexLock();
+   if (g_bSSTInitialized)
+   {
+      /* SST library already initialized */
+      nTeeError = TEEC_SUCCESS;
+      goto end;
+   }
+
+   nTeeError = stubInitializeContext();
+   if (nTeeError != TEEC_SUCCESS)
+   {
+      goto end;
+   }
+
+   /* Check if there is a signature file.
+    * If yes, send it in param3, otherwise use LOGIN_APPLICATION
+    */
+   nTeeError =  TEEC_ReadSignatureFile(&pSignatureFile, &nSignatureFileLen);
+   if (nTeeError == TEEC_ERROR_ITEM_NOT_FOUND)
+   {
+      nLoginType = TEEC_LOGIN_USER_APPLICATION;
+   }
+   else
+   {
+       if (nTeeError != TEEC_SUCCESS)
+       {
+           goto end;
+       }
+       sOperation.params[3].tmpref.buffer = pSignatureFile;
+       sOperation.params[3].tmpref.size   = nSignatureFileLen;
+       nParamType3 = TEEC_MEMREF_TEMP_INPUT;
+       nLoginType = TEEC_LOGIN_AUTHENTICATION;
+   }
+
+   sOperation.paramTypes = TEEC_PARAM_TYPES(TEEC_NONE, TEEC_NONE, TEEC_NONE, nParamType3);
+   nTeeError = TEEC_OpenSession(&g_sContext,
+                             &g_SSTSession,              /* OUT session */
+                             &SERVICE_UUID,              /* destination UUID */
+                             nLoginType,                 /* connectionMethod */
+                             NULL,                       /* connectionData */
+                             &sOperation,                /* IN OUT operation */
+                             NULL                        /* OUT returnOrigin, optional */
+                             );
+   if (nTeeError != TEEC_SUCCESS)
+   {
+      goto end_finalize_context;
+   }
+
+   g_bSSTInitialized = true;
+   stubMutexUnlock();
+   return SST_SUCCESS;
+
+end_finalize_context:
+   stubFinalizeContext();
+end:
+   stubMutexUnlock();
+   return static_SSTConvertErrorCode(nTeeError);
+}
+
+SST_ERROR SST_EXPORT_API SSTTerminate(void)
+{
+   stubMutexLock();
+   if (g_bSSTInitialized)
+   {
+      TEEC_CloseSession(&g_SSTSession);
+      stubFinalizeContext();
+      g_bSSTInitialized = false;
+   }
+   /* else if not intialized => success too */
+   stubMutexUnlock();
+   return SST_SUCCESS;
+}
+
+
+/* ------------------------------------------------------------------------
+                           Other API Functions
+------------------------------------------------------------------------- */
+
+
+/* Check that the input filename is well-formed */
+static SST_ERROR static_SSTCheckFileName(const char* pName)
+{
+   uint32_t i;
+   char     c;
+
+   if (pName == NULL)
+   {
+      return SST_ERROR_BAD_PARAMETERS;
+   }
+
+   for (i = 0; i <= SST_MAX_FILENAME; i++)
+   {
+      c = pName[i];
+      if (c == 0)
+      {
+         /* End of the string */
+         return SST_SUCCESS;
+      }
+
+      if (c == '/' || c == '\\')
+      {
+         /* Invalid character */
+         return SST_ERROR_BAD_PARAMETERS;
+      }
+
+      if (c < 0x20 || c >= 0x7F)
+      {
+         /* Filename contains illegal characters */
+         return SST_ERROR_BAD_PARAMETERS;
+      }
+   }
+   /* Filename is too long. Zero terminator not found */
+   return SST_ERROR_BAD_PARAMETERS;
+}
+
+static SST_ERROR static_SSTCheckPattern(
+      const char* pFilenamePattern)
+{
+   uint32_t i;
+   if(pFilenamePattern == NULL)
+   {
+      return S_SUCCESS;
+   }
+
+   /**
+    * Check Forbidden characters.
+    */
+   for (i = 0; pFilenamePattern[i] != 0; i++)
+   {
+      if(pFilenamePattern[i] < 0x20 )
+      {
+         return S_ERROR_BAD_PARAMETERS;
+      }
+      else if(pFilenamePattern[i] == 0x2F ) /* '/' */
+      {
+         return S_ERROR_BAD_PARAMETERS;
+      }
+      else if(pFilenamePattern[i] == 0x5C ) /* '\' */
+      {
+         /**
+          * Must be directly followed by asterisk character or question-mark
+          * character.
+          */
+         if (! ((pFilenamePattern[i+1] == '*' ||
+                   pFilenamePattern[i+1] == '?')))
+         {
+            return S_ERROR_BAD_PARAMETERS;
+         }
+      }
+      else if(pFilenamePattern[i] >= 0x7F )
+      {
+         return S_ERROR_BAD_PARAMETERS;
+      }
+   }
+
+   return S_SUCCESS;
+}
+
+
+
+SST_ERROR SST_EXPORT_API SSTOpen(const char* pFilename,
+                                 uint32_t    nFlags,
+                                 uint32_t    nReserved,
+                                 SST_HANDLE* phFile)
+{
+   TEEC_Session*     pSession;
+   TEEC_Result       nError;
+   TEEC_Operation    sOperation;
+   uint32_t          nReturnOrigin;
+   SST_ERROR         nErrorCode = SST_SUCCESS;
+
+   if (phFile == NULL || nReserved != 0)
+   {
+      return SST_ERROR_BAD_PARAMETERS;
+   }
+
+   *phFile = SST_HANDLE_INVALID;
+
+   nErrorCode = static_SSTCheckFileName(pFilename);
+   if (nErrorCode != SST_SUCCESS)
+   {
+      return nErrorCode;
+   }
+
+   pSession = static_SSTGetSession();
+   if (pSession == NULL)
+   {
+      return SST_ERROR_GENERIC;
+   }
+
+   sOperation.paramTypes = TEEC_PARAM_TYPES(TEEC_VALUE_INOUT, TEEC_MEMREF_TEMP_INPUT, TEEC_NONE, TEEC_NONE);
+   sOperation.params[0].value.a = 1;      /* Private storage */
+   sOperation.params[0].value.b = nFlags; /* Access flags */
+   sOperation.params[1].tmpref.buffer = (void*)pFilename;
+   sOperation.params[1].tmpref.size   = strlen(pFilename);
+   nError = TEEC_InvokeCommand(pSession,
+                               SERVICE_SYSTEM_SST_OPEN_COMMAND_ID,   /* commandID */
+                               &sOperation,                 /* IN OUT operation */
+                               &nReturnOrigin               /* OUT returnOrigin, optional */
+                              );
+   if (nError == TEEC_SUCCESS)
+   {
+      *phFile = (SST_HANDLE)sOperation.params[0].value.a;
+   }
+
+   return static_SSTConvertErrorCode(nError);
+}
+
+SST_ERROR SST_EXPORT_API SSTCloseHandle(SST_HANDLE  hFile)
+{
+   TEEC_Session*     pSession;
+   TEEC_Result        nError;
+   TEEC_Operation    sOperation;
+   uint32_t          nReturnOrigin;
+
+   if (hFile == S_HANDLE_NULL)
+   {
+      return SST_SUCCESS;
+   }
+
+   pSession = static_SSTGetSession();
+   if (pSession == NULL)
+   {
+      return SST_ERROR_GENERIC;
+   }
+
+   sOperation.paramTypes = TEEC_PARAM_TYPES(TEEC_VALUE_INPUT, TEEC_NONE, TEEC_NONE, TEEC_NONE);
+   sOperation.params[0].value.a = hFile;
+   nError = TEEC_InvokeCommand(pSession,
+                               SERVICE_SYSTEM_SST_CLOSE_COMMAND_ID, /* commandID */
+                               &sOperation,                  /* IN OUT operation */
+                               &nReturnOrigin            /* OUT returnOrigin, optional */
+                              );
+
+   return static_SSTConvertErrorCode(nError);
+}
+
+SST_ERROR SST_EXPORT_API SSTWrite(SST_HANDLE       hFile,
+                                  const uint8_t*   pBuffer,
+                                  uint32_t         nSize)
+{
+   TEEC_Session*     pSession;
+   TEEC_Result       nError;
+   TEEC_Operation    sOperation;
+   uint32_t          nReturnOrigin;
+
+   if (pBuffer == NULL)
+   {
+      return SST_ERROR_BAD_PARAMETERS;
+   }
+
+   if (nSize == 0)
+   {
+      return SST_SUCCESS;
+   }
+
+   pSession = static_SSTGetSession();
+   if (pSession == NULL)
+   {
+      return SST_ERROR_GENERIC;
+   }
+
+   sOperation.paramTypes = TEEC_PARAM_TYPES(TEEC_VALUE_INPUT, TEEC_MEMREF_TEMP_INPUT, TEEC_NONE, TEEC_NONE);
+   sOperation.params[0].value.a       = hFile;
+   sOperation.params[1].tmpref.buffer = (void*)pBuffer;
+   sOperation.params[1].tmpref.size   = nSize;
+
+   nError = TEEC_InvokeCommand(pSession,
+                               SERVICE_SYSTEM_SST_WRITE_COMMAND_ID, /* commandID */
+                               &sOperation,                  /* IN OUT operation */
+                               &nReturnOrigin            /* OUT returnOrigin, optional */
+                              );
+
+   return static_SSTConvertErrorCode(nError);
+}
+
+
+SST_ERROR SST_EXPORT_API SSTRead(SST_HANDLE   hFile,
+                                 uint8_t*     pBuffer,
+                                 uint32_t     nSize,
+                                 uint32_t*    pnCount)
+{
+   TEEC_Session*     pSession;
+   TEEC_Result       nError;
+   TEEC_Operation    sOperation;
+   uint32_t          nReturnOrigin;
+
+   if ((pBuffer == NULL) || (pnCount == NULL))
+   {
+      return SST_ERROR_BAD_PARAMETERS;
+   }
+   *pnCount = 0;
+
+   pSession = static_SSTGetSession();
+   if (pSession == NULL)
+   {
+      return SST_ERROR_GENERIC;
+   }
+
+   if (nSize == 0)
+   {
+      return SST_SUCCESS;
+   }
+
+   sOperation.paramTypes = TEEC_PARAM_TYPES(TEEC_VALUE_INPUT, TEEC_MEMREF_TEMP_OUTPUT, TEEC_NONE, TEEC_NONE);
+   sOperation.params[0].value.a       = hFile;
+   sOperation.params[1].tmpref.buffer = pBuffer;
+   sOperation.params[1].tmpref.size   = nSize;
+
+   nError = TEEC_InvokeCommand(pSession,
+                               SERVICE_SYSTEM_SST_READ_COMMAND_ID, /* commandID */
+                               &sOperation,                  /* IN OUT operation */
+                               &nReturnOrigin            /* OUT returnOrigin, optional */
+                              );
+
+   *pnCount = sOperation.params[1].tmpref.size; /* The returned buffer size */
+   return static_SSTConvertErrorCode(nError);
+}
+
+SST_ERROR SST_EXPORT_API SSTSeek(SST_HANDLE   hFile,
+                                 int32_t     nOffset,
+                                 SST_WHENCE   whence)
+{
+   TEEC_Session*     pSession;
+   TEEC_Result       nError;
+   TEEC_Operation    sOperation;
+   uint32_t          nReturnOrigin;
+
+   switch(whence)
+   {
+   case SST_SEEK_SET:
+   case SST_SEEK_CUR:
+   case SST_SEEK_END:
+      break;
+   default:
+      return SST_ERROR_BAD_PARAMETERS;
+   }
+
+   pSession = static_SSTGetSession();
+   if (pSession == NULL)
+   {
+      return SST_ERROR_GENERIC;
+   }
+
+   sOperation.paramTypes = TEEC_PARAM_TYPES(TEEC_VALUE_INPUT, TEEC_VALUE_INPUT, TEEC_NONE, TEEC_NONE);
+   sOperation.params[0].value.a = hFile;
+   sOperation.params[1].value.a = nOffset;
+   sOperation.params[1].value.b = (uint32_t)whence;
+
+   nError = TEEC_InvokeCommand(pSession,
+                               SERVICE_SYSTEM_SST_SEEK_COMMAND_ID, /* commandID */
+                               &sOperation,                  /* IN OUT operation */
+                               &nReturnOrigin            /* OUT returnOrigin, optional */
+                              );
+   return static_SSTConvertErrorCode(nError);
+
+}
+
+static SST_ERROR SSTGetOffsetAndSize(SST_HANDLE   hFile, uint32_t* pnOffset, uint32_t* pnSize)
+{
+   TEEC_Session*     pSession;
+   TEEC_Result       nError;
+   TEEC_Operation    sOperation;
+   uint32_t          nReturnOrigin;
+
+   pSession = static_SSTGetSession();
+   if (pSession == NULL)
+   {
+      return SST_ERROR_GENERIC;
+   }
+
+   if (pnOffset == NULL)
+   {
+      return SST_ERROR_BAD_PARAMETERS;
+   }
+
+   sOperation.paramTypes = TEEC_PARAM_TYPES(TEEC_VALUE_INOUT, TEEC_NONE, TEEC_NONE, TEEC_NONE);
+   sOperation.params[0].value.a = (uint32_t)hFile;
+
+   nError = TEEC_InvokeCommand(pSession,
+                               SERVICE_SYSTEM_SST_GET_OFFSET_AND_SIZE_COMMAND_ID, /* commandID */
+                               &sOperation,                  /* IN OUT operation */
+                               &nReturnOrigin            /* OUT returnOrigin, optional */
+                              );
+
+   if (pnOffset != NULL)
+   {
+      *pnOffset = sOperation.params[0].value.a;
+   }
+   if (pnSize != NULL)
+   {
+      *pnSize = sOperation.params[0].value.b;
+   }
+   return static_SSTConvertErrorCode(nError);
+
+}
+
+SST_ERROR SST_EXPORT_API SSTTell(SST_HANDLE   hFile,
+                                 uint32_t*    pnPos)
+{
+   return SSTGetOffsetAndSize(hFile, pnPos, NULL);
+}
+
+SST_ERROR SST_EXPORT_API SSTGetSize(const char*  pFilename,
+                                    uint32_t*    pnSize)
+{
+   TEEC_Session*     pSession;
+   TEEC_Result       nError;
+   TEEC_Operation    sOperation;
+   uint32_t          nReturnOrigin;
+
+   if ((pFilename == NULL) || (pnSize == NULL))
+   {
+      return SST_ERROR_BAD_PARAMETERS;
+   }
+
+   nError = static_SSTCheckFileName(pFilename);
+   if (nError != SST_SUCCESS)
+   {
+      return nError;
+   }
+
+   pSession = static_SSTGetSession();
+   if (pSession == NULL)
+   {
+      return SST_ERROR_GENERIC;
+   }
+
+   sOperation.paramTypes = TEEC_PARAM_TYPES(TEEC_VALUE_INOUT, TEEC_MEMREF_TEMP_INPUT, TEEC_NONE, TEEC_NONE);
+   sOperation.params[0].value.a = 1; /* private storage */
+   sOperation.params[0].value.b = 0;
+   sOperation.params[1].tmpref.buffer = (void*)pFilename;
+   sOperation.params[1].tmpref.size   = strlen(pFilename);
+
+   nError = TEEC_InvokeCommand(pSession,
+                               SERVICE_SYSTEM_SST_GET_SIZE_COMMAND_ID, /* commandID */
+                               &sOperation,                  /* IN OUT operation */
+                               &nReturnOrigin            /* OUT returnOrigin, optional */
+                              );
+
+   *pnSize = sOperation.params[0].value.a;
+   return static_SSTConvertErrorCode(nError);
+}
+
+
+SST_ERROR SST_EXPORT_API SSTEof( SST_HANDLE   hFile,
+                                 bool*        pbEof)
+{
+   uint32_t nOffset;
+   uint32_t nSize;
+   SST_ERROR nError;
+   if (pbEof == NULL)
+      return SST_ERROR_BAD_PARAMETERS;
+   nError = SSTGetOffsetAndSize(hFile, &nOffset, &nSize);
+   if (nError == SST_SUCCESS)
+   {
+      if (nOffset >= nSize)
+      {
+         *pbEof = true;
+      }
+      else
+      {
+         *pbEof = false;
+      }
+   }
+   return nError;
+}
+
+SST_ERROR SST_EXPORT_API SSTCloseAndDelete(SST_HANDLE  hFile)
+{
+   TEEC_Session*     pSession;
+   TEEC_Result       nError;
+   TEEC_Operation    sOperation;
+   uint32_t          nReturnOrigin;
+
+   pSession = static_SSTGetSession();
+   if (pSession == NULL)
+   {
+      return SST_ERROR_GENERIC;
+   }
+
+   sOperation.paramTypes = TEEC_PARAM_TYPES(TEEC_VALUE_INPUT, TEEC_NONE, TEEC_NONE, TEEC_NONE);
+   sOperation.params[0].value.a = hFile;
+
+   nError = TEEC_InvokeCommand(pSession,
+                               SERVICE_SYSTEM_SST_CLOSE_DELETE_COMMAND_ID, /* commandID */
+                               &sOperation,                  /* IN OUT operation */
+                               &nReturnOrigin            /* OUT returnOrigin, optional */
+                              );
+   return static_SSTConvertErrorCode(nError);
+}
+
+SST_ERROR SST_EXPORT_API SSTTruncate(SST_HANDLE hFile, uint32_t nLength)
+{
+   TEEC_Session*     pSession;
+   TEEC_Result       nError;
+   TEEC_Operation    sOperation;
+   uint32_t          nReturnOrigin;
+
+   pSession = static_SSTGetSession();
+   if (pSession == NULL)
+   {
+      return SST_ERROR_GENERIC;
+   }
+
+   sOperation.paramTypes = TEEC_PARAM_TYPES(TEEC_VALUE_INPUT, TEEC_NONE, TEEC_NONE, TEEC_NONE);
+   sOperation.params[0].value.a = hFile;
+   sOperation.params[0].value.b = nLength;
+
+   nError = TEEC_InvokeCommand(pSession,
+                               SERVICE_SYSTEM_SST_TRUNCATE_COMMAND_ID, /* commandID */
+                               &sOperation,                  /* IN OUT operation */
+                               &nReturnOrigin            /* OUT returnOrigin, optional */
+                              );
+   return static_SSTConvertErrorCode(nError);
+}
+
+SST_ERROR SST_EXPORT_API SSTRename(SST_HANDLE hFile,
+                                   const char* pNewFilename)
+{
+   TEEC_Session*     pSession;
+   TEEC_Result       nError;
+   TEEC_Operation    sOperation;
+   uint32_t          nReturnOrigin;
+
+   pSession = static_SSTGetSession();
+   if (pSession == NULL)
+   {
+      return SST_ERROR_GENERIC;
+   }
+
+   if (pNewFilename == NULL)
+   {
+      return SST_ERROR_BAD_PARAMETERS;
+   }
+
+   nError = static_SSTCheckFileName(pNewFilename);
+   if (nError != SST_SUCCESS)
+   {
+      return nError;
+   }
+
+   sOperation.paramTypes = TEEC_PARAM_TYPES(TEEC_VALUE_INPUT, TEEC_MEMREF_TEMP_INPUT, TEEC_NONE, TEEC_NONE);
+   sOperation.params[0].value.a = hFile;
+   sOperation.params[1].tmpref.buffer = (void*)pNewFilename;
+   sOperation.params[1].tmpref.size   = strlen(pNewFilename);
+
+   nError = TEEC_InvokeCommand(pSession,
+                               SERVICE_SYSTEM_SST_RENAME_COMMAND_ID, /* commandID */
+                               &sOperation,                  /* IN OUT operation */
+                               &nReturnOrigin            /* OUT returnOrigin, optional */
+                              );
+      return static_SSTConvertErrorCode(nError);
+}
+
+SST_ERROR SST_EXPORT_API SSTEnumerationStart(const char* pFilenamePattern,
+                                             uint32_t  nReserved1,
+                                             uint32_t  nReserved2,
+                                             SST_HANDLE* phFileEnumeration)
+{
+   TEEC_Session*     pSession;
+   TEEC_Result       nError;
+   TEEC_Operation    sOperation;
+   uint32_t          nReturnOrigin;
+
+   if (nReserved1!=0 || nReserved2!=0)
+   {
+      return SST_ERROR_BAD_PARAMETERS;
+   }
+   if (phFileEnumeration==NULL)
+   {
+      return SST_ERROR_BAD_PARAMETERS;
+   }
+   *phFileEnumeration = SST_HANDLE_INVALID;
+
+   nError = static_SSTCheckPattern(pFilenamePattern);
+   if (nError != SST_SUCCESS)
+   {
+      return nError;
+   }
+
+   pSession = static_SSTGetSession();
+   if (pSession == NULL)
+   {
+      return SST_ERROR_GENERIC;
+   }
+
+   sOperation.paramTypes = TEEC_PARAM_TYPES(TEEC_VALUE_INOUT, TEEC_MEMREF_TEMP_INPUT, TEEC_NONE, TEEC_NONE);
+   sOperation.params[0].value.a = 1;      /* Private storage */
+   sOperation.params[1].tmpref.buffer = (void*)pFilenamePattern;
+   if (pFilenamePattern != NULL)
+   {
+      sOperation.params[1].tmpref.size   = strlen(pFilenamePattern);
+   }
+   else
+   {
+      sOperation.params[1].tmpref.size   = 0;
+   }
+
+   nError = TEEC_InvokeCommand(pSession,
+                               SERVICE_SYSTEM_SST_ENUM_START_COMMAND_ID, /* commandID */
+                               &sOperation,                  /* IN OUT operation */
+                               &nReturnOrigin            /* OUT returnOrigin, optional */
+                              );
+
+   *phFileEnumeration = (SST_HANDLE)sOperation.params[0].value.a;
+   return static_SSTConvertErrorCode(nError);
+}
+
+SST_ERROR SST_EXPORT_API SSTEnumerationCloseHandle(SST_HANDLE hFileEnumeration)
+{
+   TEEC_Session*     pSession;
+   TEEC_Result       nError;
+   TEEC_Operation    sOperation;
+   uint32_t          nReturnOrigin;
+
+   pSession = static_SSTGetSession();
+   if (pSession == NULL)
+   {
+      return SST_ERROR_GENERIC;
+   }
+
+   sOperation.paramTypes = TEEC_PARAM_TYPES(TEEC_VALUE_INPUT, TEEC_NONE, TEEC_NONE, TEEC_NONE);
+   sOperation.params[0].value.a = hFileEnumeration;
+
+   nError = TEEC_InvokeCommand(pSession,
+                               SERVICE_SYSTEM_SST_ENUM_CLOSE_COMMAND_ID, /* commandID */
+                               &sOperation,                  /* IN OUT operation */
+                               &nReturnOrigin                /* OUT returnOrigin, optional */
+                              );
+
+   return static_SSTConvertErrorCode(nError);
+}
+
+SST_ERROR SST_EXPORT_API SSTEnumerationGetNext(SST_HANDLE      hFileEnumeration,
+                                               SST_FILE_INFO**   ppFileInfo)
+
+{
+   TEEC_Session*     pSession;
+   TEEC_Result       nError;
+   TEEC_Operation    sOperation;
+   uint32_t          nReturnOrigin;
+   SST_FILE_INFO*    pInfo = NULL;
+   char              sFilename[SST_MAX_FILENAME];
+
+   if (ppFileInfo==NULL)
+   {
+      return SST_ERROR_BAD_PARAMETERS;
+   }
+
+   pSession = static_SSTGetSession();
+   if (pSession == NULL)
+   {
+      return SST_ERROR_GENERIC;
+   }
+
+   sOperation.paramTypes = TEEC_PARAM_TYPES(TEEC_VALUE_INOUT, TEEC_MEMREF_TEMP_OUTPUT, TEEC_NONE, TEEC_NONE);
+   sOperation.params[0].value.a = hFileEnumeration;
+   sOperation.params[1].tmpref.buffer = sFilename;
+   sOperation.params[1].tmpref.size   = SST_MAX_FILENAME;
+
+   nError = TEEC_InvokeCommand(pSession,
+                               SERVICE_SYSTEM_SST_ENUM_GETNEXT_COMMAND_ID, /* commandID */
+                               &sOperation,                  /* IN OUT operation */
+                               &nReturnOrigin            /* OUT returnOrigin, optional */
+                              );
+
+   if (nError == TEEC_SUCCESS)
+   {
+      if (sOperation.params[1].tmpref.size <= SST_MAX_FILENAME)
+      {
+         pInfo = (SST_FILE_INFO*)malloc(sizeof(SST_FILE_INFO));
+         if (pInfo == NULL)
+         {
+            return SST_ERROR_OUT_OF_MEMORY;
+         }
+         pInfo->pName = (char*)malloc(sOperation.params[1].tmpref.size+1);
+         if (pInfo->pName == NULL)
+         {
+            free(pInfo);
+            return SST_ERROR_OUT_OF_MEMORY;
+         }
+         memcpy(pInfo->pName, sFilename, sOperation.params[1].tmpref.size);
+         /* Add zero terminator */
+         pInfo->pName[sOperation.params[1].tmpref.size] = 0;
+         pInfo->nSize = sOperation.params[0].value.b;
+      }
+   }
+  *ppFileInfo = pInfo;
+   return static_SSTConvertErrorCode(nError);
+ }
+
+SST_ERROR SST_EXPORT_API SSTDestroyFileInfo(SST_FILE_INFO*   pFileInfo)
+{
+   TEEC_Session*  pSession;
+
+   pSession = static_SSTGetSession();
+   if (pSession == NULL)
+   {
+      return SST_ERROR_GENERIC;
+   }
+
+   if (pFileInfo != NULL)
+   {
+      free(pFileInfo->pName);
+      free(pFileInfo);
+   }
+   return SST_SUCCESS;
+}