[Pdbg] [PATCH 2/5] libpdbg: Add pib_write_mask() to write scom values with mask

[Amitay Isaacs]

Signed-off-by: Amitay Isaacs <amitay at ozlabs.org>
---
 libpdbg/libpdbg.h |  1 +
 libpdbg/target.c  | 14 ++++++++++++++
 2 files changed, 15 insertions(+)

diff --git a/libpdbg/libpdbg.h b/libpdbg/libpdbg.h
index c72a96c..f1eab4f 100644
--- a/libpdbg/libpdbg.h
+++ b/libpdbg/libpdbg.h
@@ -132,6 +132,7 @@ int fsi_write_mask(struct pdbg_target *target, uint32_t addr, uint32_t val, uint
 
 int pib_read(struct pdbg_target *target, uint64_t addr, uint64_t *val);
 int pib_write(struct pdbg_target *target, uint64_t addr, uint64_t val);
+int pib_write_mask(struct pdbg_target *target, uint64_t addr, uint64_t val, uint64_t mask);
 int pib_wait(struct pdbg_target *pib_dt, uint64_t addr, uint64_t mask, uint64_t data);
 
 struct thread_regs {
diff --git a/libpdbg/target.c b/libpdbg/target.c
index a5dcba8..6befe25 100644
--- a/libpdbg/target.c
+++ b/libpdbg/target.c
@@ -153,6 +153,20 @@ int pib_write(struct pdbg_target *pib_dt, uint64_t addr, uint64_t data)
 	return rc;
 }
 
+int pib_write_mask(struct pdbg_target *pib_dt, uint64_t addr, uint64_t data, uint64_t mask)
+{
+	uint64_t value;
+	int rc;
+
+	rc = pib_read(pib_dt, addr, &value);
+	if (rc)
+		return rc;
+
+	value = (value & ~mask) | (data & mask);
+
+	return pib_write(pib_dt, addr, value);
+}
+
 /* Wait for a SCOM register addr to match value & mask == data */
 int pib_wait(struct pdbg_target *pib_dt, uint64_t addr, uint64_t mask, uint64_t data)
 {
-- 
2.21.0