arm_adi_v5: fix regression from 4553abf9064fe3c0e4ea2ed29a1d2217df74ff5f

Functions mem_ap_read() and mem_ap_write() incremented address even
if addrinc=false. I overlooked this fact and moved mem_ap_setup_tar()
set wrong addresses in no-incr mode.

Fixed by preventing address increment in no-incr mode.

Change-Id: I512e12a6a64e30cf6bc5bf77e3d57d35cc33e058
Signed-off-by: Tomas Vanek <vanekt@fbl.cz>
Suggested-by: Matthias Welwarsky <matthias@welwarsky.de>
Reviewed-on: http://openocd.zylin.com/4326
Tested-by: jenkins
Reviewed-by: Christopher Head <chead@zaber.com>
Reviewed-by: Andreas Fritiofson <andreas.fritiofson@gmail.com>

diff --git a/src/target/arm_adi_v5.c b/src/target/arm_adi_v5.c
index ae6c2c629fdb62f71bf8ad60ef8aebd843ee8b49..1ca8e4f0043ee7c31fcc7dba1262806ee3cde886 100644 (file)
--- a/src/target/arm_adi_v5.c
+++ b/src/target/arm_adi_v5.c
@@ -378,33 +378,34 @@ static int mem_ap_write(struct adiv5_ap *ap, const uint8_t *buffer, uint32_t siz
                /* How many source bytes each transfer will consume, and their location in the DRW,
                 * depends on the type of transfer and alignment. See ARM document IHI0031C. */
                uint32_t outvalue = 0;
                /* How many source bytes each transfer will consume, and their location in the DRW,
                 * depends on the type of transfer and alignment. See ARM document IHI0031C. */
                uint32_t outvalue = 0;

+               uint32_t drw_byte_idx = address;

                if (dap->ti_be_32_quirks) {
                        switch (this_size) {
                        case 4:
                if (dap->ti_be_32_quirks) {
                        switch (this_size) {
                        case 4:

-                               outvalue |= (uint32_t)*buffer++ << 8 * (3 ^ (address++ & 3) ^ addr_xor);
-                               outvalue |= (uint32_t)*buffer++ << 8 * (3 ^ (address++ & 3) ^ addr_xor);
-                               outvalue |= (uint32_t)*buffer++ << 8 * (3 ^ (address++ & 3) ^ addr_xor);
-                               outvalue |= (uint32_t)*buffer++ << 8 * (3 ^ (address++ & 3) ^ addr_xor);
+                               outvalue |= (uint32_t)*buffer++ << 8 * (3 ^ (drw_byte_idx++ & 3) ^ addr_xor);
+                               outvalue |= (uint32_t)*buffer++ << 8 * (3 ^ (drw_byte_idx++ & 3) ^ addr_xor);
+                               outvalue |= (uint32_t)*buffer++ << 8 * (3 ^ (drw_byte_idx++ & 3) ^ addr_xor);
+                               outvalue |= (uint32_t)*buffer++ << 8 * (3 ^ (drw_byte_idx & 3) ^ addr_xor);

                                break;
                        case 2:
                                break;
                        case 2:

-                               outvalue |= (uint32_t)*buffer++ << 8 * (1 ^ (address++ & 3) ^ addr_xor);
-                               outvalue |= (uint32_t)*buffer++ << 8 * (1 ^ (address++ & 3) ^ addr_xor);
+                               outvalue |= (uint32_t)*buffer++ << 8 * (1 ^ (drw_byte_idx++ & 3) ^ addr_xor);
+                               outvalue |= (uint32_t)*buffer++ << 8 * (1 ^ (drw_byte_idx & 3) ^ addr_xor);

                                break;
                        case 1:
                                break;
                        case 1:

-                               outvalue |= (uint32_t)*buffer++ << 8 * (0 ^ (address++ & 3) ^ addr_xor);
+                               outvalue |= (uint32_t)*buffer++ << 8 * (0 ^ (drw_byte_idx & 3) ^ addr_xor);

                                break;
                        }
                } else {
                        switch (this_size) {
                        case 4:
                                break;
                        }
                } else {
                        switch (this_size) {
                        case 4:

-                               outvalue |= (uint32_t)*buffer++ << 8 * (address++ & 3);
-                               outvalue |= (uint32_t)*buffer++ << 8 * (address++ & 3);
+                               outvalue |= (uint32_t)*buffer++ << 8 * (drw_byte_idx++ & 3);
+                               outvalue |= (uint32_t)*buffer++ << 8 * (drw_byte_idx++ & 3);

                                /* fallthrough */
                        case 2:
                                /* fallthrough */
                        case 2:

-                               outvalue |= (uint32_t)*buffer++ << 8 * (address++ & 3);
+                               outvalue |= (uint32_t)*buffer++ << 8 * (drw_byte_idx++ & 3);

                                /* fallthrough */
                        case 1:
                                /* fallthrough */
                        case 1:

-                               outvalue |= (uint32_t)*buffer++ << 8 * (address++ & 3);
+                               outvalue |= (uint32_t)*buffer++ << 8 * (drw_byte_idx & 3);

                        }
                }
 
                        }
                }
 


@@ -415,6 +416,8 @@ static int mem_ap_write(struct adiv5_ap *ap, const uint8_t *buffer, uint32_t siz
                        break;
 
                mem_ap_update_tar_cache(ap);
                        break;
 
                mem_ap_update_tar_cache(ap);

+               if (addrinc)
+                       address += this_size;

        }
 
        /* REVISIT: Might want to have a queued version of this function that does not run. */
        }
 
        /* REVISIT: Might want to have a queued version of this function that does not run. */


@@ -509,7 +512,8 @@ static int mem_ap_read(struct adiv5_ap *ap, uint8_t *buffer, uint32_t size, uint
                        break;
 
                nbytes -= this_size;
                        break;
 
                nbytes -= this_size;

-               address += this_size;
+               if (addrinc)
+                       address += this_size;

 
                mem_ap_update_tar_cache(ap);
        }
 
                mem_ap_update_tar_cache(ap);
        }