SMC: Initial release of libtf_crypto_sst.so

libtf_crypto_sst.so is a single shared object where the Normal
World APIs are implemented for Secure Storage, Cryptographic, and
Monotonic Counter Facility. These are the "Core Secure Services"

The crypto core secure service exposing crypto functionality
for client applications supports exactly the same mechanisms as the
ones exposed by the Internal PKCS#11 SSDI Crypto API.

In order to optimize some cryptographic operations from the External
cryptoki API, the SMC makes use of the Cryptographic HW Accelerators
in public mode (in the SMC driver), through the Cryptoki Update
Shortcut (CUS) and the aggressive mode.

Change-Id: I3ebf83a0c84fb33f0f779a6224032b7aad06ef45
Signed-off-by: Bryan Buckley <bryan.buckley@ti.com>
Signed-off-by: Trusted Logic <smc_support@trusted-logic.com>

12 files changed, 4610 insertions, 0 deletions

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;
+}