[PATCH v1 1/2] ArmPkg/ArmScmiDxe: Fix ASSERT error in SCMI DXE

[Girish Pathak <girish.pathak@arm.com>]

This change fixes a bug in the SCMI DXE which is observed with the
upcoming release of the SCP firmware.

The PROTOCOL_ID_MASK (0xF) which is used to generate an index in
the ProtocolInitFxns is wrong because protocol ids can be
anywhere in 0x10 - 15 or 0x80 - FF range. This mask generates
the same index for two different protocols e.g. for protocol ids
0x10 and 0x90, which causes duplicate initialization of a protocol
resulting in a failure.

This change removes the use of PROTOCOL_ID_MASK and instead
uses a list of protocol ids and their initialization functions
to identify a supported protocol and initialize it.

Change-Id: Ib004294d2bec853045ed6e811d123832fba555cd
Contributed-under: TianoCore Contribution Agreement 1.1
Signed-off-by: Girish Pathak <girish.pathak@arm.com>
---
 ArmPkg/Drivers/ArmScmiDxe/ScmiDxe.c | 35 ++++++++++----------
 ArmPkg/Drivers/ArmScmiDxe/ScmiDxe.h |  8 +++--
 2 files changed, 22 insertions(+), 21 deletions(-)

diff --git a/ArmPkg/Drivers/ArmScmiDxe/ScmiDxe.c b/ArmPkg/Drivers/ArmScmiDxe/ScmiDxe.c
index 2920c6f6f33c5bb8ac00c903a0b199ba5f06f4de..cc68cbc922fcc06bff8f7e0aa8b6bf64a9932874 100644
--- a/ArmPkg/Drivers/ArmScmiDxe/ScmiDxe.c
+++ b/ArmPkg/Drivers/ArmScmiDxe/ScmiDxe.c
@@ -29,13 +29,10 @@
 #include "ScmiDxe.h"
 #include "ScmiPrivate.h"
 
-STATIC CONST SCMI_PROTOCOL_INIT_TABLE ProtocolInitFxns[MAX_PROTOCOLS] = {
-  { ScmiBaseProtocolInit },
-  { NULL },
-  { NULL },
-  { ScmiPerformanceProtocolInit },
-  { ScmiClockProtocolInit },
-  { NULL }
+STATIC CONST SCMI_PROTOCOL_ENTRY Protocols[] = {
+  { SCMI_PROTOCOL_ID_BASE, ScmiBaseProtocolInit },
+  { SCMI_PROTOCOL_ID_PERFORMANCE, ScmiPerformanceProtocolInit },
+  { SCMI_PROTOCOL_ID_CLOCK, ScmiClockProtocolInit }
 };
 
 /** ARM SCMI driver entry point function.
@@ -65,14 +62,14 @@ ArmScmiDxeEntryPoint (
   UINT32              Version;
   UINT32              Index;
   UINT32              NumProtocols;
-  UINT32              ProtocolNo;
+  UINT32              ProtocolIndex = 0;
   UINT8               SupportedList[MAX_PROTOCOLS];
   UINT32              SupportedListSize = sizeof (SupportedList);
 
-  ProtocolNo = SCMI_PROTOCOL_ID_BASE & PROTOCOL_ID_MASK;
-
   // Every SCMI implementation must implement the base protocol.
-  Status = ProtocolInitFxns[ProtocolNo].Init (&ImageHandle);
+  ASSERT (Protocols[ProtocolIndex].Id == SCMI_PROTOCOL_ID_BASE);
+
+  Status = Protocols[ProtocolIndex].InitFxn (&ImageHandle);
   if (EFI_ERROR (Status)) {
     ASSERT (FALSE);
     return Status;
@@ -123,13 +120,15 @@ ArmScmiDxeEntryPoint (
   }
 
   // Install supported protocol on ImageHandle.
-  for (Index = 0; Index < NumProtocols; Index++) {
-    ProtocolNo = SupportedList[Index] & PROTOCOL_ID_MASK;
-    if (ProtocolInitFxns[ProtocolNo].Init != NULL) {
-      Status = ProtocolInitFxns[ProtocolNo].Init (&ImageHandle);
-      if (EFI_ERROR (Status)) {
-        ASSERT (FALSE);
-        return Status;
+  while (++ProtocolIndex < ARRAY_SIZE (Protocols)) {
+    for (Index = 0; Index < NumProtocols; Index++) {
+      if (Protocols[ProtocolIndex].Id == SupportedList[Index]) {
+        Status = Protocols[ProtocolIndex].InitFxn (&ImageHandle);
+        if (EFI_ERROR (Status)) {
+          ASSERT (FALSE);
+          return Status;
+        }
+        break;
       }
     }
   }
diff --git a/ArmPkg/Drivers/ArmScmiDxe/ScmiDxe.h b/ArmPkg/Drivers/ArmScmiDxe/ScmiDxe.h
index 29cdde173659c701116b021a3c437a92b473e4e5..5815e1e78074067760b6f618e248526ee25e59c8 100644
--- a/ArmPkg/Drivers/ArmScmiDxe/ScmiDxe.h
+++ b/ArmPkg/Drivers/ArmScmiDxe/ScmiDxe.h
@@ -17,8 +17,9 @@
 #ifndef SCMI_DXE_H_
 #define SCMI_DXE_H_
 
+#include "ScmiPrivate.h"
+
 #define MAX_PROTOCOLS        6
-#define PROTOCOL_ID_MASK     0xF
 #define MAX_VENDOR_LEN       SCMI_MAX_STR_LEN
 
 /** Pointer to protocol initialization function.
@@ -35,7 +36,8 @@ EFI_STATUS
   );
 
 typedef struct {
-  SCMI_PROTOCOL_INIT_FXN Init;
-} SCMI_PROTOCOL_INIT_TABLE;
+  SCMI_PROTOCOL_ID Id;            // Protocol Id.
+  SCMI_PROTOCOL_INIT_FXN InitFxn; // Protocol init function.
+} SCMI_PROTOCOL_ENTRY;
 
 #endif /* SCMI_DXE_H_ */
-- 
'Guid(CE165669-3EF3-493F-B85D-6190EE5B9759)'