1 files changed, 28 insertions, 51 deletions
diff --git a/ArmPkg/Library/ArmSmcLib/Arm/ArmSmc.asm b/ArmPkg/Library/ArmSmcLib/Arm/ArmSmc.asm
index f46bd7ecd4..ad55ae7752 100644
--- a/ArmPkg/Library/ArmSmcLib/Arm/ArmSmc.asm
+++ b/ArmPkg/Library/ArmSmcLib/Arm/ArmSmc.asm
@@ -1,5 +1,5 @@
 //
-//  Copyright (c) 2012, ARM Limited. All rights reserved.
+//  Copyright (c) 2012-2014, ARM Limited. All rights reserved.
 //
 //  This program and the accompanying materials
 //  are licensed and made available under the terms and conditions of the BSD License
@@ -12,64 +12,41 @@
 //
 
     EXPORT ArmCallSmc
-    EXPORT ArmCallSmcArg1
-    EXPORT ArmCallSmcArg2
-    EXPORT ArmCallSmcArg3
 
     AREA   ArmSmc, CODE, READONLY
 
 ArmCallSmc
-    push    {r1}
-    mov     r1, r0
-    ldr     r0,[r1]
-    smc     #0
-    str     r0,[r1]
-    pop     {r1}
-    bx      lr
+    push    {r4-r8}
+    // r0 will be popped just after the SMC call
+    pop     {r0}
 
-ArmCallSmcArg1
-    push    {r2-r3}
-    mov     r2, r0
-    mov     r3, r1
-    ldr     r0,[r2]
-    ldr     r1,[r3]
-    smc     #0
-    str     r0,[r2]
-    str     r1,[r3]
-    pop     {r2-r3}
-    bx      lr
+    // Load the SMC arguments values into the appropriate registers
+    ldr     r7, [r0, #28]
+    ldr     r6, [r0, #24]
+    ldr     r5, [r0, #20]
+    ldr     r4, [r0, #16]
+    ldr     r3, [r0, #12]
+    ldr     r2, [r0, #8]
+    ldr     r1, [r0, #4]
+    ldr     r0, [r0, #0]
 
-ArmCallSmcArg2
-    push    {r3-r5}
-    mov     r3, r0
-    mov     r4, r1
-    mov     r5, r2
-    ldr     r0,[r3]
-    ldr     r1,[r4]
-    ldr     r2,[r5]
     smc     #0
-    str     r0,[r3]
-    str     r1,[r4]
-    str     r2,[r5]
-    pop     {r3-r5}
-    bx      lr
 
-ArmCallSmcArg3
-    push    {r4-r7}
-    mov     r4, r0
-    mov     r5, r1
-    mov     r6, r2
-    mov     r7, r3
-    ldr     r0,[r4]
-    ldr     r1,[r5]
-    ldr     r2,[r6]
-    ldr     r3,[r7]
-    smc     #0
-    str     r0,[r4]
-    str     r1,[r5]
-    str     r2,[r6]
-    str     r3,[r7]
-    pop     {r4-r7}
+    // Pop the ARM_SMC_ARGS structure address from the stack into r8
+    pop     {r8}
+
+    // Load the SMC returned values into the appropriate registers
+    // A SMC call can return up to 4 values - we do not need to store back r4-r7.
+    str     r3, [r8, #12]
+    str     r2, [r8, #8]
+    str     r1, [r8, #4]
+    str     r0, [r8, #0]
+
+    mov     r0, r8
+
+    // Restore the registers r4-r8
+    pop     {r4-r8}
+
     bx      lr
 
     END