1 files changed, 69 insertions, 0 deletions
diff --git a/services/spd/trusty/trusty_helpers.S b/services/spd/trusty/trusty_helpers.S
new file mode 100644
index 00000000..da5cb573
--- /dev/null
+++ b/services/spd/trusty/trusty_helpers.S
@@ -0,0 +1,69 @@
+/*
+ * Copyright (c) 2016, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <asm_macros.S>
+
+.macro push ra, rb, sp=sp
+	stp \ra, \rb, [\sp,#-16]!
+.endm
+
+.macro pop ra, rb, sp=sp
+	ldp \ra, \rb, [\sp], #16
+.endm
+
+	.global trusty_context_switch_helper
+func trusty_context_switch_helper
+	push	x8, xzr
+	push	x19, x20
+	push	x21, x22
+	push	x23, x24
+	push	x25, x26
+	push	x27, x28
+	push	x29, x30
+
+	mov	x9, sp
+	ldr	x10, [x0]
+	mov	sp, x10
+	str	x9, [x0]
+
+	pop	x29, x30
+	pop	x27, x28
+	pop	x25, x26
+	pop	x23, x24
+	pop	x21, x22
+	pop	x19, x20
+	pop	x8, xzr
+
+        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
+
+	.global trusty_init_context_stack
+func trusty_init_context_stack
+	push	x8, xzr, x1
+	push	xzr, xzr, x1
+	push	xzr, xzr, x1
+	push	xzr, xzr, x1
+	push	xzr, xzr, x1
+	push	xzr, xzr, x1
+	adr	x9, el3_exit
+	push	xzr, x9, x1
+	str	x1, [x0]
+	ret
+endfunc trusty_init_context_stack