diff --git a/include/lib/aarch64/smcc_helpers.h b/include/lib/aarch64/smcc_helpers.h
index 6e63383..7fae7d8 100644
--- a/include/lib/aarch64/smcc_helpers.h
+++ b/include/lib/aarch64/smcc_helpers.h
@@ -56,6 +56,22 @@
 	write_ctx_reg(get_gpregs_ctx(_h), CTX_GPREG_X3, (_x3));	\
 	SMC_RET3(_h, (_x0), (_x1), (_x2));			\
 }
+#define SMC_RET5(_h, _x0, _x1, _x2, _x3, _x4)	{		\
+	write_ctx_reg(get_gpregs_ctx(_h), CTX_GPREG_X4, (_x4));	\
+	SMC_RET4(_h, (_x0), (_x1), (_x2), (_x3));		\
+}
+#define SMC_RET6(_h, _x0, _x1, _x2, _x3, _x4, _x5)	{	\
+	write_ctx_reg(get_gpregs_ctx(_h), CTX_GPREG_X5, (_x5));	\
+	SMC_RET5(_h, (_x0), (_x1), (_x2), (_x3), (_x4));	\
+}
+#define SMC_RET7(_h, _x0, _x1, _x2, _x3, _x4, _x5, _x6)	{	\
+	write_ctx_reg(get_gpregs_ctx(_h), CTX_GPREG_X6, (_x6));	\
+	SMC_RET6(_h, (_x0), (_x1), (_x2), (_x3), (_x4), (_x5));	\
+}
+#define SMC_RET8(_h, _x0, _x1, _x2, _x3, _x4, _x5, _x6, _x7) {	\
+	write_ctx_reg(get_gpregs_ctx(_h), CTX_GPREG_X7, (_x7));	\
+	SMC_RET7(_h, (_x0), (_x1), (_x2), (_x3), (_x4), (_x5), (_x6));	\
+}
 
 /*
  * Convenience macros to access general purpose registers using handle provided
diff --git a/services/spd/tlkd/tlkd_main.c b/services/spd/tlkd/tlkd_main.c
index 0fddcb2..5ebe4fd 100644
--- a/services/spd/tlkd/tlkd_main.c
+++ b/services/spd/tlkd/tlkd_main.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2016, ARM Limited and Contributors. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions are met:
@@ -58,6 +58,11 @@
  ******************************************************************************/
 tlk_context_t tlk_ctx;
 
+/*******************************************************************************
+ * CPU number on which TLK booted up
+ ******************************************************************************/
+static int boot_cpu;
+
 /* TLK UID: RFC-4122 compliant UUID (version-5, sha-1) */
 DEFINE_SVC_UUID(tlk_uuid,
 		0xbd11e9c9, 0x2bba, 0x52ee, 0xb1, 0x72,
@@ -133,6 +138,12 @@
 	cm_init_my_context(tlk_entry_point);
 
 	/*
+	 * TLK runs only on a single CPU. Store the value of the boot
+	 * CPU for sanity checking later.
+	 */
+	boot_cpu = plat_my_core_pos();
+
+	/*
 	 * Arrange for an entry into the test secure payload.
 	 */
 	return tlkd_synchronous_sp_entry(&tlk_ctx);
@@ -163,8 +174,8 @@
 	/* Passing a NULL context is a critical programming error */
 	assert(handle);
 
-	/* These SMCs are only supported by CPU0 */
-	if ((read_mpidr() & MPIDR_CPU_MASK) != 0)
+	/* These SMCs are only supported by a single CPU */
+	if (boot_cpu != plat_my_core_pos())
 		SMC_RET1(handle, SMC_UNK);
 
 	/* Determine which security state this SMC originated from */
diff --git a/services/spd/trusty/smcall.h b/services/spd/trusty/smcall.h
index a1d91e5..2abcee6 100644
--- a/services/spd/trusty/smcall.h
+++ b/services/spd/trusty/smcall.h
@@ -94,5 +94,6 @@
 
 #define SMC_SC_VDEV_RESET	  SMC_STDCALL_NR(SMC_ENTITY_TRUSTED_OS, 23)
 #define SMC_SC_VDEV_KICK_VQ	  SMC_STDCALL_NR(SMC_ENTITY_TRUSTED_OS, 24)
+#define SMC_SC_SET_ROT_PARAMS	  SMC_STDCALL_NR(SMC_ENTITY_TRUSTED_OS, 65535)
 
 #endif /* __LIB_SM_SMCALL_H */
diff --git a/services/spd/trusty/trusty.c b/services/spd/trusty/trusty.c
index b21ce71..9c9de91 100644
--- a/services/spd/trusty/trusty.c
+++ b/services/spd/trusty/trusty.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2016, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2016-2017, ARM Limited and Contributors. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions are met:
@@ -28,7 +28,8 @@
  * POSSIBILITY OF SUCH DAMAGE.
  */
 
-#include <assert.h>
+#include <arch_helpers.h>
+#include <assert.h> /* for context_mgmt.h */
 #include <bl_common.h>
 #include <bl31.h>
 #include <context_mgmt.h>
@@ -41,8 +42,15 @@
 #include "smcall.h"
 #include "sm_err.h"
 
+/* macro to check if Hypervisor is enabled in the HCR_EL2 register */
+#define HYP_ENABLE_FLAG		0x286001
+
+/* length of Trusty's input parameters (in bytes) */
+#define TRUSTY_PARAMS_LEN_BYTES	(4096*2)
+
 struct trusty_stack {
 	uint8_t space[PLATFORM_STACK_SIZE] __aligned(16);
+	uint32_t end;
 };
 
 struct trusty_cpu_ctx {
@@ -65,31 +73,60 @@
 	uint64_t	r1;
 	uint64_t	r2;
 	uint64_t	r3;
+	uint64_t	r4;
+	uint64_t	r5;
+	uint64_t	r6;
+	uint64_t	r7;
 };
 
 struct trusty_cpu_ctx trusty_cpu_ctx[PLATFORM_CORE_COUNT];
 
 struct args trusty_init_context_stack(void **sp, void *new_stack);
-struct args trusty_context_switch_helper(void **sp, uint64_t r0, uint64_t r1,
-					 uint64_t r2, uint64_t r3);
+struct args trusty_context_switch_helper(void **sp, void *smc_params);
+
+static uint32_t current_vmid;
 
 static struct trusty_cpu_ctx *get_trusty_ctx(void)
 {
 	return &trusty_cpu_ctx[plat_my_core_pos()];
 }
 
+static uint32_t is_hypervisor_mode(void)
+{
+	uint64_t hcr = read_hcr();
+
+	return !!(hcr & HYP_ENABLE_FLAG);
+}
+
 static struct args trusty_context_switch(uint32_t security_state, uint64_t r0,
 					 uint64_t r1, uint64_t r2, uint64_t r3)
 {
 	struct args ret;
 	struct trusty_cpu_ctx *ctx = get_trusty_ctx();
+	struct trusty_cpu_ctx *ctx_smc;
 
 	assert(ctx->saved_security_state != security_state);
 
+	ret.r7 = 0;
+	if (is_hypervisor_mode()) {
+		/* According to the ARM DEN0028A spec, VMID is stored in x7 */
+		ctx_smc = cm_get_context(NON_SECURE);
+		assert(ctx_smc);
+		ret.r7 = SMC_GET_GP(ctx_smc, CTX_GPREG_X7);
+	}
+	/* r4, r5, r6 reserved for future use. */
+	ret.r6 = 0;
+	ret.r5 = 0;
+	ret.r4 = 0;
+	ret.r3 = r3;
+	ret.r2 = r2;
+	ret.r1 = r1;
+	ret.r0 = r0;
+
 	cm_el1_sysregs_context_save(security_state);
 
 	ctx->saved_security_state = security_state;
-	ret = trusty_context_switch_helper(&ctx->saved_sp, r0, r1, r2, r3);
+	ret = trusty_context_switch_helper(&ctx->saved_sp, &ret);
 
 	assert(ctx->saved_security_state == !security_state);
 
@@ -200,11 +237,25 @@
 			 uint64_t flags)
 {
 	struct args ret;
+	uint32_t vmid = 0;
+	entry_point_info_t *ep_info = bl31_plat_get_next_image_ep_info(SECURE);
+
+	/*
+	 * Return success for SET_ROT_PARAMS if Trusty is not present, as
+	 * Verified Boot is not even supported and returning success here
+	 * would not compromise the boot process.
+	 */
+	if (!ep_info && (smc_fid == SMC_SC_SET_ROT_PARAMS)) {
+		SMC_RET1(handle, 0);
+	} else if (!ep_info) {
+		SMC_RET1(handle, SMC_UNK);
+	}
 
 	if (is_caller_secure(flags)) {
 		if (smc_fid == SMC_SC_NS_RETURN) {
 			ret = trusty_context_switch(SECURE, x1, 0, 0, 0);
-			SMC_RET4(handle, ret.r0, ret.r1, ret.r2, ret.r3);
+			SMC_RET8(handle, ret.r0, ret.r1, ret.r2, ret.r3,
+				 ret.r4, ret.r5, ret.r6, ret.r7);
 		}
 		INFO("%s (0x%x, 0x%lx, 0x%lx, 0x%lx, 0x%lx, %p, %p, 0x%lx) \
 		     cpu %d, unknown smc\n",
@@ -220,8 +271,21 @@
 		case SMC_FC_FIQ_EXIT:
 			return trusty_fiq_exit(handle, x1, x2, x3);
 		default:
+			if (is_hypervisor_mode())
+				vmid = SMC_GET_GP(handle, CTX_GPREG_X7);
+
+			if ((current_vmid != 0) && (current_vmid != vmid)) {
+				/* This message will cause SMC mechanism
+				 * abnormal in multi-guest environment.
+				 * Change it to WARN in case you need it.
+				 */
+				VERBOSE("Previous SMC not finished.\n");
+				SMC_RET1(handle, SM_ERR_BUSY);
+			}
+			current_vmid = vmid;
 			ret = trusty_context_switch(NON_SECURE, smc_fid, x1,
 				x2, x3);
+			current_vmid = 0;
 			SMC_RET1(handle, ret.r0);
 		}
 	}
@@ -231,6 +295,7 @@
 {
 	void el3_exit(void);
 	entry_point_info_t *ep_info;
+	struct args zero_args = {0};
 	struct trusty_cpu_ctx *ctx = get_trusty_ctx();
 	uint32_t cpu = plat_my_core_pos();
 	int reg_width = GET_RW(read_ctx_reg(get_el3state_ctx(&ctx->cpu_ctx),
@@ -262,9 +327,9 @@
 	cm_set_next_eret_context(SECURE);
 
 	ctx->saved_security_state = ~0; /* initial saved state is invalid */
-	trusty_init_context_stack(&ctx->saved_sp, &ctx->secure_stack);
+	trusty_init_context_stack(&ctx->saved_sp, &ctx->secure_stack.end);
 
-	trusty_context_switch_helper(&ctx->saved_sp, 0, 0, 0, 0);
+	trusty_context_switch_helper(&ctx->saved_sp, &zero_args);
 
 	cm_el1_sysregs_context_restore(NON_SECURE);
 	cm_set_next_eret_context(NON_SECURE);
@@ -332,43 +397,35 @@
 static int32_t trusty_setup(void)
 {
 	entry_point_info_t *ep_info;
-	uint32_t instr;
 	uint32_t flags;
 	int ret;
-	int aarch32 = 0;
 
+	/* Get trusty's entry point info */
 	ep_info = bl31_plat_get_next_image_ep_info(SECURE);
 	if (!ep_info) {
 		INFO("Trusty image missing.\n");
 		return -1;
 	}
 
-	instr = *(uint32_t *)ep_info->pc;
-
-	if (instr >> 24 == 0xea) {
-		INFO("trusty: Found 32 bit image\n");
-		aarch32 = 1;
-	} else if (instr >> 8 == 0xd53810) {
-		INFO("trusty: Found 64 bit image\n");
-	} else {
-		INFO("trusty: Found unknown image, 0x%x\n", instr);
-	}
-
+	/* Trusty runs in AARCH64 mode */
 	SET_PARAM_HEAD(ep_info, PARAM_EP, VERSION_1, SECURE | EP_ST_ENABLE);
-	if (!aarch32)
-		ep_info->spsr = SPSR_64(MODE_EL1, MODE_SP_ELX,
-					DISABLE_ALL_EXCEPTIONS);
-	else
-		ep_info->spsr = SPSR_MODE32(MODE32_svc, SPSR_T_ARM,
-					    SPSR_E_LITTLE,
-					    DAIF_FIQ_BIT |
-					    DAIF_IRQ_BIT |
-					    DAIF_ABT_BIT);
+	ep_info->spsr = SPSR_64(MODE_EL1, MODE_SP_ELX, DISABLE_ALL_EXCEPTIONS);
+
+	/*
+	 * arg0 = TZDRAM aperture available for BL32
+	 * arg1 = BL32 boot params
+	 * arg2 = BL32 boot params length
+	 */
+	ep_info->args.arg1 = ep_info->args.arg2;
+	ep_info->args.arg2 = TRUSTY_PARAMS_LEN_BYTES;
 
+	/* register init handler */
 	bl31_register_bl32_init(trusty_init);
 
+	/* register power management hooks */
 	psci_register_spd_pm_hook(&trusty_pm);
 
+	/* register interrupt handler */
 	flags = 0;
 	set_interrupt_rm_flag(flags, NON_SECURE);
 	ret = register_interrupt_type_handler(INTR_TYPE_S_EL1,
diff --git a/services/spd/trusty/trusty_helpers.S b/services/spd/trusty/trusty_helpers.S
index 9bbb044..7e459ac 100644
--- a/services/spd/trusty/trusty_helpers.S
+++ b/services/spd/trusty/trusty_helpers.S
@@ -60,8 +60,20 @@
 	pop	x21, x22
 	pop	x19, x20
 	pop	x8, xzr
-	stp	x1, x2, [x8]
-	stp	x3, x4, [x8, #16]
+
+        ldr     x2, [x1]
+        ldr     x3, [x1, #0x08]
+        ldr     x4, [x1, #0x10]
+        ldr     x5, [x1, #0x18]
+        ldr     x6, [x1, #0x20]
+        ldr     x7, [x1, #0x28]
+        ldr     x10, [x1, #0x30]
+        ldr     x11, [x1, #0x38]
+
+        stp     x2, x3, [x8]
+        stp     x4, x5, [x8, #16]
+        stp     x6, x7, [x8, #32]
+        stp     x10, x11, [x8, #48]
 
 	ret
 endfunc trusty_context_switch_helper
diff --git a/tools/fiptool/fiptool.c b/tools/fiptool/fiptool.c
index f3f831b..542a946 100644
--- a/tools/fiptool/fiptool.c
+++ b/tools/fiptool/fiptool.c
@@ -80,8 +80,6 @@
 
 static image_desc_t *image_desc_head;
 static size_t nr_image_descs;
-static image_t *image_head;
-static size_t nr_images;
 static uuid_t uuid_null = { 0 };
 static int verbose;
 
@@ -200,6 +198,7 @@
 	free(desc->name);
 	free(desc->cmdline_name);
 	free(desc->action_arg);
+	free(desc->image);
 	free(desc);
 }
 
@@ -244,74 +243,6 @@
 	}
 }
 
-static void add_image(image_t *image)
-{
-	image_t **p = &image_head;
-
-	while (*p)
-		p = &(*p)->next;
-
-	assert(*p == NULL);
-	*p = image;
-
-	nr_images++;
-}
-
-static void replace_image(image_t *image)
-{
-	image_t **p = &image_head;
-
-	while (*p) {
-		if (!memcmp(&(*p)->toc_e.uuid, &image->toc_e.uuid,
-			    sizeof(image->toc_e.uuid)))
-			break;
-		p = &(*p)->next;
-	}
-
-	assert(*p != NULL);
-
-	image->next = (*p)->next;
-	*p = image;
-}
-
-static void free_image(image_t *image)
-{
-	free(image->buffer);
-	free(image);
-}
-
-static void remove_image(image_t *image)
-{
-	image_t *tmp, **p = &image_head;
-
-	while (*p) {
-		if (*p == image)
-			break;
-		p = &(*p)->next;
-	}
-
-	assert(*p != NULL);
-
-	tmp = *p;
-	*p = tmp->next;
-	free_image(tmp);
-
-	nr_images--;
-}
-
-static void free_images(void)
-{
-	image_t *image = image_head, *tmp;
-
-	while (image != NULL) {
-		tmp = image->next;
-		free_image(image);
-		image = tmp;
-		nr_images--;
-	}
-	assert(nr_images == 0);
-}
-
 static image_desc_t *lookup_image_desc_from_uuid(const uuid_t *uuid)
 {
 	image_desc_t *desc;
@@ -332,16 +263,6 @@
 	return NULL;
 }
 
-static image_t *lookup_image_from_uuid(const uuid_t *uuid)
-{
-	image_t *image;
-
-	for (image = image_head; image != NULL; image = image->next)
-		if (!memcmp(&image->toc_e.uuid, uuid, sizeof(*uuid)))
-			return image;
-	return NULL;
-}
-
 static void uuid_to_str(char *s, size_t len, const uuid_t *u)
 {
 	assert(len >= (_UUID_STR_LEN + 1));
@@ -457,7 +378,8 @@
 			add_image_desc(desc);
 		}
 
-		add_image(image);
+		assert(desc->image == NULL);
+		desc->image = image;
 
 		toc_entry++;
 	}
@@ -542,7 +464,7 @@
 
 static int info_cmd(int argc, char *argv[])
 {
-	image_t *image;
+	image_desc_t *desc;
 	fip_toc_header_t toc_header;
 
 	if (argc != 2)
@@ -560,11 +482,11 @@
 		    (unsigned long long)toc_header.flags);
 	}
 
-	for (image = image_head; image != NULL; image = image->next) {
-		image_desc_t *desc;
+	for (desc = image_desc_head; desc != NULL; desc = desc->next) {
+		image_t *image = desc->image;
 
-		desc = lookup_image_desc_from_uuid(&image->toc_e.uuid);
-		assert(desc != NULL);
+		if (image == NULL)
+			continue;
 		printf("%s: offset=0x%llX, size=0x%llX, cmdline=\"--%s\"",
 		       desc->name,
 		       (unsigned long long)image->toc_e.offset_address,
@@ -580,7 +502,6 @@
 		putchar('\n');
 	}
 
-	free_images();
 	return 0;
 }
 
@@ -593,11 +514,16 @@
 static int pack_images(const char *filename, uint64_t toc_flags, unsigned long align)
 {
 	FILE *fp;
-	image_t *image;
+	image_desc_t *desc;
 	fip_toc_header_t *toc_header;
 	fip_toc_entry_t *toc_entry;
 	char *buf;
 	uint64_t entry_offset, buf_size, payload_size = 0;
+	size_t nr_images = 0;
+
+	for (desc = image_desc_head; desc != NULL; desc = desc->next)
+		if (desc->image != NULL)
+			nr_images++;
 
 	buf_size = sizeof(fip_toc_header_t) +
 	    sizeof(fip_toc_entry_t) * (nr_images + 1);
@@ -614,7 +540,11 @@
 	toc_entry = (fip_toc_entry_t *)(toc_header + 1);
 
 	entry_offset = buf_size;
-	for (image = image_head; image != NULL; image = image->next) {
+	for (desc = image_desc_head; desc != NULL; desc = desc->next) {
+		image_t *image = desc->image;
+
+		if (image == NULL)
+			continue;
 		payload_size += image->toc_e.size;
 		entry_offset = (entry_offset + align - 1) & ~(align - 1);
 		image->toc_e.offset_address = entry_offset;
@@ -640,7 +570,11 @@
 	if (verbose)
 		log_dbgx("Payload size: %zu bytes", payload_size);
 
-	for (image = image_head; image != NULL; image = image->next) {
+	for (desc = image_desc_head; desc != NULL; desc = desc->next) {
+		image_t *image = desc->image;
+
+		if (image == NULL)
+			continue;
 		if (fseek(fp, image->toc_e.offset_address, SEEK_SET))
 			log_errx("Failed to set file position");
 
@@ -664,26 +598,26 @@
 
 	/* Add or replace images in the FIP file. */
 	for (desc = image_desc_head; desc != NULL; desc = desc->next) {
-		image_t *new_image, *old_image;
+		image_t *image;
 
 		if (desc->action != DO_PACK)
 			continue;
 
-		new_image = read_image_from_file(&desc->uuid,
+		image = read_image_from_file(&desc->uuid,
 		    desc->action_arg);
-		old_image = lookup_image_from_uuid(&desc->uuid);
-		if (old_image != NULL) {
+		if (desc->image != NULL) {
 			if (verbose) {
 				log_dbgx("Replacing %s with %s",
 				    desc->cmdline_name,
 				    desc->action_arg);
 			}
-			replace_image(new_image);
+			free(desc->image);
+			desc->image = image;
 		} else {
 			if (verbose)
 				log_dbgx("Adding image %s",
 				    desc->action_arg);
-			add_image(new_image);
+			desc->image = image;
 		}
 	}
 }
@@ -808,7 +742,6 @@
 	update_fip();
 
 	pack_images(argv[0], toc_flags, align);
-	free_images();
 	return 0;
 }
 
@@ -922,7 +855,6 @@
 	update_fip();
 
 	pack_images(outfile, toc_flags, align);
-	free_images();
 	return 0;
 }
 
@@ -1028,7 +960,7 @@
 	/* Unpack all specified images. */
 	for (desc = image_desc_head; desc != NULL; desc = desc->next) {
 		char file[PATH_MAX];
-		image_t *image;
+		image_t *image = desc->image;
 
 		if (!unpack_all && desc->action != DO_UNPACK)
 			continue;
@@ -1041,7 +973,6 @@
 			snprintf(file, sizeof(file), "%s",
 			    desc->action_arg);
 
-		image = lookup_image_from_uuid(&desc->uuid);
 		if (image == NULL) {
 			if (!unpack_all)
 				log_warnx("%s does not exist in %s",
@@ -1059,7 +990,6 @@
 		}
 	}
 
-	free_images();
 	return 0;
 }
 
@@ -1168,17 +1098,15 @@
 	parse_fip(argv[0], &toc_header);
 
 	for (desc = image_desc_head; desc != NULL; desc = desc->next) {
-		image_t *image;
-
 		if (desc->action != DO_REMOVE)
 			continue;
 
-		image = lookup_image_from_uuid(&desc->uuid);
-		if (image != NULL) {
+		if (desc->image != NULL) {
 			if (verbose)
 				log_dbgx("Removing %s",
 				    desc->cmdline_name);
-			remove_image(image);
+			free(desc->image);
+			desc->image = NULL;
 		} else {
 			log_warnx("%s does not exist in %s",
 			    desc->cmdline_name, argv[0]);
@@ -1186,7 +1114,6 @@
 	}
 
 	pack_images(outfile, toc_header.flags, align);
-	free_images();
 	return 0;
 }
 
diff --git a/tools/fiptool/fiptool.h b/tools/fiptool/fiptool.h
index be0c6f0..c4c86bc 100644
--- a/tools/fiptool/fiptool.h
+++ b/tools/fiptool/fiptool.h
@@ -58,13 +58,13 @@
 	char              *cmdline_name;
 	int                action;
 	char              *action_arg;
+	struct image      *image;
 	struct image_desc *next;
 } image_desc_t;
 
 typedef struct image {
 	struct fip_toc_entry toc_e;
 	void                *buffer;
-	struct image        *next;
 } image_t;
 
 typedef struct cmd {