zcy
/
tinyriscv-openocd
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

arm_adi_v5: Fix packed and unaligned memory writes 

For packed and/or unaligned accesses, the write functions reordered the
source buffer in place. Causing in the best case a segfault, in the worst
case silent data corruption.

Rewrite the data fetching to directly match the byte lane mapping
according to IHI0031C, without destroying the buffer.

Also slightly clean up variable usage and harmonize all the write
functions.

Change-Id: I9a01cfc5578653f9ec02043ff6b61a7a20f90d67
Signed-off-by: Andreas Fritiofson <andreas.fritiofson@gmail.com>
Reviewed-on: http://openocd.zylin.com/1646
Tested-by: jenkins
Reviewed-by: Spencer Oliver <spen@spen-soft.co.uk>
__archive__
Andreas Fritiofson 2013-09-22 18:50:25 +02:00 committed by Spencer Oliver
parent 4e79b48e2c
commit 2ab5d672ea
1 changed files with 37 additions and 68 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

105
src/target/arm_adi_v5.c
View File

@ -261,41 +261,16 @@ int mem_ap_write_atomic_u32(struct adiv5_dap *dap, uint32_t address,
return dap_run(dap); return dap_run(dap);
} }
/*****************************************************************************
* *
* mem_ap_write_buf(struct adiv5_dap *dap, uint8_t *buffer, int count, uint32_t address, bool addr_incr) *
* *
* Write a buffer in target order (little endian) *
* *
*****************************************************************************/
int mem_ap_write_buf_u32(struct adiv5_dap *dap, const uint8_t *buffer, int count, uint32_t address, bool addr_incr) int mem_ap_write_buf_u32(struct adiv5_dap *dap, const uint8_t *buffer, int count, uint32_t address, bool addr_incr)
{ {
int wcount, blocksize, writecount, errorcount = 0, retval = ERROR_OK; int wcount, blocksize, writecount, errorcount = 0, retval = ERROR_OK;
uint32_t adr = address; uint32_t adr = address;
const uint8_t *pBuffer = buffer; uint32_t incr_flag = addr_incr ? CSW_ADDRINC_SINGLE : CSW_ADDRINC_OFF;
uint32_t incr_flag = CSW_ADDRINC_OFF;
count >>= 2; wcount = count >> 2;
wcount = count;
/* if we have an unaligned access - reorder data */
if (adr & 0x3u) {
for (writecount = 0; writecount < count; writecount++) {
int i;
uint32_t outvalue;
memcpy(&outvalue, pBuffer, sizeof(uint32_t));
for (i = 0; i < 4; i++) {
*((uint8_t *)pBuffer + (adr & 0x3)) = outvalue;
outvalue >>= 8;
adr++;
}
pBuffer += sizeof(uint32_t);
}
}
while (wcount > 0) { while (wcount > 0) {
/* Adjust to write blocks within boundaries aligned to the TAR autoincremnent size*/ /* Adjust to write blocks within boundaries aligned to the TAR auto-increment size */
blocksize = max_tar_block_size(dap->tar_autoincr_block, address); blocksize = max_tar_block_size(dap->tar_autoincr_block, address);
if (wcount < blocksize) if (wcount < blocksize)
blocksize = wcount; blocksize = wcount;
@ -304,27 +279,27 @@ int mem_ap_write_buf_u32(struct adiv5_dap *dap, const uint8_t *buffer, int count
if (blocksize == 0) if (blocksize == 0)
blocksize = 1; blocksize = 1;
if (addr_incr)
incr_flag = CSW_ADDRINC_SINGLE;
retval = dap_setup_accessport(dap, CSW_32BIT | incr_flag, address); retval = dap_setup_accessport(dap, CSW_32BIT | incr_flag, address);
if (retval != ERROR_OK) if (retval != ERROR_OK)
return retval; return retval;
for (writecount = 0; writecount < blocksize; writecount++) { for (writecount = 0; writecount < blocksize; writecount++) {
uint32_t tmp; uint32_t outvalue = 0;
tmp = buf_get_u32(buffer + 4 * writecount, 0, 32); outvalue |= (uint32_t)*buffer++ << 8 * (adr++ & 3);
retval = dap_queue_ap_write(dap, AP_REG_DRW, tmp); outvalue |= (uint32_t)*buffer++ << 8 * (adr++ & 3);
outvalue |= (uint32_t)*buffer++ << 8 * (adr++ & 3);
outvalue |= (uint32_t)*buffer++ << 8 * (adr++ & 3);
retval = dap_queue_ap_write(dap, AP_REG_DRW, outvalue);
if (retval != ERROR_OK) if (retval != ERROR_OK)
break; break;
} }
retval = dap_run(dap); retval = dap_run(dap);
if (retval == ERROR_OK) { if (retval == ERROR_OK) {
wcount = wcount - blocksize; wcount -= blocksize;
if (addr_incr) if (addr_incr)
address = address + 4 * blocksize; address += 4 * blocksize;
buffer = buffer + 4 * blocksize;
} else } else
errorcount++; errorcount++;
@ -341,14 +316,14 @@ static int mem_ap_write_buf_packed_u16(struct adiv5_dap *dap,
const uint8_t *buffer, int count, uint32_t address) const uint8_t *buffer, int count, uint32_t address)
{ {
int retval = ERROR_OK; int retval = ERROR_OK;
int wcount, blocksize, writecount, i; int wcount, blocksize, writecount;
wcount = count >> 1; wcount = count >> 1;
while (wcount > 0) { while (wcount > 0) {
int nbytes; int nbytes;
/* Adjust to write blocks within boundaries aligned to the TAR autoincremnent size*/ /* Adjust to write blocks within boundaries aligned to the TAR auto-increment size */
blocksize = max_tar_block_size(dap->tar_autoincr_block, address); blocksize = max_tar_block_size(dap->tar_autoincr_block, address);
if (wcount < blocksize) if (wcount < blocksize)
@ -376,18 +351,17 @@ static int mem_ap_write_buf_packed_u16(struct adiv5_dap *dap,
return retval; return retval;
} }
address += nbytes >> 1; address += nbytes;
buffer += nbytes;
} else { } else {
uint32_t outvalue; assert(nbytes == 4);
memcpy(&outvalue, buffer, sizeof(uint32_t));
for (i = 0; i < nbytes; i++) { uint32_t outvalue = 0;
*((uint8_t *)buffer + (address & 0x3)) = outvalue; outvalue |= (uint32_t)*buffer++ << 8 * (address++ & 3);
outvalue >>= 8; outvalue |= (uint32_t)*buffer++ << 8 * (address++ & 3);
address++; outvalue |= (uint32_t)*buffer++ << 8 * (address++ & 3);
} outvalue |= (uint32_t)*buffer++ << 8 * (address++ & 3);
memcpy(&outvalue, buffer, sizeof(uint32_t));
retval = dap_queue_ap_write(dap, retval = dap_queue_ap_write(dap,
AP_REG_DRW, outvalue); AP_REG_DRW, outvalue);
if (retval != ERROR_OK) if (retval != ERROR_OK)
@ -402,7 +376,6 @@ static int mem_ap_write_buf_packed_u16(struct adiv5_dap *dap,
} }
} }
buffer += nbytes >> 1;
writecount -= nbytes >> 1; writecount -= nbytes >> 1;
} while (writecount); } while (writecount);
@ -423,9 +396,11 @@ int mem_ap_write_buf_u16(struct adiv5_dap *dap, const uint8_t *buffer, int count
retval = dap_setup_accessport(dap, CSW_16BIT | CSW_ADDRINC_SINGLE, address); retval = dap_setup_accessport(dap, CSW_16BIT | CSW_ADDRINC_SINGLE, address);
if (retval != ERROR_OK) if (retval != ERROR_OK)
return retval; return retval;
uint16_t svalue;
memcpy(&svalue, buffer, sizeof(uint16_t)); uint32_t outvalue = 0;
uint32_t outvalue = (uint32_t)svalue << 8 * (address & 0x3); outvalue |= (uint32_t)*buffer++ << 8 * (address++ & 3);
outvalue |= (uint32_t)*buffer++ << 8 * (address++ & 3);
retval = dap_queue_ap_write(dap, AP_REG_DRW, outvalue); retval = dap_queue_ap_write(dap, AP_REG_DRW, outvalue);
if (retval != ERROR_OK) if (retval != ERROR_OK)
break; break;
@ -435,8 +410,6 @@ int mem_ap_write_buf_u16(struct adiv5_dap *dap, const uint8_t *buffer, int count
break; break;
count -= 2; count -= 2;
address += 2;
buffer += 2;
} }
return retval; return retval;
@ -446,14 +419,14 @@ static int mem_ap_write_buf_packed_u8(struct adiv5_dap *dap,
const uint8_t *buffer, int count, uint32_t address) const uint8_t *buffer, int count, uint32_t address)
{ {
int retval = ERROR_OK; int retval = ERROR_OK;
int wcount, blocksize, writecount, i; int wcount, blocksize, writecount;
wcount = count; wcount = count;
while (wcount > 0) { while (wcount > 0) {
int nbytes; int nbytes;
/* Adjust to write blocks within boundaries aligned to the TAR autoincremnent size*/ /* Adjust to write blocks within boundaries aligned to the TAR auto-increment size */
blocksize = max_tar_block_size(dap->tar_autoincr_block, address); blocksize = max_tar_block_size(dap->tar_autoincr_block, address);
if (wcount < blocksize) if (wcount < blocksize)
@ -477,17 +450,16 @@ static int mem_ap_write_buf_packed_u8(struct adiv5_dap *dap,
} }
address += nbytes; address += nbytes;
buffer += nbytes;
} else { } else {
uint32_t outvalue; assert(nbytes == 4);
memcpy(&outvalue, buffer, sizeof(uint32_t));
for (i = 0; i < nbytes; i++) { uint32_t outvalue = 0;
*((uint8_t *)buffer + (address & 0x3)) = outvalue; outvalue |= (uint32_t)*buffer++ << 8 * (address++ & 3);
outvalue >>= 8; outvalue |= (uint32_t)*buffer++ << 8 * (address++ & 3);
address++; outvalue |= (uint32_t)*buffer++ << 8 * (address++ & 3);
} outvalue |= (uint32_t)*buffer++ << 8 * (address++ & 3);
memcpy(&outvalue, buffer, sizeof(uint32_t));
retval = dap_queue_ap_write(dap, retval = dap_queue_ap_write(dap,
AP_REG_DRW, outvalue); AP_REG_DRW, outvalue);
if (retval != ERROR_OK) if (retval != ERROR_OK)
@ -502,7 +474,6 @@ static int mem_ap_write_buf_packed_u8(struct adiv5_dap *dap,
} }
} }
buffer += nbytes;
writecount -= nbytes; writecount -= nbytes;
} while (writecount); } while (writecount);
@ -523,7 +494,7 @@ int mem_ap_write_buf_u8(struct adiv5_dap *dap, const uint8_t *buffer, int count,
retval = dap_setup_accessport(dap, CSW_8BIT | CSW_ADDRINC_SINGLE, address); retval = dap_setup_accessport(dap, CSW_8BIT | CSW_ADDRINC_SINGLE, address);
if (retval != ERROR_OK) if (retval != ERROR_OK)
return retval; return retval;
uint32_t outvalue = (uint32_t)*buffer << 8 * (address & 0x3); uint32_t outvalue = (uint32_t)*buffer++ << 8 * (address++ & 0x3);
retval = dap_queue_ap_write(dap, AP_REG_DRW, outvalue); retval = dap_queue_ap_write(dap, AP_REG_DRW, outvalue);
if (retval != ERROR_OK) if (retval != ERROR_OK)
break; break;
@ -533,8 +504,6 @@ int mem_ap_write_buf_u8(struct adiv5_dap *dap, const uint8_t *buffer, int count,
break; break;
count--; count--;
address++;
buffer++;
} }
return retval; return retval;