ArmPkg/ArmScmiDxe: Add clock enable function

[mirror_edk2.git] / ArmPkg / Drivers / ArmScmiDxe / ScmiClockProtocol.c

diff --git a/ArmPkg/Drivers/ArmScmiDxe/ScmiClockProtocol.c b/ArmPkg/Drivers/ArmScmiDxe/ScmiClockProtocol.c
index 64d2afab722ae196e4f75e027216be642b4ca812..c7f27a301f17c37aba2832d2ea25b7797a29c136 100644 (file)
--- a/ArmPkg/Drivers/ArmScmiDxe/ScmiClockProtocol.c
+++ b/ArmPkg/Drivers/ArmScmiDxe/ScmiClockProtocol.c
@@ -19,6 +19,7 @@
 #include <Library/DebugLib.h>\r
 #include <Library/UefiBootServicesTableLib.h>\r
 #include <Protocol/ArmScmiClockProtocol.h>\r
+#include <Protocol/ArmScmiClock2Protocol.h>\r
 \r
 #include "ArmScmiClockProtocolPrivate.h"\r
 #include "ScmiPrivate.h"\r
@@ -388,6 +389,53 @@ ClockRateSet (
   return Status;\r
 }\r
 \r
+/** Enable/Disable specified clock.\r
+\r
+  @param[in]  This        A Pointer to SCMI_CLOCK_PROTOCOL Instance.\r
+  @param[in]  ClockId     Identifier for the clock device.\r
+  @param[in]  Enable      TRUE to enable, FALSE to disable.\r
+\r
+  @retval EFI_SUCCESS          Clock enable/disable successful.\r
+  @retval EFI_DEVICE_ERROR     SCP returns an SCMI error.\r
+  @retval !(EFI_SUCCESS)       Other errors.\r
+**/\r
+STATIC\r
+EFI_STATUS\r
+ClockEnable (\r
+  IN SCMI_CLOCK2_PROTOCOL *This,\r
+  IN UINT32               ClockId,\r
+  IN BOOLEAN              Enable\r
+  )\r
+{\r
+  EFI_STATUS                  Status;\r
+  CLOCK_CONFIG_SET_ATTRIBUTES *ClockConfigSetAttributes;\r
+  SCMI_COMMAND                Cmd;\r
+  UINT32                      PayloadLength;\r
+\r
+  Status = ScmiCommandGetPayload ((UINT32**)&ClockConfigSetAttributes);\r
+  if (EFI_ERROR (Status)) {\r
+    return Status;\r
+  }\r
+\r
+  // Fill arguments for clock protocol command.\r
+  ClockConfigSetAttributes->ClockId    = ClockId;\r
+  ClockConfigSetAttributes->Attributes = Enable ? BIT0 : 0;\r
+\r
+  Cmd.ProtocolId = SCMI_PROTOCOL_ID_CLOCK;\r
+  Cmd.MessageId  = SCMI_MESSAGE_ID_CLOCK_CONFIG_SET;\r
+\r
+  PayloadLength = sizeof (CLOCK_CONFIG_SET_ATTRIBUTES);\r
+\r
+  // Execute and wait for response on a SCMI channel.\r
+  Status = ScmiCommandExecute (\r
+             &Cmd,\r
+             &PayloadLength,\r
+             NULL\r
+             );\r
+\r
+  return Status;\r
+}\r
+\r
 // Instance of the SCMI clock management protocol.\r
 STATIC CONST SCMI_CLOCK_PROTOCOL ScmiClockProtocol = {\r
   ClockGetVersion,\r
@@ -398,6 +446,18 @@ STATIC CONST SCMI_CLOCK_PROTOCOL ScmiClockProtocol = {
   ClockRateSet\r
  };\r
 \r
+// Instance of the SCMI clock management protocol.\r
+STATIC CONST SCMI_CLOCK2_PROTOCOL ScmiClock2Protocol = {\r
+  (SCMI_CLOCK2_GET_VERSION)ClockGetVersion,\r
+  (SCMI_CLOCK2_GET_TOTAL_CLOCKS)ClockGetTotalClocks,\r
+  (SCMI_CLOCK2_GET_CLOCK_ATTRIBUTES)ClockGetClockAttributes,\r
+  (SCMI_CLOCK2_DESCRIBE_RATES)ClockDescribeRates,\r
+  (SCMI_CLOCK2_RATE_GET)ClockRateGet,\r
+  (SCMI_CLOCK2_RATE_SET)ClockRateSet,\r
+  SCMI_CLOCK2_PROTOCOL_VERSION,\r
+  ClockEnable\r
+ };\r
+\r
 /** Initialize clock management protocol and install protocol on a given handle.\r
 \r
   @param[in] Handle              Handle to install clock management protocol.\r
@@ -413,6 +473,8 @@ ScmiClockProtocolInit (
                 Handle,\r
                 &gArmScmiClockProtocolGuid,\r
                 &ScmiClockProtocol,\r
+                &gArmScmiClock2ProtocolGuid,\r
+                &ScmiClock2Protocol,\r
                 NULL\r
                 );\r
 }\r