int upload_program( unsigned int start, unsigned int length )
{
static unsigned char upload_program_string[] = "$UP ";
int count;
unsigned long check;
if (length * 3 > sizeof(buffer))
return 0;
upload_program_string[3] = start >> 8;
upload_program_string[4] = start;
upload_program_string[5] = length >> 8;
upload_program_string[6] = length;
write_drain( ttyfd, upload_program_string, sizeof(upload_program_string) - 1 );
/* bytes received not counting check sum */
count = received( 3 + 3 * length ) - 3;
if ( count > 0 )
{
check = check_sum( buffer, count, 3 );
if (check != program_word( buffer + count ))
{
fprintf( stderr, "Bad data check sum: %x/%x\n",
program_word( buffer + count ), check );
count = 0;
}
}
else
{
count = 0;
}
return count;
}
static int
program_flash(Environ *e, Self *s, int offset, uByte *data, int len)
{
Flash *flash = s->type;
int i;
uByte *buf = 0;
int buflen = 0;
uByte *bp;
int bpoff;
int bpsize;
Bool diag = diagnostic_mode(e);
int width = s->width;
int partswide = s->partswide;
const Sector *sectors = flash->sectors;
int mask = width - 1;
if (offset < 0)
return 0;
DPRINTF(("program_flash: e %p, s %p, offset %#x, data %p, len %d\n",
e, s, offset, data, len));
while (len)
{
int sector;
int soffset;
int ssize;
int off;
int sz;
int diffs;
for (i = 1; sectors[i].size; i++)
if (sectors[i].start * partswide > offset)
break;
sector = i - 1;
soffset = sectors[sector].start * partswide;
ssize = sectors[sector].size * partswide;
DPRINTF(("program_flash: sector %d, offset %#x, size %d\n",
sector, soffset, ssize));
/* check for write past end of flash */
if (soffset + ssize <= offset)
{
if (buf)
free(buf);
return 0;
}
off = offset - soffset;
sz = len < (ssize - off) ? len : (ssize - off);
DPRINTF(("program_flash: off %#x, sz %d\n", off, sz));
if (sz < 0 || sz > ssize)
{
cprintf(e, "sz out of range, %d\n", sz);
if (buf)
free(buf);
return 0;
}
/* check to see if we need to erase or program*/
diffs = compare_flash(e, s, offset, data, sz, 0);
DPRINTF(("program_flash: compare, %#x, %d, %d\n", offset, sz, diffs));
if (diffs & 2)
{
diffs = 1;
if (off || sz < ssize)
{
/* preserve the sector contents */
if (buflen < ssize)
{
if (buf)
free(buf);
buf = (uByte *)malloc(ssize);
buflen = ssize;
if (!buf)
{
cprintf(e, "Unable to allocate sector buffer\n");
return 0;
}
DPRINTF(("buf = %p\n", buf));
}
for (i = 0; i < ssize; i += width)
read_word(e, s, soffset + i, &buf[i]);
for (i = 0; i < sz; i++)
buf[off + i] = data[i];
bp = buf;
}
else
bp = data;
bpoff = soffset;
bpsize = ssize;
DPRINTF(("program_flash: bp %p, bpoff %#x, bpsize %d\n",
bp, bpoff, bpsize));
/* erase sector */
if (diag)
cprintf(e, "Erasing sector %d\n", sector);
else
spin_cursor(e);
DPRINTF(("program_flash: erasing sector %#x\n", soffset));
sector_erase(e, s, soffset);
}
else
{
bp = data;
bpoff = offset;
bpsize = sz;
DPRINTF(("program_flash: 2 bp %p, bpoff %#x, bpsize %d\n",
bp, bpoff, bpsize));
}
if (diffs & 1)
{
if (diag)
cprintf(e, "Programming sector %d\n", sector);
else
spin_cursor(e);
DPRINTF(("program_flash: programming offset %#x, size %d\n", bpoff, bpsize));
/* program */
if (bpoff & mask)
{
/* handle odd bytes */
int n = width - (bpoff & mask);
if (n > bpsize)
n = bpsize;
DPRINTF(("program_flash: %d misaligned bytes at %#x\n", n, bpoff, bp));
read_word(e, s, bpoff & ~mask, s->buf);
for (i = 0; i < n; i++)
s->buf[(bpoff & mask) + i] = bp[i];
program_word(e, s, bpoff & ~mask, s->buf);
}
else
i = 0;
for (; i + width - 1 < bpsize; i += width)
{
if ((((i / width) + 1) % (4*1024)) == 0)
spin_cursor(e);
program_word(e, s, (bpoff + i) & ~mask, &bp[i]);
}
DPRINTF(("program_flash: %d aligned bytes remainder at %#x, %p\n", i, bpoff, bp));
if (i < bpsize)
{
/* handle odd bytes at the tail */
int j;
DPRINTF(("program_flash: %d misaligned bytes remainder at %#x, %p\n", bpsize - i, bpoff + i, bp + i));
read_word(e, s, (bpoff + i) & ~mask, s->buf);
DPRINTF(("program_flash: buf[0] %#x, buf[1] %#x\n", s->buf[0], s->buf[1]));
for (j = 0; i < bpsize; j++, i++)
s->buf[j] = bp[i];
DPRINTF(("program_flash: new buf[0] %#x, buf[1] %#x\n", s->buf[0], s->buf[1]));
program_word(e, s, (bpoff + i) & ~mask, s->buf);
}
/* verify bytes */
diffs = compare_flash(e, s, bpoff, bp, bpsize, 1);
DPRINTF(("program_flash: compare: bpoff %p, bp %#x, bpsize %d, diffs %d\n",
bpoff, bp, bpsize, diffs));
}
offset += sz;
data += sz;
len -= sz;
}
if (buf)
free(buf);
DPRINTF(("buf = %p free\n", buf));
DPRINTF(("program_flash: offset %#x, data %#x, len %d\n",
offset, data, len));
/* return OK */
return 1;
}
