From 54f73858f97443c8a604a8f209c29d25656d2d53 Mon Sep 17 00:00:00 2001
From: Uwe Bonnes <bon@elektron.ikp.physik.tu-darmstadt.de>
Date: Thu, 31 May 2018 21:39:19 +0200
Subject: [PATCH] Provide a target function to write with given size.

---
 src/target/adiv5.c   | 19 ++++++++++++-------
 src/target/adiv5.h   |  9 +++++++++
 src/target/cortexm.c |  8 ++++++++
 src/target/cortexm.h |  2 ++
 4 files changed, 31 insertions(+), 7 deletions(-)

diff --git a/src/target/adiv5.c b/src/target/adiv5.c
index 8d86936..b6236d8 100644
--- a/src/target/adiv5.c
+++ b/src/target/adiv5.c
@@ -457,11 +457,6 @@ void adiv5_dp_init(ADIv5_DP_t *dp)
 	adiv5_dp_unref(dp);
 }
 
-enum align {
-	ALIGN_BYTE =  0,
-	ALIGN_HALFWORD = 1,
-	ALIGN_WORD = 2
-};
 #define ALIGNOF(x) (((x) & 3) == 0 ? ALIGN_WORD : \
                     (((x) & 1) == 0 ? ALIGN_HALFWORD : ALIGN_BYTE))
 
@@ -477,6 +472,7 @@ static void ap_mem_access_setup(ADIv5_AP_t *ap, uint32_t addr, enum align align)
 	case ALIGN_HALFWORD:
 		csw |= ADIV5_AP_CSW_SIZE_HALFWORD;
 		break;
+	case ALIGN_DWORD:
 	case ALIGN_WORD:
 		csw |= ADIV5_AP_CSW_SIZE_WORD;
 		break;
@@ -495,6 +491,7 @@ static void * extract(void *dest, uint32_t src, uint32_t val, enum align align)
 	case ALIGN_HALFWORD:
 		*(uint16_t *)dest = (val >> ((src & 0x2) << 3) & 0xFFFF);
 		break;
+	case ALIGN_DWORD:
 	case ALIGN_WORD:
 		*(uint32_t *)dest = val;
 		break;
@@ -534,10 +531,10 @@ adiv5_mem_read(ADIv5_AP_t *ap, void *dest, uint32_t src, size_t len)
 }
 
 void
-adiv5_mem_write(ADIv5_AP_t *ap, uint32_t dest, const void *src, size_t len)
+adiv5_mem_write_sized(ADIv5_AP_t *ap, uint32_t dest, const void *src,
+					  size_t len, enum align align)
 {
 	uint32_t odest = dest;
-	enum align align = MIN(ALIGNOF(dest), ALIGNOF(len));
 
 	len >>= align;
 	ap_mem_access_setup(ap, dest, align);
@@ -551,6 +548,7 @@ adiv5_mem_write(ADIv5_AP_t *ap, uint32_t dest, const void *src, size_t len)
 		case ALIGN_HALFWORD:
 			tmp = ((uint32_t)*(uint16_t *)src) << ((dest & 2) << 3);
 			break;
+		case ALIGN_DWORD:
 		case ALIGN_WORD:
 			tmp = *(uint32_t *)src;
 			break;
@@ -568,6 +566,13 @@ adiv5_mem_write(ADIv5_AP_t *ap, uint32_t dest, const void *src, size_t len)
 	}
 }
 
+void
+adiv5_mem_write(ADIv5_AP_t *ap, uint32_t dest, const void *src, size_t len)
+{
+	enum align align = MIN(ALIGNOF(dest), ALIGNOF(len));
+	adiv5_mem_write_sized(ap, dest, src, len, align);
+}
+
 void adiv5_ap_write(ADIv5_AP_t *ap, uint16_t addr, uint32_t value)
 {
 	adiv5_dp_write(ap->dp, ADIV5_DP_SELECT,
diff --git a/src/target/adiv5.h b/src/target/adiv5.h
index ca81471..2929a31 100644
--- a/src/target/adiv5.h
+++ b/src/target/adiv5.h
@@ -100,6 +100,13 @@
 #define ADIV5_LOW_WRITE		0
 #define ADIV5_LOW_READ		1
 
+enum align {
+	ALIGN_BYTE     = 0,
+	ALIGN_HALFWORD = 1,
+	ALIGN_WORD     = 2,
+	ALIGN_DWORD    = 3
+};
+
 /* Try to keep this somewhat absract for later adding SW-DP */
 typedef struct ADIv5_DP_s {
 	int refcnt;
@@ -167,6 +174,8 @@ void adiv5_jtag_dp_handler(jtag_dev_t *dev);
 
 void adiv5_mem_read(ADIv5_AP_t *ap, void *dest, uint32_t src, size_t len);
 void adiv5_mem_write(ADIv5_AP_t *ap, uint32_t dest, const void *src, size_t len);
+void adiv5_mem_write_sized(ADIv5_AP_t *ap, uint32_t dest, const void *src,
+						   size_t len, enum align align);
 
 #endif
 
diff --git a/src/target/cortexm.c b/src/target/cortexm.c
index 33344d6..dba2349 100644
--- a/src/target/cortexm.c
+++ b/src/target/cortexm.c
@@ -459,6 +459,14 @@ static void cortexm_regs_write(target *t, const void *data)
 		}
 }
 
+int cortexm_mem_write_sized(
+	target *t, target_addr dest, const void *src, size_t len, enum align align)
+{
+	cortexm_cache_clean(t, dest, len, true);
+	adiv5_mem_write_sized(cortexm_ap(t), dest, src, len, align);
+	return target_check_error(t);
+}
+
 static uint32_t cortexm_pc_read(target *t)
 {
 	target_mem_write32(t, CORTEXM_DCRSR, 0x0F);
diff --git a/src/target/cortexm.h b/src/target/cortexm.h
index e9bf547..9aa4733 100644
--- a/src/target/cortexm.h
+++ b/src/target/cortexm.h
@@ -175,6 +175,8 @@ void cortexm_detach(target *t);
 void cortexm_halt_resume(target *t, bool step);
 int cortexm_run_stub(target *t, uint32_t loadaddr,
                      uint32_t r0, uint32_t r1, uint32_t r2, uint32_t r3);
+int cortexm_mem_write_sized(
+	target *t, target_addr dest, const void *src, size_t len, enum align align);
 
 #endif