+ u32 outvalue;
+ int retval = ERROR_OK;
+ int wcount, blocksize, writecount, i;
+
+ swjdp->trans_mode = TRANS_MODE_COMPOSITE;
+
+ wcount = count;
+
+ while (wcount > 0)
+ {
+ int nbytes;
+
+ /* Adjust to read within 4K block boundaries */
+ blocksize = (0x1000 - (0xFFF & address));
+
+ if (wcount < blocksize)
+ blocksize = wcount;
+
+ ahbap_setup_accessport(swjdp, CSW_8BIT | CSW_ADDRINC_PACKED, address);
+ writecount = blocksize;
+
+ do
+ {
+ nbytes = MIN(writecount, 4);
+
+ if (nbytes < 4 )
+ {
+ if (ahbap_write_buf_u8(swjdp, buffer, nbytes, address) != ERROR_OK)
+ {
+ LOG_WARNING("Block read error address 0x%x, count 0x%x", address, count);
+ return ERROR_JTAG_DEVICE_ERROR;
+ }
+
+ address += nbytes;
+ }
+ else
+ {
+ outvalue = *((u32*)buffer);
+
+ for (i = 0; i < nbytes; i++ )
+ {
+ *((u8*)buffer + (address & 0x3)) = outvalue;
+ outvalue >>= 8;
+ address++;
+ }
+
+ outvalue = *((u32*)buffer);
+ ahbap_write_reg_u32(swjdp, AHBAP_DRW, outvalue);
+ if (swjdp_transaction_endcheck(swjdp) != ERROR_OK)
+ {
+ LOG_WARNING("Block read error address 0x%x, count 0x%x", address, count);
+ return ERROR_JTAG_DEVICE_ERROR;
+ }
+ }
+
+ buffer += nbytes;
+ writecount -= nbytes;
+
+ } while (writecount);
+ wcount -= blocksize;
+ }
+
+ return retval;
+}
+
+int ahbap_write_buf_u8(swjdp_common_t *swjdp, u8 *buffer, int count, u32 address)
+{
+ u32 outvalue;
+ int retval = ERROR_OK;
+
+ if (count >= 4)
+ return ahbap_write_buf_packed_u8(swjdp, buffer, count, address);