Merge pull request #59 from zyp/add_halfwords

Add support for halfword memory accesses.

src/adiv5.c

@@ -46,6 +46,8 @@ static int ap_check_error(struct target_s *target);
 
 static int ap_mem_read_words(struct target_s *target, uint32_t *dest, uint32_t src, int len);
 static int ap_mem_write_words(struct target_s *target, uint32_t dest, const uint32_t *src, int len);
+static int ap_mem_read_halfwords(struct target_s *target, uint16_t *dest, uint32_t src, int len);
+static int ap_mem_write_halfwords(struct target_s *target, uint32_t dest, const uint16_t *src, int len);
 static int ap_mem_read_bytes(struct target_s *target, uint8_t *dest, uint32_t src, int len);
 static int ap_mem_write_bytes(struct target_s *target, uint32_t dest, const uint8_t *src, int len);
 
@@ -152,6 +154,8 @@ void adiv5_dp_init(ADIv5_DP_t *dp)
 
 		t->mem_read_words = ap_mem_read_words;
 		t->mem_write_words = ap_mem_write_words;
+		t->mem_read_halfwords = ap_mem_read_halfwords;
+		t->mem_write_halfwords = ap_mem_write_halfwords;
 		t->mem_read_bytes = ap_mem_read_bytes;
 		t->mem_write_bytes = ap_mem_write_bytes;
 
@@ -219,6 +223,42 @@ ap_mem_read_words(struct target_s *target, uint32_t *dest, uint32_t src, int len
 	return 0;
 }
 
+static int
+ap_mem_read_halfwords(struct target_s *target, uint16_t *dest, uint32_t src, int len)
+{
+	ADIv5_AP_t *ap = adiv5_target_ap(target);
+	uint32_t tmp;
+	uint32_t osrc = src;
+
+	len >>= 1;
+
+	adiv5_ap_write(ap, ADIV5_AP_CSW, ap->csw |
+		ADIV5_AP_CSW_SIZE_HALFWORD | ADIV5_AP_CSW_ADDRINC_SINGLE);
+	adiv5_dp_low_access(ap->dp, ADIV5_LOW_AP, ADIV5_LOW_WRITE,
+					ADIV5_AP_TAR, src);
+	adiv5_dp_low_access(ap->dp, ADIV5_LOW_AP, ADIV5_LOW_READ,
+					ADIV5_AP_DRW, 0);
+	while(--len) {
+		tmp = adiv5_dp_low_access(ap->dp, ADIV5_LOW_AP, ADIV5_LOW_READ,
+					ADIV5_AP_DRW, 0);
+		*dest++ = (tmp >> ((src & 0x2) << 3) & 0xFFFF);
+
+		src += 2;
+		/* Check for 10 bit address overflow */
+		if ((src ^ osrc) & 0xfffffc00) {
+			osrc = src;
+			adiv5_dp_low_access(ap->dp, ADIV5_LOW_AP,
+					ADIV5_LOW_WRITE, ADIV5_AP_TAR, src);
+			adiv5_dp_low_access(ap->dp, ADIV5_LOW_AP,
+					ADIV5_LOW_READ, ADIV5_AP_DRW, 0);
+		}
+	}
+	tmp = adiv5_dp_low_access(ap->dp, 0, 1, ADIV5_DP_RDBUFF, 0);
+	*dest++ = (tmp >> ((src & 0x2) << 3) & 0xFFFF);
+
+	return 0;
+}
+
 static int
 ap_mem_read_bytes(struct target_s *target, uint8_t *dest, uint32_t src, int len)
 {
@@ -280,6 +320,33 @@ ap_mem_write_words(struct target_s *target, uint32_t dest, const uint32_t *src,
 	return 0;
 }
 
+static int
+ap_mem_write_halfwords(struct target_s *target, uint32_t dest, const uint16_t *src, int len)
+{
+	ADIv5_AP_t *ap = adiv5_target_ap(target);
+	uint32_t odest = dest;
+
+	len >>= 1;
+
+	adiv5_ap_write(ap, ADIV5_AP_CSW, ap->csw |
+		ADIV5_AP_CSW_SIZE_HALFWORD | ADIV5_AP_CSW_ADDRINC_SINGLE);
+	adiv5_dp_low_access(ap->dp, ADIV5_LOW_AP, ADIV5_LOW_WRITE,
+					ADIV5_AP_TAR, dest);
+	while(len--) {
+		uint32_t tmp = (uint32_t)*src++ << ((dest & 2) << 3);
+		adiv5_dp_low_access(ap->dp, ADIV5_LOW_AP, ADIV5_LOW_WRITE,
+					ADIV5_AP_DRW, tmp);
+		dest += 2;
+		/* Check for 10 bit address overflow */
+		if ((dest ^ odest) & 0xfffffc00) {
+			odest = dest;
+			adiv5_dp_low_access(ap->dp, ADIV5_LOW_AP,
+					ADIV5_LOW_WRITE, ADIV5_AP_TAR, dest);
+		}
+	}
+	return 0;
+}
+
 static int
 ap_mem_write_bytes(struct target_s *target, uint32_t dest, const uint8_t *src, int len)
 {

src/arm7tdmi.c

@@ -129,6 +129,8 @@ void arm7tdmi_jtag_handler(jtag_dev_t *dev)
 	t->check_error = (void *)do_nothing;
 	t->mem_read_words = (void *)do_nothing;
 	t->mem_write_words = (void *)do_nothing;
+	t->mem_read_halfwords = (void *)do_nothing;
+	t->mem_write_halfwords = (void *)do_nothing;
 	t->mem_read_bytes = (void *)do_nothing;
 	t->mem_write_bytes = (void *)do_nothing;
 	t->regs_size = 16 * sizeof(uint32_t);

src/gdb_main.c

@@ -101,6 +101,8 @@ gdb_main(void)
 			uint8_t mem[len];
 			if(((addr & 3) == 0) && ((len & 3) == 0))
 				target_mem_read_words(cur_target, (void*)mem, addr, len);
+			else if(((addr & 1) == 0) && ((len & 1) == 0))
+				target_mem_read_halfwords(cur_target, (void*)mem, addr, len);
 			else
 				target_mem_read_bytes(cur_target, (void*)mem, addr, len);
 			if(target_check_error(cur_target))
@@ -127,6 +129,8 @@ gdb_main(void)
 			unhexify(mem, pbuf + hex, len);
 			if(((addr & 3) == 0) && ((len & 3) == 0))
 				target_mem_write_words(cur_target, addr, (void*)mem, len);
+			else if(((addr & 1) == 0) && ((len & 1) == 0))
+				target_mem_write_halfwords(cur_target, addr, (void*)mem, len);
 			else
 				target_mem_write_bytes(cur_target, addr, (void*)mem, len);
 			if(target_check_error(cur_target))

src/include/target.h

@@ -56,6 +56,12 @@ target *target_attach(target *t, target_destroy_callback destroy_cb);
 #define target_mem_write_words(target, dest, src, len)	\
 	(target)->mem_write_words((target), (dest), (src), (len))
 
+#define target_mem_read_halfwords(target, dest, src, len)	\
+	(target)->mem_read_halfwords((target), (dest), (src), (len))
+
+#define target_mem_write_halfwords(target, dest, src, len)	\
+	(target)->mem_write_halfwords((target), (dest), (src), (len))
+
 #define target_mem_read_bytes(target, dest, src, len)	\
 	(target)->mem_read_bytes((target), (dest), (src), (len))
 
@@ -136,6 +142,11 @@ struct target_s {
 	int (*mem_write_words)(struct target_s *target, uint32_t dest,
 				const uint32_t *src, int len);
 
+	int (*mem_read_halfwords)(struct target_s *target, uint16_t *dest, uint32_t src,
+				int len);
+	int (*mem_write_halfwords)(struct target_s *target, uint32_t dest,
+				const uint16_t *src, int len);
+
 	int (*mem_read_bytes)(struct target_s *target, uint8_t *dest, uint32_t src,
 				int len);
 	int (*mem_write_bytes)(struct target_s *target, uint32_t dest,