+ int nbytes;
+
+ /* Adjust to read within 4K block boundaries */
+ blocksize = (0x1000 - (0xFFF & address)) >> 1;
+
+ if (wcount < blocksize)
+ blocksize = wcount;
+
+ /* handle unaligned data at 4k boundary */
+ if (blocksize == 0)
+ blocksize = 1;
+
+ ahbap_setup_accessport(swjdp, CSW_16BIT | CSW_ADDRINC_PACKED, address);
+ writecount = blocksize;
+
+ do
+ {
+ nbytes = MIN((writecount << 1), 4);
+
+ if (nbytes < 4 )
+ {
+ if (ahbap_write_buf_u16(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 >> 1;
+ }
+ 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 >> 1;
+ writecount -= nbytes >> 1;
+
+ } while (writecount);
+ wcount -= blocksize;
+ }
+
+ return retval;
+}
+
+int ahbap_write_buf_u16(swjdp_common_t *swjdp, u8 *buffer, int count, u32 address)
+{
+ u32 outvalue;
+ int retval = ERROR_OK;
+
+ if (count >= 4)
+ return ahbap_write_buf_packed_u16(swjdp, buffer, count, address);
+
+ swjdp->trans_mode = TRANS_MODE_COMPOSITE;
+
+ while (count > 0)
+ {
+ ahbap_setup_accessport(swjdp, CSW_16BIT | CSW_ADDRINC_SINGLE, address);
+ outvalue = *((u16*)buffer) << 8 * (address & 0x3);