ArmPkg/BdsLib: Update 'cpu-release-addr' with the UEFI locations if the method is...

[mirror_edk2.git] / ArmPkg / Library / BdsLib / BdsLinuxFdt.c

diff --git a/ArmPkg/Library/BdsLib/BdsLinuxFdt.c b/ArmPkg/Library/BdsLib/BdsLinuxFdt.c
index c873d96369887227de06614167a626b5d6f220f9..5fccf8cac791152370e44fb1daa8f40771347080 100644 (file)
--- a/ArmPkg/Library/BdsLib/BdsLinuxFdt.c
+++ b/ArmPkg/Library/BdsLib/BdsLinuxFdt.c
@@ -12,9 +12,12 @@
 *\r
 **/\r
 \r
+#include <Library/ArmSmcLib.h>\r
 #include <Library/PcdLib.h>\r
 #include <libfdt.h>\r
 \r
+#include <IndustryStandard/ArmSmc.h>\r
+\r
 #include "BdsInternal.h"\r
 #include "BdsLinuxLoader.h"\r
 \r
@@ -218,6 +221,7 @@ PrepareFdt (
   INTN                  cpu_node;\r
   INTN                  lenp;\r
   CONST VOID*           BootArg;\r
+  CONST VOID*           Method;\r
   EFI_PHYSICAL_ADDRESS  InitrdImageStart;\r
   EFI_PHYSICAL_ADDRESS  InitrdImageEnd;\r
   FdtRegion             Region;\r
@@ -237,6 +241,35 @@ PrepareFdt (
   UINTN                 DescriptorSize;\r
   UINT32                DescriptorVersion;\r
   UINTN                 Pages;\r
+  BOOLEAN               PsciSmcSupported;\r
+  UINTN                 Rx;\r
+\r
+  //\r
+  // Ensure the Power State Coordination Interface (PSCI) SMCs are there if supported\r
+  //\r
+  PsciSmcSupported = FALSE;\r
+  if (FeaturePcdGet (PcdArmPsciSupport) == TRUE) {\r
+    // Check the SMC response to the Presence SMC\r
+    Rx   = ARM_SMC_ID_PRESENCE;\r
+    ArmCallSmc (&Rx);\r
+    if (Rx == 1) {\r
+      // Check the SMC UID\r
+      Rx   = ARM_SMC_ID_UID;\r
+      ArmCallSmc (&Rx);\r
+      if (Rx == ARM_TRUSTZONE_UID_4LETTERID) {\r
+        Rx   = ARM_SMC_ID_UID + 1;\r
+        ArmCallSmc (&Rx);\r
+        //TODO: Replace ARM magic number\r
+        if (Rx == 0x40524d48) {\r
+          PsciSmcSupported = TRUE;\r
+        }\r
+      }\r
+      if (PsciSmcSupported == FALSE) {\r
+        DEBUG((EFI_D_ERROR,"Warning: The Power State Coordination Interface (PSCI) is not supported"\r
+                           "by your platform Trusted Firmware.\n"));\r
+      }\r
+    }\r
+  }\r
 \r
   err = fdt_check_header ((VOID*)(UINTN)(*FdtBlobBase));\r
   if (err != 0) {\r
@@ -418,19 +451,53 @@ PrepareFdt (
           fdt_setprop(fdt, cpu_node, "reg", &Index, sizeof(Index));\r
         }\r
 \r
-        fdt_setprop_string(fdt, cpu_node, "enable-method", "spin-table");\r
-        CpuReleaseAddr = cpu_to_fdt64(ArmCoreInfoTable[Index].MailboxSetAddress);\r
-        fdt_setprop(fdt, cpu_node, "cpu-release-addr", &CpuReleaseAddr, sizeof(CpuReleaseAddr));\r
-\r
-        // If it is not the primary core than the cpu should be disabled\r
-        if (((ArmCoreInfoTable[Index].ClusterId != ClusterId) || (ArmCoreInfoTable[Index].CoreId != CoreId))) {\r
-          fdt_setprop_string(fdt, cpu_node, "status", "disabled");\r
+        // If Power State Coordination Interface (PSCI) is not supported then it is expected the secondary\r
+        // cores are spinning waiting for the Operating System to release them\r
+        if (PsciSmcSupported == FALSE) {\r
+          // We as the bootloader are responsible for either creating or updating\r
+          // these entries. Do not trust the entries in the DT. We only know about\r
+          // 'spin-table' type. Do not try to update other types if defined.\r
+          Method = fdt_getprop(fdt, cpu_node, "enable-method", &lenp);\r
+          if ( (Method == NULL) || (!AsciiStrCmp((CHAR8 *)Method, "spin-table")) ) {\r
+            fdt_setprop_string(fdt, cpu_node, "enable-method", "spin-table");\r
+            CpuReleaseAddr = cpu_to_fdt64(ArmCoreInfoTable[Index].MailboxSetAddress);\r
+            fdt_setprop(fdt, cpu_node, "cpu-release-addr", &CpuReleaseAddr, sizeof(CpuReleaseAddr));\r
+\r
+            // If it is not the primary core than the cpu should be disabled\r
+            if (((ArmCoreInfoTable[Index].ClusterId != ClusterId) || (ArmCoreInfoTable[Index].CoreId != CoreId))) {\r
+              fdt_setprop_string(fdt, cpu_node, "status", "disabled");\r
+            }\r
+          } else {\r
+            Print(L"Warning: Unsupported enable-method type for CPU[%d] : %a\n", Index, (CHAR8 *)Method);\r
+          }\r
         }\r
       }\r
       break;\r
     }\r
   }\r
 \r
+  // If the Power State Coordination Interface is supported then we signal it in the Device Tree\r
+  if (PsciSmcSupported == TRUE) {\r
+    // Before to create it we check if the node is not already defined in the Device Tree\r
+    node = fdt_subnode_offset(fdt, 0, "psci");\r
+    if (node < 0) {\r
+      // The 'psci' node does not exist, create it\r
+      node = fdt_add_subnode(fdt, 0, "psci");\r
+      if (node < 0) {\r
+        DEBUG((EFI_D_ERROR,"Error on creating 'psci' node\n"));\r
+        Status = EFI_INVALID_PARAMETER;\r
+        goto FAIL_NEW_FDT;\r
+      } else {\r
+        fdt_setprop_string(fdt, node, "compatible", "arm,psci");\r
+        fdt_setprop_string(fdt, node, "method", "smc");\r
+        fdt_setprop_cell(fdt, node, "cpu_suspend", ARM_SMC_ARM_CPU_SUSPEND);\r
+        fdt_setprop_cell(fdt, node, "cpu_off", ARM_SMC_ARM_CPU_OFF);\r
+        fdt_setprop_cell(fdt, node, "cpu_on", ARM_SMC_ARM_CPU_ON);\r
+        fdt_setprop_cell(fdt, node, "cpu_migrate", ARM_SMC_ARM_MIGRATE);\r
+      }\r
+    }\r
+  }\r
+\r
   DEBUG_CODE_BEGIN();\r
     //DebugDumpFdt (fdt);\r
   DEBUG_CODE_END();\r