TEE working now for Android, tested ok with COPS!

* Updated according to review comments
- Added ST-Ericsson copyright headers to all tee files.
- Fixed problem with not using readl/writel when using ioremap.
- Fixed problem with forgetting to do iounmap on special case.
- Fixed incorrect usage when doing copy_to_user when writing to the device.
- Added architecture dependent file for the tee service that calls the secure world.
- Added support for more several inputs (sharedmemory buffers) for tee.
- Added dummy macro to map MT_MEMORY device.
- Fixed memory leak in secure world due to not closing a TEE session correctly from the kernel.
- Now we only copies input buffer from user space for tee.
- Documented structures in tee.h.
- Moved SVP implementation into arch/arm/mach-ux500 folder.
- Added new config flags for ux500 and SVP regarding TEE driver.
- Update mach-ux500/Kconfig:
  - Enable TEE_UX500 by default when using target hardware.
  - Enabel TEE_SVP by default when building simulator.
- Fix the cache sync problem: not request ROM code to clean cache
- ioremap for ICN_BASE, remove static mapping in cpu-db8500.c.
- Fix ioremap of ICN_BASE and do iounmap after use.

ST-Ericsson ID: WP269815

Change-Id: Ie861a90ec790e95fb3992e560512661693548a43
Change-Id: I9d04d0013aba15165d0af2d6313c6ec4bd76a22c
Reviewed-on: http://gerrit.lud.stericsson.com/gerrit/4168
Reviewed-by: Shujuan CHEN <shujuan.chen@stericsson.com>
Tested-by: Shujuan CHEN <shujuan.chen@stericsson.com>

8 files changed, 209 insertions, 311 deletions

diff --git a/drivers/tee/Kconfig b/drivers/tee/Kconfig
index 3056a916600..a452e888d77 100644
--- a/drivers/tee/Kconfig
+++ b/drivers/tee/Kconfig
@@ -9,4 +9,5 @@ config TEE_SUPPORT
 	bool "Trusted Execution Environment Support"
 	default y
 	---help---
-		This enables the Trusted Execution Environment (TEE) support.
+	  This implements the Trusted Execution Environment (TEE) Client
+	  API Specification from GlobalPlatform Device Technology.
diff --git a/drivers/tee/Makefile b/drivers/tee/Makefile
index 7fac2911a8a..b937eb19d72 100644
--- a/drivers/tee/Makefile
+++ b/drivers/tee/Makefile
@@ -4,12 +4,5 @@
 # License terms: GNU General Public License (GPL) version 2
 #
 
-EXTRA_CFLAGS = -O0
-
-ifdef CONFIG_MACH_U5500_SIMULATOR
-	obj-$(CONFIG_TEE_SUPPORT) += tee_service_svp.o
-endif
-
 obj-$(CONFIG_TEE_SUPPORT) += tee_service.o
 obj-$(CONFIG_TEE_SUPPORT) += tee_driver.o
-obj-$(CONFIG_TEE_SUPPORT) += ta/
diff --git a/drivers/tee/ta/Makefile b/drivers/tee/ta/Makefile
deleted file mode 100644
index 70775154e18..00000000000
--- a/drivers/tee/ta/Makefile
+++ /dev/null
@@ -1,8 +0,0 @@
-#
-# Copyright (C) ST-Ericsson SA 2010
-# Author: Martin Hovang (martin.xm.hovang@stericsson.com)
-# License terms: GNU General Public License (GPL) version 2
-#
-
-obj-y += tee_ta_start_modem_svp.o
-
diff --git a/drivers/tee/ta/tee_ta_start_modem.h b/drivers/tee/ta/tee_ta_start_modem.h
deleted file mode 100644
index 648f0e0827e..00000000000
--- a/drivers/tee/ta/tee_ta_start_modem.h
+++ /dev/null
@@ -1,48 +0,0 @@
-/*
- * Data types and interface for TEE application for starting the modem.
- *
- * Copyright (C) ST-Ericsson SA 2010
- * Author: Shujuan Chen <shujuan.chen@stericsson.com>
- * License terms: GNU General Public License (GPL) version 2
- */
-
-#ifndef TEE_TA_START_MODEM_H
-#define TEE_TA_START_MODEM_H
-
-#define COMMAND_ID_START_MODEM 		0x00000001
-
-#define UUID_TEE_TA_START_MODEM_LOW	0x8AD94107
-#define UUID_TEE_TA_START_MODEM_MID	0x6E50
-#define UUID_TEE_TA_START_MODEM_HIGH	0x418E
-#define UUID_TEE_TA_START_MODEM_CLOCKSEQ \
-	{0xB1, 0x14, 0x75, 0x7D, 0x60, 0x21, 0xBD, 0x36}
-
-struct mcore_segment_descr {
-	void *segment;
-	void *hash;
-	size_t size;
-};
-
-struct access_image_descr {
-	void *elf_hdr;
-	void *pgm_hdr_tbl;
-	void *signature;
-	unsigned long nbr_segment;
-	struct mcore_segment_descr *descr;
-};
-
-/* TODO: To be redefined with only info needed by Secure world. */
-struct tee_ta_start_modem {
-	void *access_mem_start;
-	size_t shared_mem_size;
-	size_t access_private_mem_size;
-	struct access_image_descr access_image_descr;
-};
-
-/**
- * This is the function to handle the modem release.
- */
-int tee_ta_start_modem(struct tee_ta_start_modem *data);
-
-#endif
-
diff --git a/drivers/tee/ta/tee_ta_start_modem_svp.c b/drivers/tee/ta/tee_ta_start_modem_svp.c
deleted file mode 100644
index 2ab2adb82b2..00000000000
--- a/drivers/tee/ta/tee_ta_start_modem_svp.c
+++ /dev/null
@@ -1,56 +0,0 @@
-/*
- * Trusted application for starting the modem.
- *
- * Copyright (C) ST-Ericsson SA 2010
- * Author: Shujuan Chen <shujuan.chen@stericsson.com>
- * License terms: GNU General Public License (GPL) version 2
- */
-
-#include <linux/err.h>
-#include <linux/io.h>
-#include <linux/kernel.h>
-#include <linux/elf.h>
-#include <mach/hardware.h>
-
-#include "tee_ta_start_modem.h"
-
-static int reset_modem(unsigned long modem_start_addr)
-{
-	unsigned char *base = ioremap(ACCCON_BASE_SEC, 0x2FF);
-	if (!base)
-		return -ENOMEM;
-
-	printk(KERN_INFO "[reset_modem] Setting modem start address!\n");
-	writel(base + (ACCCON_CPUVEC_RESET_ADDR_OFFSET/sizeof(uint32_t)),
-	       modem_start_addr);
-
-	printk(KERN_INFO "[reset_modem] resetting the modem!\n");
-	writel(base + (ACCCON_ACC_CPU_CTRL_OFFSET/sizeof(uint32_t)), 1);
-
-	iounmap(base);
-
-	return 0;
-}
-
-int tee_ta_start_modem(struct tee_ta_start_modem *data)
-{
-	int ret = 0;
-	struct elfhdr *elfhdr;
-	unsigned char *vaddr;
-
-	vaddr = ioremap((unsigned long)data->access_image_descr.elf_hdr,
-			sizeof(struct elfhdr));
-	if (!vaddr)
-		return -ENOMEM;
-
-	elfhdr = (struct elfhdr *)readl(vaddr);
-	printk(KERN_INFO "Reading in kernel:elfhdr 0x%x:elfhdr->entry=0x%x\n",
-			(uint32_t)elfhdr, (uint32_t)elfhdr->e_entry);
-
-	printk(KERN_INFO "[tee_ta_start_modem] reset modem()...\n");
-	ret = reset_modem(elfhdr->e_entry);
-
-	iounmap(vaddr);
-
-	return ret;
-}
diff --git a/drivers/tee/tee_driver.c b/drivers/tee/tee_driver.c
index 5f9ea5744e1..551c92cc054 100644
--- a/drivers/tee/tee_driver.c
+++ b/drivers/tee/tee_driver.c
@@ -14,12 +14,16 @@
 #include <linux/miscdevice.h>
 #include <linux/uaccess.h>
 #include <linux/tee.h>
+#include <linux/slab.h>
 
 #define TEED_NAME "tee"
 
 #define TEED_STATE_OPEN_DEV 0
 #define TEED_STATE_OPEN_SESSION 1
 
+#define TEEC_MEM_INPUT  0x00000001
+#define TEEC_MEM_OUTPUT 0x00000002
+
 static int tee_open(struct inode *inode, struct file *file);
 static int tee_release(struct inode *inode, struct file *file);
 static int tee_read(struct file *filp, char __user *buffer,
@@ -33,54 +37,160 @@ static inline void set_emsg(struct tee_session *ts, u32 msg)
 	ts->origin = TEED_ORIGIN_DRIVER;
 }
 
+static void reset_session(struct tee_session *ts)
+{
+	ts->state = TEED_STATE_OPEN_DEV;
+	ts->err = TEED_SUCCESS;
+	ts->origin = TEED_ORIGIN_DRIVER;
+	ts->id = 0;
+	ts->ta = NULL;
+	ts->uuid = NULL;
+	ts->cmd = 0;
+	ts->driver_cmd = TEED_OPEN_SESSION;
+	ts->ta_size = 0;
+	ts->op = NULL;
+}
+
 static int copy_ta(struct tee_session *ts,
-		   struct tee_write *buf)
+		   struct tee_session *ku_buffer)
 {
-	ts->ta = kmalloc(buf->ta_size, GFP_KERNEL);
+	ts->ta = kmalloc(ku_buffer->ta_size, GFP_KERNEL);
 	if (ts->ta == NULL) {
-		printk(KERN_INFO "[%s] error, out of memory (ta)\n",
+		pr_err("[%s] error, out of memory (ta)\n",
 		       __func__);
 		set_emsg(ts, TEED_ERROR_OUT_OF_MEMORY);
 		return -ENOMEM;
 	}
 
-	ts->ta_size = buf->ta_size;
+	ts->ta_size = ku_buffer->ta_size;
 
-	memcpy(ts->ta, buf->ta, buf->ta_size);
+	memcpy(ts->ta, ku_buffer->ta, ku_buffer->ta_size);
 	return 0;
 }
 
 static int copy_uuid(struct tee_session *ts,
-		     struct tee_write *buf)
+		     struct tee_session *ku_buffer)
 {
 	ts->uuid = kmalloc(sizeof(struct tee_uuid), GFP_KERNEL);
 
 	if (ts->uuid == NULL) {
-		printk(KERN_INFO "[%s] error, out of memory (uuid)\n",
+		pr_err("[%s] error, out of memory (uuid)\n",
 		       __func__);
 		set_emsg(ts, TEED_ERROR_OUT_OF_MEMORY);
 		return -ENOMEM;
 	}
 
-	memcpy(ts->uuid, buf->uuid, sizeof(struct tee_uuid));
+	memcpy(ts->uuid, ku_buffer->uuid, sizeof(struct tee_uuid));
+
+	return 0;
+}
+
+static inline void free_operation(struct tee_session *ts)
+{
+	int i;
+
+	for (i = 0; i < 4; ++i) {
+		kfree(ts->op->shm[i].buffer);
+		ts->op->shm[i].buffer = NULL;
+	}
+
+	kfree(ts->op);
+	ts->op = NULL;
+}
+
+static inline void memrefs_phys_to_virt(struct tee_session *ts)
+{
+	int i;
+
+	for (i = 0; i < 4; ++i) {
+		if (ts->op->flags & (1 << i)) {
+			ts->op->shm[i].buffer =
+				phys_to_virt((unsigned long)
+					     ts->op->shm[i].buffer);
+		}
+	}
+}
+
+static int copy_memref_to_user(struct tee_operation *op,
+			       struct tee_operation *ubuf_op,
+			       int memref)
+{
+	unsigned long bytes_left;
+
+	bytes_left = copy_to_user(ubuf_op->shm[memref].buffer,
+				  op->shm[memref].buffer,
+				  op->shm[memref].size);
+
+	if (bytes_left != 0) {
+		pr_err("[%s] Failed to copy result to user space (%lu "
+		       "bytes left of buffer).\n", __func__, bytes_left);
+		return bytes_left;
+	}
+
+	bytes_left = put_user(op->shm[memref].size, &ubuf_op->shm[memref].size);
+
+	if (bytes_left != 0) {
+		pr_err("[%s] Failed to copy result to user space (%lu "
+		       "bytes left of size).\n", __func__, bytes_left);
+		return -EINVAL;
+	}
+
+	bytes_left = put_user(op->shm[memref].flags,
+			      &ubuf_op->shm[memref].flags);
+	if (bytes_left != 0) {
+		pr_err("[%s] Failed to copy result to user space (%lu "
+		       "bytes left of flags).\n", __func__, bytes_left);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int copy_memref_to_kernel(struct tee_operation *op,
+				 struct tee_operation *kbuf_op,
+				 int memref)
+{
+	/* Buffer freed in invoke_command if this function fails */
+	op->shm[memref].buffer = kmalloc(kbuf_op->shm[memref].size, GFP_KERNEL);
+
+	if (!op->shm[memref].buffer) {
+		pr_err("[%s] out of memory\n", __func__);
+		return -ENOMEM;
+	}
+
+	/*
+	 * Copy shared memory operations to a local kernel
+	 * buffer if they are of type input.
+	 */
+	if (kbuf_op->shm[memref].flags & TEEC_MEM_INPUT) {
+		memcpy(op->shm[memref].buffer,
+		       kbuf_op->shm[memref].buffer,
+		       kbuf_op->shm[memref].size);
+	}
+
+	op->shm[memref].size = kbuf_op->shm[memref].size;
+	op->shm[memref].flags = kbuf_op->shm[memref].flags;
+
+	/* Secure world expects physical addresses. */
+	op->shm[memref].buffer = (void *)virt_to_phys(op->shm[memref].buffer);
 
 	return 0;
 }
 
 static int open_tee_device(struct tee_session *ts,
-			   struct tee_write *buf)
+			   struct tee_session *ku_buffer)
 {
 	int ret;
 
-	if (buf->id != TEED_OPEN_SESSION) {
+	if (ku_buffer->driver_cmd != TEED_OPEN_SESSION) {
 		set_emsg(ts, TEED_ERROR_BAD_STATE);
 		return -EINVAL;
 	}
 
-	if (buf->ta) {
-		ret = copy_ta(ts, buf);
-	} else if (buf->uuid) {
-		ret = copy_uuid(ts, buf);
+	if (ku_buffer->ta) {
+		ret = copy_ta(ts, ku_buffer);
+	} else if (ku_buffer->uuid) {
+		ret = copy_uuid(ts, ku_buffer);
 	} else {
 		set_emsg(ts, TEED_ERROR_COMMUNICATION);
 		return -EINVAL;
@@ -92,100 +202,67 @@ static int open_tee_device(struct tee_session *ts,
 }
 
 static int invoke_command(struct tee_session *ts,
-			  struct tee_write *kbuf,
-			  struct tee_write __user *ubuf)
+			  struct tee_session *ku_buffer,
+			  struct tee_session __user *u_buffer)
 {
 	int i;
 	int ret = 0;
-	struct tee_operation *ubuf_op =
-		(struct tee_operation *)kbuf->inData;
+	struct tee_operation *kbuf_op =
+		(struct tee_operation *)ku_buffer->op;
 
-	ts->idata = kmalloc(sizeof(struct tee_operation), GFP_KERNEL);
+	ts->op = kmalloc(sizeof(struct tee_operation), GFP_KERNEL);
 
-	if (!ts->idata) {
-		if (ts->idata == NULL) {
-			printk(KERN_INFO "[%s] error, out of memory "
-			       "(idata)\n", __func__);
+	if (!ts->op) {
+		if (ts->op == NULL) {
+			pr_err("[%s] error, out of memory "
+			       "(op)\n", __func__);
 			set_emsg(ts, TEED_ERROR_OUT_OF_MEMORY);
 			ret = -ENOMEM;
 			goto err;
 		}
 	}
 
-	/* Copy memrefs to kernel space */
-	ts->idata->flags = ubuf_op->flags;
-	ts->cmd = kbuf->cmd;
+	/* Copy memrefs to kernel space. */
+	ts->op->flags = kbuf_op->flags;
+	ts->cmd = ku_buffer->cmd;
 
 	for (i = 0; i < 4; ++i) {
-		if (ubuf_op->flags & (1 << i)) {
-			ts->idata->shm[i].buffer =
-				kmalloc(ubuf_op->shm[i].size,
-					GFP_KERNEL);
-
-			if (!ts->idata->shm[i].buffer) {
-				printk(KERN_ERR "[%s] out of memory\n",
-				       __func__);
-				set_emsg(ts, TEED_ERROR_OUT_OF_MEMORY);
-				ret = -ENOMEM;
+		/* We only want to copy memrefs in use. */
+		if (kbuf_op->flags & (1 << i)) {
+			ret = copy_memref_to_kernel(ts->op, kbuf_op, i);
+
+			if (ret)
 				goto err;
-			}
-			/*
-			 * Copy all shared memory operations to a local
-			 * kernel buffer.
-			 */
-			memcpy(ts->idata->shm[i].buffer,
-			       ubuf_op->shm[i].buffer,
-			       ubuf_op->shm[i].size);
-
-			ts->idata->shm[i].size = ubuf_op->shm[i].size;
-			ts->idata->shm[i].flags = ubuf_op->shm[i].flags;
-
-			/* Secure world expects physical addresses. */
-			ts->idata->shm[i].buffer =
-				virt_to_phys(ts->idata->shm[i].buffer);
 		} else {
-			ts->idata->shm[i].buffer = NULL;
-			ts->idata->shm[i].size = 0;
-			ts->idata->shm[i].flags = 0;
+			ts->op->shm[i].buffer = NULL;
+			ts->op->shm[i].size = 0;
+			ts->op->shm[i].flags = 0;
 		}
 	}
 
 	/* To call secure world */
-	if (call_sec_world(ts)) {
+	if (call_sec_world(ts, TEED_INVOKE)) {
 		ret = -EINVAL;
 		goto err;
 	}
 
 	/*
-	 * Convert physical addresses back to virtual address so the kernel can
-	 * free the buffers when closing the session.
+	 * Convert physical addresses back to virtual address so the
+	 * kernel can free the buffers when closing the session.
 	 */
-	for (i = 0; i < 4; ++i) {
-		if (ubuf_op->flags & (1 << i)) {
-			ts->idata->shm[i].buffer =
-				phys_to_virt(ts->idata->shm[i].buffer);
-		}
-	}
+	memrefs_phys_to_virt(ts);
 
 	for (i = 0; i < 4; ++i) {
-		if (ubuf_op->flags & (1 << i)) {
-			u32 bytes_left = copy_to_user(ubuf_op->shm[i].buffer,
-					 ts->idata->shm[i].buffer,
-					 ts->idata->shm[i].size);
-			if (bytes_left != 0) {
-				printk(KERN_ERR "[%s] Failed to copy result to "
-				       "user space (%d bytes left).\n", __func__, bytes_left);
-			}
+		if ((kbuf_op->flags & (1 << i)) &&
+		    (kbuf_op->shm[i].flags & TEEC_MEM_OUTPUT)) {
+			struct tee_operation *ubuf_op =
+				(struct tee_operation *)u_buffer->op;
+
+			ret = copy_memref_to_user(ts->op, ubuf_op, i);
 		}
 	}
-
 err:
-	if (ret) {
-		for (i = 0; i < 4; ++i)
-			kfree(ts->idata->shm[i].buffer);
-
-		kfree(ts->idata);
-	}
+	free_operation(ts);
 
 	return ret;
 }
@@ -194,23 +271,22 @@ static int tee_open(struct inode *inode, struct file *filp)
 {
 	struct tee_session *ts;
 
-	filp->private_data = kmalloc(sizeof(struct tee_session), GFP_KERNEL);
+	filp->private_data = kmalloc(sizeof(struct tee_session),
+				     GFP_KERNEL);
 
 	if (filp->private_data == NULL)
 		return -ENOMEM;
 
 	ts = (struct tee_session *) (filp->private_data);
-	ts->state = TEED_STATE_OPEN_DEV;
-	ts->err = TEED_SUCCESS;
-	ts->origin = TEED_ORIGIN_DRIVER;
+
+	reset_session(ts);
+
 	ts->sync = kmalloc(sizeof(struct mutex), GFP_KERNEL);
+
+	if (!ts->sync)
+		return -ENOMEM;
+
 	mutex_init(ts->sync);
-	ts->ta = NULL;
-	ts->uuid = NULL;
-	ts->id = 0;
-	ts->idata = NULL;
-	ts->ta_size = 0;
-	ts->err = TEED_SUCCESS;
 
 	return 0;
 }
@@ -222,25 +298,26 @@ static int tee_release(struct inode *inode, struct file *filp)
 
 	ts = (struct tee_session *) (filp->private_data);
 
-	if (ts != NULL) {
-		if (ts->idata) {
-			for (i = 0; i < 4; ++i) {
-				kfree(ts->idata->shm[i].buffer);
-				ts->idata->shm[i].buffer = NULL;
-			}
+	if (ts == NULL)
+			goto no_ts;
 
+	if (ts->op) {
+		for (i = 0; i < 4; ++i) {
+			kfree(ts->op->shm[i].buffer);
+			ts->op->shm[i].buffer = NULL;
 		}
+	}
 
-		kfree(ts->idata);
-		ts->idata = NULL;
+	kfree(ts->op);
+	ts->op = NULL;
 
-		kfree(ts->sync);
-		ts->sync = NULL;
+	kfree(ts->sync);
+	ts->sync = NULL;
 
-		kfree(ts->ta);
-		ts->ta = NULL;
-	}
+	kfree(ts->ta);
+	ts->ta = NULL;
 
+no_ts:
 	kfree(filp->private_data);
 	filp->private_data = NULL;
 
@@ -258,7 +335,7 @@ static int tee_read(struct file *filp, char __user *buffer,
 	struct tee_session *ts;
 
 	if (length != sizeof(struct tee_read)) {
-		printk(KERN_INFO "[%s] error, incorrect input length\n",
+		pr_err("[%s] error, incorrect input length\n",
 		       __func__);
 		return -EINVAL;
 	}
@@ -266,20 +343,20 @@ static int tee_read(struct file *filp, char __user *buffer,
 	ts = (struct tee_session *) (filp->private_data);
 
 	if (ts == NULL || ts->sync == NULL) {
-		printk(KERN_INFO "[%s] error, private_data not initialized\n",
-		       __func__);
+		pr_err("[%s] error, private_data not "
+		       "initialized\n", __func__);
 		return -EINVAL;
 	}
 
 	mutex_lock(ts->sync);
 
-	buf.id = ts->err;
+	buf.err = ts->err;
 	buf.origin = ts->origin;
 
 	mutex_unlock(ts->sync);
 
 	if (copy_to_user(buffer, &buf, length)) {
-		printk(KERN_INFO "[%s] error, copy_to_user failed!\n",
+		pr_err("[%s] error, copy_to_user failed!\n",
 		       __func__);
 		return -EINVAL;
 	}
@@ -293,27 +370,27 @@ static int tee_read(struct file *filp, char __user *buffer,
 static int tee_write(struct file *filp, const char __user *buffer,
 		     size_t length, loff_t *offset)
 {
-	struct tee_write buf;
+	struct tee_session ku_buffer;
 	struct tee_session *ts;
 	int ret = length;
 
-	if (length != sizeof(struct tee_write)) {
-		printk(KERN_INFO "[%s] error, incorrect input length\n",
+	if (length != sizeof(struct tee_session)) {
+		pr_err("[%s] error, incorrect input length\n",
 		       __func__);
 		return -EINVAL;
 	}
 
-	if (copy_from_user(&buf, buffer, length)) {
-		printk(KERN_INFO "[%s] error, tee_session copy_from_user "
-		       "failed\n", __func__);
+	if (copy_from_user(&ku_buffer, buffer, length)) {
+		pr_err("[%s] error, tee_session "
+		       "copy_from_user failed\n", __func__);
 		return -EINVAL;
 	}
 
 	ts = (struct tee_session *) (filp->private_data);
 
 	if (ts == NULL || ts->sync == NULL) {
-		printk(KERN_INFO "[%s] error, private_data not initialized\n",
-		       __func__);
+		pr_err("[%s] error, private_data not "
+		       "initialized\n", __func__);
 		return -EINVAL;
 	}
 
@@ -321,22 +398,25 @@ static int tee_write(struct file *filp, const char __user *buffer,
 
 	switch (ts->state) {
 	case TEED_STATE_OPEN_DEV:
-		ret = open_tee_device(ts, &buf);
+		ret = open_tee_device(ts, &ku_buffer);
 		break;
 
 	case TEED_STATE_OPEN_SESSION:
-		switch (buf.id) {
+		switch (ku_buffer.driver_cmd) {
 		case TEED_INVOKE:
-			ret = invoke_command(ts, &buf,
-					     (struct tee_write *)buffer);
+			ret = invoke_command(ts, &ku_buffer,
+					     (struct tee_session *)buffer);
 			break;
 
 		case TEED_CLOSE_SESSION:
 			/* no caching implemented yet... */
+			if (call_sec_world(ts, TEED_CLOSE_SESSION))
+				ret = -EINVAL;
+
 			kfree(ts->ta);
 			ts->ta = NULL;
 
-			ts->state = TEED_STATE_OPEN_DEV;
+			reset_session(ts);
 			break;
 
 		default:
@@ -345,14 +425,14 @@ static int tee_write(struct file *filp, const char __user *buffer,
 		}
 		break;
 	default:
-		printk(KERN_INFO "[%s] unknown state\n", __func__);
+		pr_err("[%s] unknown state\n", __func__);
 		set_emsg(ts, TEED_ERROR_BAD_STATE);
 		ret = -EINVAL;
 	}
 
 	/*
-	 * We expect that ret has value zero when reaching the end here. If
-	 * it has any other value some error must have occured.
+	 * We expect that ret has value zero when reaching the end here.
+	 * If it has any other value some error must have occured.
 	 */
 	if (!ret)
 		ret = length;
@@ -385,8 +465,8 @@ static int __init tee_init(void)
 	err = misc_register(&tee_dev);
 
 	if (err) {
-		printk(KERN_ERR "[%s] error %d adding character device TEE\n",
-		       __func__, err);
+		pr_err("[%s] error %d adding character device "
+		       "TEE\n", __func__, err);
 	}
 
 	return err;
diff --git a/drivers/tee/tee_service.c b/drivers/tee/tee_service.c
index 47dcdcd5f0d..b01e9d0ac39 100644
--- a/drivers/tee/tee_service.c
+++ b/drivers/tee/tee_service.c
@@ -7,10 +7,11 @@
  */
 #include <linux/kernel.h>
 #include <linux/tee.h>
+#include <linux/device.h>
 
-int __weak call_sec_world(struct tee_session *ts)
+int __weak call_sec_world(struct tee_session *ts, int sec_cmd)
 {
-	printk(KERN_INFO "[%s] Generic call_sec_world called!\n", __func__);
+	pr_info("[%s] Generic call_sec_world called!\n", __func__);
 
 	return 0;
 }
diff --git a/drivers/tee/tee_service_svp.c b/drivers/tee/tee_service_svp.c
deleted file mode 100644
index 2f3caa80732..00000000000
--- a/drivers/tee/tee_service_svp.c
+++ /dev/null
@@ -1,65 +0,0 @@
-/*
- * TEE service to handle the calls to trusted applications in SVP.
- *
- * Copyright (C) ST-Ericsson SA 2010
- * Author: Shujuan Chen <shujuan.chen@stericsson.com>
- * License terms: GNU General Public License (GPL) version 2
- */
-
-#include <linux/kernel.h>
-#include <linux/string.h>
-#include <linux/tee.h>
-#include <linux/err.h>
-#include "ta/tee_ta_start_modem.h"
-
-static int cmp_uuid_start_modem(struct tee_uuid *uuid)
-{
-	int ret = -EINVAL;
-
-	if (uuid == NULL)
-		return ret;
-
-	/* This handles the calls to TA for start the modem */
-	if ((uuid->timeLow == UUID_TEE_TA_START_MODEM_LOW) &&
-	    (uuid->timeMid == UUID_TEE_TA_START_MODEM_MID) &&
-	    (uuid->timeHiAndVersion == UUID_TEE_TA_START_MODEM_HIGH)) {
-
-		uint8_t clockSeqAndNode[TEE_UUID_CLOCK_SIZE] =
-			UUID_TEE_TA_START_MODEM_CLOCKSEQ;
-
-		ret = memcmp(uuid->clockSeqAndNode, clockSeqAndNode,
-			       TEE_UUID_CLOCK_SIZE);
-	}
-
-	return ret;
-}
-
-int call_sec_world(struct tee_session *ts)
-{
-	int ret = -EINVAL;
-
-	printk(KERN_INFO "call_sec_world() is called!\n");
-
-	if (ts == NULL)
-		return ret;
-
-	if (!cmp_uuid_start_modem(ts->uuid)) {
-		switch (ts->cmd) {
-		case COMMAND_ID_START_MODEM:
-			ret = tee_ta_start_modem((struct tee_ta_start_modem *)
-						 ts->idata);
-			if (ret) {
-				ts->err = TEED_ERROR_GENERIC;
-				ts->origin = TEED_ORIGIN_TEE_APPLICATION;
-				printk(KERN_INFO
-				       "tee_ta_start_modem() failed!\n");
-			}
-		default:
-			break;
-		}
-	}
-
-	/* TODO: to handle more trusted applications. */
-
-	return ret;
-}