Fix putScomUnderMask

Change-Id: I06452679730b236a4e00f4df89265c3f8fcf5a52
Reviewed-on: http://ralgit01.raleigh.ibm.com/gerrit1/41487
Tested-by: Jenkins Server <pfd-jenkins+hostboot@us.ibm.com>
Tested-by: FSP CI Jenkins <fsp-CI-jenkins+hostboot@us.ibm.com>
Reviewed-by: Benjamin Gass <bgass@us.ibm.com>
Reviewed-by: Sachin Gupta <sgupta2m@in.ibm.com>

1 files changed, 26 insertions, 0 deletions

diff --git a/src/hwpf/plat_hw_access.C b/src/hwpf/plat_hw_access.C
index bdf6ae3b..99f92a39 100644
--- a/src/hwpf/plat_hw_access.C
+++ b/src/hwpf/plat_hw_access.C
@@ -315,6 +315,32 @@ fapi_try_exit:
     return fapi2::current_err;
 }
 
+
+fapi2::ReturnCode putscom_under_mask(const void *i_target,
+                                     uint32_t i_addr,
+                                     uint64_t i_data,
+                                     uint64_t i_mask)
+{
+    uint64_t l_read = 0;
+    fapi2::ReturnCode l_rc = FAPI2_RC_SUCCESS;
+
+    do
+    {
+        l_rc = getscom_abs_wrap(i_target, i_addr, &l_read);
+
+        if(l_rc)
+        {
+            break;
+        }
+
+        l_read = (l_read & ~i_mask) | (i_data & i_mask);
+
+        l_rc = putscom_abs_wrap(i_target, i_addr, l_read);
+    } while (false);
+
+    return l_rc;
+}
+
 uint32_t p9_pibErrRetry( const uint32_t i_addr, uint64_t *io_data,
                          const uint8_t i_pibErr, const bool i_isRead)
 {