diff --git a/src/adiv5.c b/src/adiv5.c index 45bf94f..e7f45c9 100644 --- a/src/adiv5.c +++ b/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) { diff --git a/src/arm7tdmi.c b/src/arm7tdmi.c index 26c5233..1c7b443 100644 --- a/src/arm7tdmi.c +++ b/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); diff --git a/src/gdb_main.c b/src/gdb_main.c index c99ebac..dc56a59 100644 --- a/src/gdb_main.c +++ b/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)) diff --git a/src/include/target.h b/src/include/target.h index 067b051..f28f2f8 100644 --- a/src/include/target.h +++ b/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,