[Pdbg] [PATCH 1/5] libpdbg: Add fsi_write_mask() to write cfam 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 711c817..c72a96c 100644
--- a/libpdbg/libpdbg.h
+++ b/libpdbg/libpdbg.h
@@ -128,6 +128,7 @@ struct pdbg_target *pdbg_address_absolute(struct pdbg_target *target, uint64_t *
 /* Procedures */
 int fsi_read(struct pdbg_target *target, uint32_t addr, uint32_t *val);
 int fsi_write(struct pdbg_target *target, uint32_t addr, uint32_t val);
+int fsi_write_mask(struct pdbg_target *target, uint32_t addr, uint32_t val, uint32_t mask);
 
 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);
diff --git a/libpdbg/target.c b/libpdbg/target.c
index e193e7e..a5dcba8 100644
--- a/libpdbg/target.c
+++ b/libpdbg/target.c
@@ -217,6 +217,20 @@ int fsi_write(struct pdbg_target *fsi_dt, uint32_t addr, uint32_t data)
 	return fsi->write(fsi, addr64, data);
 }
 
+int fsi_write_mask(struct pdbg_target *fsi_dt, uint32_t addr, uint32_t data, uint32_t mask)
+{
+	uint32_t value;
+	int rc;
+
+	rc = fsi_read(fsi_dt, addr, &value);
+	if (rc)
+		return rc;
+
+	value = (value & ~mask) | (data & mask);
+
+	return fsi_write(fsi_dt, addr, value);
+}
+
 int mem_read(struct pdbg_target *target, uint64_t addr, uint8_t *output, uint64_t size, uint8_t block_size, bool ci)
 {
 	int rc = -1;
-- 
2.21.0