@@ -45,6 +45,19 @@
#define HVC_SET_VECTORS 2
+/*
+ * HVC_CALL_FUNC - Execute a function at EL2.
+ *
+ * @x0: Physical address of the function to be executed.
+ * @x1: Passed as the first argument to the function.
+ * @x2: Passed as the second argument to the function.
+ * @x3: Passed as the third argument to the function.
+ *
+ * The called function must preserve the contents of register x18.
+ */
+
+#define HVC_CALL_FUNC 3
+
#define BOOT_CPU_MODE_EL1 (0xe11)
#define BOOT_CPU_MODE_EL2 (0xe12)
@@ -67,8 +67,19 @@ el1_sync:
b 2f
1: cmp x18, #HVC_SET_VECTORS
- b.ne 2f
+ b.ne 1f
msr vbar_el2, x0
+ b 2f
+
+1: cmp x18, #HVC_CALL_FUNC
+ b.ne 2f
+ mov x18, lr
+ mov lr, x0
+ mov x0, x1
+ mov x1, x2
+ mov x2, x3
+ blr lr
+ mov lr, x18
2: eret
ENDPROC(el1_sync)