ArmPkg/BdsLib: Added support to declare Power State Coordination Interface (PSCI) to the Flat Device Tree (FDT)

Contributed-under: TianoCore Contribution Agreement 1.0
Signed-off-by: Olivier Martin <olivier.martin@arm.com>



git-svn-id: https://edk2.svn.sourceforge.net/svnroot/edk2/trunk/edk2@14185 6f19259b-4bc3-4df7-8a09-765794883524

ArmPkg/ArmPkg.dec

@@ -65,6 +65,9 @@
   # it has been configured by the CPU DXE
   gArmTokenSpaceGuid.PcdDebuggerExceptionSupport|FALSE|BOOLEAN|0x00000032
   
+  # Define if the Power State Coordination Interface (PSCI) is supported by the Platform Trusted Firmware
+  gArmTokenSpaceGuid.PcdArmPsciSupport|FALSE|BOOLEAN|0x00000033
+  
 [PcdsFixedAtBuild.common]
   gArmTokenSpaceGuid.PcdTrustzoneSupport|FALSE|BOOLEAN|0x00000006
 

ArmPkg/Library/BdsLib/BdsLib.inf

@@ -35,9 +35,11 @@
   MdePkg/MdePkg.dec
   EmbeddedPkg/EmbeddedPkg.dec
   ArmPkg/ArmPkg.dec
+  ArmPlatformPkg/ArmPlatformPkg.dec
 
 [LibraryClasses]
   ArmLib
+  ArmSmcLib
   BaseLib
   DebugLib
   DevicePathLib
@@ -65,6 +67,7 @@
   gEfiLoadedImageProtocolGuid
     
 [FeaturePcd]
+  gArmTokenSpaceGuid.PcdArmPsciSupport
 
 [FixedPcd]
   gArmTokenSpaceGuid.PcdSystemMemoryBase

ArmPkg/Library/BdsLib/BdsLinuxFdt.c

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

ArmPlatformPkg/ArmPlatformPkg-2ndstage.dsc

@@ -81,6 +81,7 @@
   DmaLib|ArmPkg/Library/ArmDmaLib/ArmDmaLib.inf
   ArmGicLib|ArmPkg/Drivers/PL390Gic/PL390GicLib.inf
   ArmPlatformStackLib|ArmPlatformPkg/Library/ArmPlatformStackLib/ArmPlatformStackLib.inf
+  ArmSmcLib|ArmPkg/Library/ArmSmcLib/ArmSmcLib.inf
 
   SerialPortLib|MdePkg/Library/BaseSerialPortLibNull/BaseSerialPortLibNull.inf
   TimerLib|MdePkg/Library/BaseTimerLibNullTemplate/BaseTimerLibNullTemplate.inf

ArmPlatformPkg/ArmPlatformPkg.dsc

@@ -80,6 +80,7 @@
   ArmDisassemblerLib|ArmPkg/Library/ArmDisassemblerLib/ArmDisassemblerLib.inf
   DmaLib|ArmPkg/Library/ArmDmaLib/ArmDmaLib.inf
   ArmGicLib|ArmPkg/Drivers/PL390Gic/PL390GicLib.inf
+  ArmSmcLib|ArmPkg/Library/ArmSmcLib/ArmSmcLib.inf
 
   SerialPortLib|MdePkg/Library/BaseSerialPortLibNull/BaseSerialPortLibNull.inf
   TimerLib|MdePkg/Library/BaseTimerLibNullTemplate/BaseTimerLibNullTemplate.inf