static int
hexnumstr (char *buf, ULONGEST num)
{
int i;
int len = hexnumlen (num);
buf[len] = '\0';
for (i = len - 1; i >= 0; i--)
{
buf[i] = "0123456789abcdef"[(num & 0xf)];
num >>= 4;
}
return len;
}
// Based on routines in gdb/remote.c
int
CeCosTestDownloadFilter::put_binary (unsigned char* buf, int len,
unsigned long dl_address,
int packet_size,
CeCosSerial& serial)
{
int i;
unsigned char csum;
Buffer buf2(packet_size);
unsigned char ch;
int tcount = 0;
unsigned char *p, *p2, *plen;
while (len > 0) {
/* Subtract header overhead from MAX payload size:
$M<memaddr>,<len>:#nn */
int max_buf_size =
buf2.Size()
- ( 2 + hexnumlen(dl_address) + 1 + hexnumlen(buf2.Size()) + 4);
/* Copy the packet into buffer BUF2, encapsulating it
and giving it a checksum. */
int todo = MIN (len, max_buf_size);
p = (unsigned char*) buf2.Data();
*p++ = '$';
// Add X header.
*p++ = 'X';
p += hexnumstr(p, dl_address);
*p++ = ',';
plen = p; /* remember where len field goes */
p += hexnumstr(p, todo);
*p++ = ':';
int escaped = 0;
for (i = 0;
(i < todo) && (i + escaped) < (max_buf_size - 2);
i++)
{
switch (buf[i] & 0xff)
{
case '$':
case '#':
case 0x7d:
/* These must be escaped */
escaped++;
*p++ = 0x7d;
*p++ = (unsigned char) ((buf[i] & 0xff) ^ 0x20);
break;
default:
*p++ = (unsigned char) (buf[i] & 0xff);
break;
}
}
if (i < todo)
{
/* Escape chars have filled up the buffer prematurely,
and we have actually sent fewer bytes than planned.
Fix-up the length field of the packet. */
/* FIXME: will fail if new len is a shorter string than
old len. */
plen += hexnumstr (plen, i);
*plen++ = ':';
}
// Calculate checksum
p2 = (unsigned char*)buf2.Data();
p2++; // skip $
csum = 0;
while (p2 < p)
csum = (unsigned char)(csum + *p2++);
*p++ = '#';
*p++ = (unsigned char) tohex ((csum >> 4) & 0xf);
*p++ = (unsigned char) tohex (csum & 0xf);
/* Send it over and over until we get a positive ack. */
int resend = 1;
const unsigned char* write_ptr = (const unsigned char*) buf2.Data();
int write_len = (int)p-(int)buf2.Data();
while (resend)
{
unsigned int __written;
Trace("Sending bytes for %p-%p\n", dl_address, dl_address+i);
TargetWrite(serial, write_ptr, write_len);
/* read until either a timeout occurs (-2) or '+' is read */
for(;;)
{
unsigned int __read;
serial.Read(&ch, 1, __read);
if (0 == __read) {
tcount ++;
if (tcount > 3) {
Trace("Timeout in putpkt_binary\n");
return 0;
}
break; /* Retransmit buffer */
}
switch (ch)
{
case '+':
// Now expect OK packet from target
unsigned char ok_msg[6];// $OK#9a
serial.Read(ok_msg, 6, __read);
// Reply with ACK
serial.Write((void*)"+", 1, __written);
// And process next packet.
resend = 0;
break;
case '-':
// Bad packet CRC. Retransmit.
Trace ("Bad CRC\n");
break;
default:
Trace("Got junk..%02x\n", ch);
continue; // keep reading
}
break; /* Here to retransmit */
}
}
len -= i;
dl_address += i;
buf += i;
}
return 1;
}
