diff --git a/docs/spd/trusty-dispatcher.rst b/docs/spd/trusty-dispatcher.rst
index f1982ea..10b111d 100644
--- a/docs/spd/trusty-dispatcher.rst
+++ b/docs/spd/trusty-dispatcher.rst
@@ -8,6 +8,23 @@
 Open Source Project (AOSP) webpage for Trusty hosted at
 https://source.android.com/security/trusty
 
+Boot parameters
+===============
+
+Custom boot parameters can be passed to Trusty by providing a platform
+specific function:
+
+.. code:: c
+
+    void plat_trusty_set_boot_args(aapcs64_params_t *args)
+
+If this function is provided ``args->arg0`` must be set to the memory
+size allocated to trusty. If the platform does not provide this
+function, but defines ``TSP_SEC_MEM_SIZE``, a default implementation
+will pass the memory size from ``TSP_SEC_MEM_SIZE``. ``args->arg1``
+can be set to a platform specific parameter block, and ``args->arg2``
+should then be set to the size of that block.
+
 Supported platforms
 ===================
 
diff --git a/plat/nvidia/tegra/common/tegra_bl31_setup.c b/plat/nvidia/tegra/common/tegra_bl31_setup.c
index d5d3d53..d89ad7b 100644
--- a/plat/nvidia/tegra/common/tegra_bl31_setup.c
+++ b/plat/nvidia/tegra/common/tegra_bl31_setup.c
@@ -24,6 +24,9 @@
 #include <tegra_def.h>
 #include <tegra_private.h>
 
+/* length of Trusty's input parameters (in bytes) */
+#define TRUSTY_PARAMS_LEN_BYTES	(4096*2)
+
 extern void zeromem16(void *mem, unsigned int length);
 
 /*******************************************************************************
@@ -58,6 +61,8 @@
 static plat_params_from_bl2_t plat_bl31_params_from_bl2 = {
 	.tzdram_size = (uint64_t)TZDRAM_SIZE
 };
+static unsigned long bl32_mem_size;
+static unsigned long bl32_boot_params;
 
 /*******************************************************************************
  * This variable holds the non-secure image entry address
@@ -147,8 +152,11 @@
 	assert(from_bl2->bl33_ep_info);
 	bl33_image_ep_info = *from_bl2->bl33_ep_info;
 
-	if (from_bl2->bl32_ep_info)
+	if (from_bl2->bl32_ep_info) {
 		bl32_image_ep_info = *from_bl2->bl32_ep_info;
+		bl32_mem_size = from_bl2->bl32_ep_info->args.arg0;
+		bl32_boot_params = from_bl2->bl32_ep_info->args.arg2;
+	}
 
 	/*
 	 * Parse platform specific parameters - TZDRAM aperture base and size
@@ -233,6 +241,15 @@
 	INFO("BL3-1: Boot CPU: %s Processor [%lx]\n", (impl == DENVER_IMPL) ?
 		"Denver" : "ARM", read_mpidr());
 }
+
+#ifdef SPD_trusty
+void plat_trusty_set_boot_args(aapcs64_params_t *args)
+{
+	args->arg0 = bl32_mem_size;
+	args->arg1 = bl32_boot_params;
+	args->arg2 = TRUSTY_PARAMS_LEN_BYTES;
+}
+#endif
 
 /*******************************************************************************
  * Initialize the gic, configure the SCR.
diff --git a/services/spd/trusty/generic-arm64-smcall.c b/services/spd/trusty/generic-arm64-smcall.c
new file mode 100644
index 0000000..38da279
--- /dev/null
+++ b/services/spd/trusty/generic-arm64-smcall.c
@@ -0,0 +1,94 @@
+/*
+ * Copyright (c) 2016, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <debug.h>
+#include <runtime_svc.h>
+
+#include "generic-arm64-smcall.h"
+
+int trusty_disable_serial_debug;
+
+struct dputc_state {
+	char linebuf[128];
+	unsigned l;
+};
+
+static struct dputc_state dputc_state[2];
+
+static void trusty_dputc(char ch, int secure)
+{
+	unsigned i;
+	struct dputc_state *s = &dputc_state[!secure];
+
+	if (trusty_disable_serial_debug)
+		return;
+
+	s->linebuf[s->l++] = ch;
+	if (s->l == sizeof(s->linebuf) || ch == '\n') {
+		if (secure)
+			printf("secure os: ");
+		else
+			printf("non-secure os: ");
+		for (i = 0; i < s->l; i++) {
+			putchar(s->linebuf[i]);
+		}
+		if (ch != '\n') {
+			printf(" <...>\n");
+		}
+		s->l = 0;
+	}
+}
+
+static uint64_t trusty_get_reg_base(uint32_t reg)
+{
+	switch (reg) {
+	case 0:
+		return PLAT_ARM_GICD_BASE;
+
+	case 1:
+		return PLAT_ARM_GICC_BASE;
+
+	default:
+		NOTICE("%s(0x%x) unknown reg\n", __func__, reg);
+		return SMC_UNK;
+	}
+}
+
+static uint64_t trusty_generic_platform_smc(uint32_t smc_fid,
+			 uint64_t x1,
+			 uint64_t x2,
+			 uint64_t x3,
+			 uint64_t x4,
+			 void *cookie,
+			 void *handle,
+			 uint64_t flags)
+{
+	switch (smc_fid) {
+	case SMC_FC_DEBUG_PUTC:
+		trusty_dputc(x1, is_caller_secure(flags));
+		SMC_RET1(handle, 0);
+
+	case SMC_FC_GET_REG_BASE:
+	case SMC_FC64_GET_REG_BASE:
+		SMC_RET1(handle, trusty_get_reg_base(x1));
+
+	default:
+		NOTICE("%s(0x%x, 0x%lx) unknown smc\n", __func__, smc_fid, x1);
+		SMC_RET1(handle, SMC_UNK);
+	}
+}
+
+/* Define a SPD runtime service descriptor for fast SMC calls */
+DECLARE_RT_SVC(
+	trusty_fast,
+
+	SMC_ENTITY_PLATFORM_MONITOR,
+	SMC_ENTITY_PLATFORM_MONITOR,
+	SMC_TYPE_FAST,
+	NULL,
+	trusty_generic_platform_smc
+);
+
diff --git a/services/spd/trusty/generic-arm64-smcall.h b/services/spd/trusty/generic-arm64-smcall.h
new file mode 100644
index 0000000..06efc72
--- /dev/null
+++ b/services/spd/trusty/generic-arm64-smcall.h
@@ -0,0 +1,27 @@
+/*
+ * Copyright (c) 2016, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include "smcall.h"
+
+#define	SMC_ENTITY_PLATFORM_MONITOR	61
+
+/*
+ * SMC calls implemented by EL3 monitor
+ */
+
+/*
+ * Write character in r1 to debug console
+ */
+#define SMC_FC_DEBUG_PUTC	SMC_FASTCALL_NR(SMC_ENTITY_PLATFORM_MONITOR, 0x0)
+
+/*
+ * Get register base address
+ * r1: SMC_GET_GIC_BASE_GICD or SMC_GET_GIC_BASE_GICC
+ */
+#define SMC_GET_GIC_BASE_GICD	0
+#define SMC_GET_GIC_BASE_GICC	1
+#define SMC_FC_GET_REG_BASE	SMC_FASTCALL_NR(SMC_ENTITY_PLATFORM_MONITOR, 0x1)
+#define SMC_FC64_GET_REG_BASE	SMC_FASTCALL64_NR(SMC_ENTITY_PLATFORM_MONITOR, 0x1)
diff --git a/services/spd/trusty/trusty.c b/services/spd/trusty/trusty.c
index ecbcfae..d6e5726 100644
--- a/services/spd/trusty/trusty.c
+++ b/services/spd/trusty/trusty.c
@@ -13,6 +13,7 @@
 #include <interrupt_mgmt.h>
 #include <platform.h>
 #include <runtime_svc.h>
+#include <stdbool.h>
 #include <string.h>
 
 #include "sm_err.h"
@@ -21,9 +22,6 @@
 /* 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;
@@ -105,10 +103,8 @@
 	 * when it's needed the PSCI caller has preserved FP context before
 	 * going here.
 	 */
-#if CTX_INCLUDE_FPREGS
 	if (r0 != SMC_FC_CPU_SUSPEND && r0 != SMC_FC_CPU_RESUME)
 		fpregs_context_save(get_fpregs_ctx(cm_get_context(security_state)));
-#endif
 	cm_el1_sysregs_context_save(security_state);
 
 	ctx->saved_security_state = security_state;
@@ -117,10 +113,8 @@
 	assert(ctx->saved_security_state == !security_state);
 
 	cm_el1_sysregs_context_restore(security_state);
-#if CTX_INCLUDE_FPREGS
 	if (r0 != SMC_FC_CPU_SUSPEND && r0 != SMC_FC_CPU_RESUME)
 		fpregs_context_restore(get_fpregs_ctx(cm_get_context(security_state)));
-#endif
 
 	cm_set_next_eret_context(security_state);
 
@@ -299,6 +293,7 @@
 	ep_info = bl31_plat_get_next_image_ep_info(SECURE);
 	assert(ep_info);
 
+	fpregs_context_save(get_fpregs_ctx(cm_get_context(NON_SECURE)));
 	cm_el1_sysregs_context_save(NON_SECURE);
 
 	cm_set_context(&ctx->cpu_ctx, SECURE);
@@ -315,6 +310,7 @@
 	}
 
 	cm_el1_sysregs_context_restore(SECURE);
+	fpregs_context_restore(get_fpregs_ctx(cm_get_context(SECURE)));
 	cm_set_next_eret_context(SECURE);
 
 	ctx->saved_security_state = ~0; /* initial saved state is invalid */
@@ -323,27 +319,28 @@
 	trusty_context_switch_helper(&ctx->saved_sp, &zero_args);
 
 	cm_el1_sysregs_context_restore(NON_SECURE);
+	fpregs_context_restore(get_fpregs_ctx(cm_get_context(NON_SECURE)));
 	cm_set_next_eret_context(NON_SECURE);
 
 	return 0;
 }
 
-static void trusty_cpu_suspend(void)
+static void trusty_cpu_suspend(uint32_t off)
 {
 	struct args ret;
 
-	ret = trusty_context_switch(NON_SECURE, SMC_FC_CPU_SUSPEND, 0, 0, 0);
+	ret = trusty_context_switch(NON_SECURE, SMC_FC_CPU_SUSPEND, off, 0, 0);
 	if (ret.r0 != 0) {
 		INFO("%s: cpu %d, SMC_FC_CPU_SUSPEND returned unexpected value, %ld\n",
 		     __func__, plat_my_core_pos(), ret.r0);
 	}
 }
 
-static void trusty_cpu_resume(void)
+static void trusty_cpu_resume(uint32_t on)
 {
 	struct args ret;
 
-	ret = trusty_context_switch(NON_SECURE, SMC_FC_CPU_RESUME, 0, 0, 0);
+	ret = trusty_context_switch(NON_SECURE, SMC_FC_CPU_RESUME, on, 0, 0);
 	if (ret.r0 != 0) {
 		INFO("%s: cpu %d, SMC_FC_CPU_RESUME returned unexpected value, %ld\n",
 		     __func__, plat_my_core_pos(), ret.r0);
@@ -352,7 +349,7 @@
 
 static int32_t trusty_cpu_off_handler(uint64_t unused)
 {
-	trusty_cpu_suspend();
+	trusty_cpu_suspend(1);
 
 	return 0;
 }
@@ -364,18 +361,18 @@
 	if (!ctx->saved_sp) {
 		trusty_init();
 	} else {
-		trusty_cpu_resume();
+		trusty_cpu_resume(1);
 	}
 }
 
 static void trusty_cpu_suspend_handler(uint64_t unused)
 {
-	trusty_cpu_suspend();
+	trusty_cpu_suspend(0);
 }
 
 static void trusty_cpu_suspend_finish_handler(uint64_t unused)
 {
-	trusty_cpu_resume();
+	trusty_cpu_resume(0);
 }
 
 static const spd_pm_ops_t trusty_pm = {
@@ -385,11 +382,23 @@
 	.svc_suspend_finish = trusty_cpu_suspend_finish_handler,
 };
 
+void plat_trusty_set_boot_args(aapcs64_params_t *args);
+
+#ifdef TSP_SEC_MEM_SIZE
+#pragma weak plat_trusty_set_boot_args
+void plat_trusty_set_boot_args(aapcs64_params_t *args)
+{
+	args->arg0 = TSP_SEC_MEM_SIZE;
+}
+#endif
+
 static int32_t trusty_setup(void)
 {
 	entry_point_info_t *ep_info;
+	uint32_t instr;
 	uint32_t flags;
 	int ret;
+	bool aarch32 = false;
 
 	/* Get trusty's entry point info */
 	ep_info = bl31_plat_get_next_image_ep_info(SECURE);
@@ -398,17 +407,29 @@
 		return -1;
 	}
 
-	/* Trusty runs in AARCH64 mode */
-	SET_PARAM_HEAD(ep_info, PARAM_EP, VERSION_1, SECURE | EP_ST_ENABLE);
-	ep_info->spsr = SPSR_64(MODE_EL1, MODE_SP_ELX, DISABLE_ALL_EXCEPTIONS);
+	instr = *(uint32_t *)ep_info->pc;
 
-	/*
-	 * 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;
+	if (instr >> 24 == 0xeaU) {
+		INFO("trusty: Found 32 bit image\n");
+		aarch32 = true;
+	} else if (instr >> 8 == 0xd53810U || instr >> 16 == 0x9400U) {
+		INFO("trusty: Found 64 bit image\n");
+	} else {
+		NOTICE("trusty: Found unknown image, 0x%x\n", instr);
+	}
+
+	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);
+	(void)memset(&ep_info->args, 0, sizeof(ep_info->args));
+	plat_trusty_set_boot_args(&ep_info->args);
 
 	/* register init handler */
 	bl31_register_bl32_init(trusty_init);
@@ -425,6 +446,31 @@
 	if (ret)
 		ERROR("trusty: failed to register fiq handler, ret = %d\n", ret);
 
+	if (aarch32) {
+		entry_point_info_t *ns_ep_info;
+		uint32_t spsr;
+
+		ns_ep_info = bl31_plat_get_next_image_ep_info(NON_SECURE);
+		if (!ep_info) {
+			NOTICE("Trusty: non-secure image missing.\n");
+			return -1;
+		}
+		spsr = ns_ep_info->spsr;
+		if (GET_RW(spsr) == MODE_RW_64 && GET_EL(spsr) == MODE_EL2) {
+			spsr &= ~(MODE_EL_MASK << MODE_EL_SHIFT);
+			spsr |= MODE_EL1 << MODE_EL_SHIFT;
+		}
+		if (GET_RW(spsr) == MODE_RW_32 && GET_M32(spsr) == MODE32_hyp) {
+			spsr &= ~(MODE32_MASK << MODE32_SHIFT);
+			spsr |= MODE32_svc << MODE32_SHIFT;
+		}
+		if (spsr != ns_ep_info->spsr) {
+			NOTICE("Trusty: Switch bl33 from EL2 to EL1 (spsr 0x%x -> 0x%x)\n",
+			       ns_ep_info->spsr, spsr);
+			ns_ep_info->spsr = spsr;
+		}
+	}
+
 	return 0;
 }
 
diff --git a/services/spd/trusty/trusty.mk b/services/spd/trusty/trusty.mk
index beca875..a571fa2 100644
--- a/services/spd/trusty/trusty.mk
+++ b/services/spd/trusty/trusty.mk
@@ -8,3 +8,11 @@
 
 SPD_SOURCES		:=	services/spd/trusty/trusty.c		\
 				services/spd/trusty/trusty_helpers.S
+
+ifeq (${TRUSTY_SPD_WITH_GENERIC_SERVICES},1)
+SPD_SOURCES		+=	services/spd/trusty/generic-arm64-smcall.c
+endif
+
+NEED_BL32		:=	yes
+
+CTX_INCLUDE_FPREGS	:=	1