[platforms: PATCH v2 06/10] Marvell/Library: MppLib: Take 0xFF placeholders into account

[Marcin Wojtas <mw@semihalf.com>]

From: Ard Biesheuvel <ard.biesheuvel@linaro.org>

The MppSel definition PCDs contain 0xFF placeholders for values that
should be left untouched. MppLib needs to be taught how to take those
into account.

Contributed-under: TianoCore Contribution Agreement 1.1
Signed-off-by: Ard Biesheuvel <ard.biesheuvel@linaro.org>
Signed-off-by: Marcin Wojtas <mw@semihalf.com>
---
 Platform/Marvell/Library/MppLib/MppLib.c | 14 ++++++++++----
 1 file changed, 10 insertions(+), 4 deletions(-)

diff --git a/Platform/Marvell/Library/MppLib/MppLib.c b/Platform/Marvell/Library/MppLib/MppLib.c
index 383c820..297725f 100644
--- a/Platform/Marvell/Library/MppLib/MppLib.c
+++ b/Platform/Marvell/Library/MppLib/MppLib.c
@@ -79,18 +79,24 @@ SetRegisterValue (
   BOOLEAN ReverseFlag
   )
 {
-  UINT32 i, j, CtrlVal;
+  UINT32 i, j, CtrlVal, CtrlMask, PinIndex;
   INTN Sign;
 
   Sign = ReverseFlag ? -1 : 1;
 
   for (i = 0; i < RegCount; i++) {
     CtrlVal = 0;
+    CtrlMask = 0;
     for (j = 0; j < MPP_PINS_PER_REG; j++) {
-      CtrlVal |= MPP_PIN_VAL(7 * (UINTN) ReverseFlag + j * Sign,
-        MppRegPcd[i][7 * (UINTN) ReverseFlag + j * Sign]);
+
+      PinIndex = 7 * (UINTN)ReverseFlag + j * Sign;
+
+      if (MppRegPcd[i][PinIndex] != 0xff) {
+        CtrlVal |= MPP_PIN_VAL(PinIndex, MppRegPcd[i][PinIndex]);
+        CtrlMask |= MPP_PIN_VAL(PinIndex, 0xf);
+      }
     }
-    MmioWrite32 (BaseAddr + 4 * i * Sign, CtrlVal);
+    MmioAndThenOr32 (BaseAddr + 4 * i * Sign, ~CtrlMask, CtrlVal);
   }
 }
 
-- 
2.7.4