int tftp_recv_netascii(struct udp_pcb * udp, struct sockaddr_in * sin,
int block, char * buf, int len)
{
char * line;
FILE * f;
int ret;
int n;
int i;
f = null_fopen(NULL);
DCC_LOG1(LOG_TRACE, "1. f=%p", f);
i = 0;
n = 1;
ret = 0;
while (i < len) {
for (line = &buf[i]; i < len; i++) {
/* CR or [CR, LF] */
if (buf[i] == '\r') {
buf[i++] = '\0';
if (buf[i] == '\n')
i++;
break;
}
/* LF or [LF, CR] */
if (buf[i] == '\n') {
buf[i++] = '\0';
if (buf[i] == '\r')
i++;
break;
}
}
if (i == len)
buf[i] = '\0';
if ((ret = exec(f, yard_ice_cmd_tab, line)) < 0) {
DCC_LOG1(LOG_ERROR, "shell_exec(): %d", ret);
break;
}
n++;
DCC_LOG1(LOG_TRACE, "line %d", n);
}
if (ret < 0) {
sprintf(buf, "ERROR %d on line %d: %s", -ret, n, target_strerror(ret));
DCC_LOG1(LOG_TRACE, "2. f=%p", f);
ret = tftp_error(udp, sin, TFTP_EACCESS, buf);
DCC_LOG1(LOG_TRACE, "3. f=%p", f);
} else {
ret = tftp_ack(udp, block, sin);
}
DCC_LOG1(LOG_TRACE, "f=%p", f);
fclose(f);
return ret;
}
void __attribute__((noreturn)) tftp_daemon_task(struct debugger * dbg)
{
uint8_t buf[MAX_TFTP_MSG];
struct tftphdr * hdr = (struct tftphdr *)buf;
char * msg = (char *)buf;
struct sockaddr_in sin;
struct udp_pcb * udp;
struct tftp_req req;
int state = TFTPD_IDLE;
unsigned int addr_start = 0;
unsigned int addr_end = 0;
int block = 0;
int opc;
int len;
int blksize = TFTP_SEGSIZE;
DCC_LOG1(LOG_TRACE, "thread: %d", __os_thread_self());
if ((udp = udp_alloc()) == NULL) {
DCC_LOG(LOG_WARNING, "udp_alloc() fail!");
abort();
}
if (udp_bind(udp, INADDR_ANY, htons(IPPORT_TFTP)) < 0) {
DCC_LOG(LOG_WARNING, "udp_bind() fail!");
abort();
}
for (;;) {
if ((len = udp_recv(udp, buf, MAX_TFTP_MSG, &sin)) < 0) {
if (len == -ECONNREFUSED) {
DCC_LOG(LOG_WARNING, "udp_rcv ICMP error: ECONNREFUSED");
}
if (len == -EFAULT) {
DCC_LOG(LOG_WARNING, "udp_rcv error: EFAULT");
}
if (len == -ENOTCONN) {
DCC_LOG(LOG_WARNING, "udp_rcv error: ENOTCONN");
}
continue;
}
opc = htons(hdr->th_opcode);
if ((opc != TFTP_RRQ) && (opc != TFTP_WRQ)) {
DCC_LOG1(LOG_WARNING, "invalid opc: %d", opc);
WARN("TFTP: invalid opc: %d", opc);
continue;
}
if (udp_connect(udp, sin.sin_addr.s_addr, sin.sin_port) < 0) {
DCC_LOG(LOG_WARNING, "udp_connect() error");
WARN("TFTP: UDP connect failed!");
continue;
}
DCC_LOG2(LOG_TRACE, "Connected to: %I.%d", sin.sin_addr.s_addr,
ntohs(sin.sin_port));
// INF("Connected to: %08x.%d", sin.sin_addr.s_addr,
// ntohs(sin.sin_port));
for (;;) {
DCC_LOG3(LOG_INFO, "%I.%d %d",
sin.sin_addr.s_addr, ntohs(sin.sin_port), len);
DCC_LOG2(LOG_INFO, "len=%d, opc=%s", len, tftp_opc[opc]);
switch (opc) {
case TFTP_RRQ:
DCC_LOG(LOG_TRACE, "read request: ...");
tftp_req_parse((char *)&(hdr->th_stuff), &req);
blksize = req.blksize;
INF("TFTP: RRQ '%s' '%s' blksize=%d", req.fname,
tftp_mode[req.mode], req.blksize);
if (tftp_decode_fname(dbg, req.fname) < 0) {
ERR("TFTP: bad address.");
tftp_error(udp, &sin, TFTP_ENOTFOUND, "BAD ADDR.");
break;
}
/* set the transfer info */
addr_start = dbg->transf.base;
addr_end = addr_start + dbg->transf.size;
block = 0;
DCC_LOG2(LOG_TRACE, "start=0x%08x end=0x%08x",
addr_start, addr_end);
DBG("TFTP: start=0x%08x end=0x%08x",
addr_start, addr_end);
if (req.mode == TFTP_NETASCII) {
state = TFTPD_SEND_NETASCII;
} else if (req.mode == TFTP_OCTET) {
state = TFTPD_SEND_OCTET;
} else {
tftp_error(udp, &sin, TFTP_EUNDEF, NULL);
break;
}
if (req.opt_len) {
tftp_oack(udp, &sin, req.opt, req.opt_len);
break;
}
if (req.mode == TFTP_NETASCII)
goto send_netascii;
if (req.mode == TFTP_OCTET)
goto send_octet;
break;
case TFTP_WRQ:
/* Write Request */
DCC_LOG(LOG_TRACE, "write request...");
tftp_req_parse((char *)&(hdr->th_stuff), &req);
blksize = req.blksize;
INF("WRQ '%s' '%s' blksize=%d", req.fname,
tftp_mode[req.mode], req.blksize);
if (tftp_decode_fname(dbg, req.fname) < 0) {
tftp_error(udp, &sin, TFTP_ENOTFOUND, "BAD ADDR.");
break;
}
/* set the transfer info */
addr_start = dbg->transf.base;
addr_end = addr_start + dbg->transf.size;
block = 0;
DCC_LOG2(LOG_TRACE, "start=0x%08x end=0x%08x",
addr_start, addr_end);
DBG("TFTP: start=0x%08x end=0x%08x",
addr_start, addr_end);
if ((req.mode == TFTP_NETASCII) || (req.mode == TFTP_OCTET)) {
state = (req.mode == TFTP_NETASCII) ?
TFTPD_RECV_NETASCII : TFTPD_RECV_OCTET;
if (req.opt_len)
tftp_oack(udp, &sin, req.opt, req.opt_len);
else
tftp_ack(udp, block, &sin);
break;
}
tftp_error(udp, &sin, TFTP_EUNDEF, NULL);
break;
case TFTP_ACK:
block = htons(hdr->th_block);
DCC_LOG1(LOG_TRACE, "ACK: %d.", block);
if (state == TFTPD_SEND_NETASCII) {
unsigned int addr;
int rem;
int n;
send_netascii:
addr = addr_start + (block * 256);
rem = addr_end - addr;
if (rem < 0) {
state = TFTPD_IDLE;
DCC_LOG1(LOG_TRACE, "eot: %d bytes sent.",
addr_end - addr_start);
break;
}
n = (rem < 256) ? rem : 256;
DCC_LOG2(LOG_TRACE, "send netascii: addr=0x%08x n=%d",
addr, n);
/* build the packet */
len = tftp_hex(addr, hdr->th_data, n);
goto send_data;
}
if (state == TFTPD_SEND_OCTET) {
unsigned int addr;
int rem;
int n;
send_octet:
addr = addr_start + (block * blksize);
rem = addr_end - addr;
if (rem < 0) {
state = TFTPD_IDLE;
DCC_LOG1(LOG_TRACE, "eot: %d bytes sent.",
addr_end - addr_start);
break;
}
n = (rem < blksize) ? rem : blksize;
DCC_LOG2(LOG_TRACE, "send octet: addr=0x%08x n=%d",
addr, n);
/* build the packet */
len = target_mem_read(addr, hdr->th_data, n);
if (len < 0) {
DCC_LOG(LOG_WARNING, "target memory read error.");
len = 0;
}
send_data:
hdr->th_opcode = htons(TFTP_DATA);
hdr->th_block = htons(block + 1);
DCC_LOG2(LOG_TRACE, "block %d: %d bytes.",
block + 1, len);
if (udp_sendto(udp, hdr,
sizeof(struct tftphdr) + len, &sin) < 0) {
DCC_LOG(LOG_WARNING, "udp_sendto() fail");
state = TFTPD_IDLE;
break;
}
break;
}
DCC_LOG(LOG_WARNING, "state invalid!");
break;
case TFTP_DATA:
/* skip the header */
len -= 4;
DCC_LOG2(LOG_TRACE, "block=%d len=%d",
htons(hdr->th_block), len);
DBG("TFTP: DATA block=%d len=%d", htons(hdr->th_block), len);
if (htons(hdr->th_block) != (block + 1)) {
/* retransmission, just ack */
DCC_LOG2(LOG_WARNING, "retransmission, block=%d len=%d",
block, len);
tftp_ack(udp, block, &sin);
break;
}
if (state == TFTPD_RECV_OCTET) {
unsigned int addr;
int n;
addr = addr_start + (block * blksize);
block++;
if (len != blksize) {
DCC_LOG(LOG_TRACE, "last packet...");
state = TFTPD_IDLE;
if (len == 0) {
tftp_ack(udp, block, &sin);
break;
}
} else {
DCC_LOG2(LOG_TRACE, "rcvd octet: addr=0x%08x n=%d",
addr, len);
/* ACK the packet before writing to
speed up the transfer, errors are postponed... */
tftp_ack(udp, block, &sin);
}
n = target_mem_write(addr, hdr->th_data, len);
if (n < len) {
if (n < 0) {
DCC_LOG(LOG_ERROR, "target_mem_write()!");
sprintf(msg, "TARGET WRITE FAIL: %08x", addr);
WARN("memory write failed at "
"addr=0x%08x, code %d", addr, n);
} else {
DCC_LOG2(LOG_WARNING, "short writ: "
"ret(%d) < len(%d)!", n, len);
sprintf(msg, "TARGET SHORT WRITE: %08x",
addr + n);
}
tftp_error(udp, &sin, TFTP_EUNDEF, msg);
state = TFTPD_RECV_ERROR;
} else {
if (n > len) {
DCC_LOG2(LOG_WARNING, "long write: "
"ret(%d) < len(%d)!", n, len);
}
if (state == TFTPD_IDLE) {
/* ack the last packet ... */
tftp_ack(udp, block, &sin);
}
}
break;
}
if (state == TFTPD_RECV_ERROR) {
// tftp_error(udp, &sin, TFTP_EUNDEF, "TARGET WRITE FAIL.");
state = TFTPD_IDLE;
break;
}
if (state == TFTPD_RECV_NETASCII) {
block++;
if (len != blksize) {
state = TFTPD_IDLE;
if (len == 0) {
tftp_ack(udp, block, &sin);
break;
}
}
DCC_LOG1(LOG_TRACE, "ASCII recv %d...", len);
tftp_recv_netascii(udp, &sin, block,
(char *)hdr->th_data, len);
break;
}
tftp_error(udp, &sin, TFTP_EUNDEF, NULL);
break;
case TFTP_ERROR:
DCC_LOG2(LOG_TRACE, "error: %d: %s.",
htons(hdr->th_code), hdr->th_data);
break;
}
if (state == TFTPD_IDLE) {
DCC_LOG(LOG_TRACE, "[IDLE]");
break;
}
DBG("TFTP: UDP receive ...");
if ((len = udp_recv_tmo(udp, buf, MAX_TFTP_MSG, &sin, 5000)) < 0) {
if (len == -ETIMEDOUT) {
DCC_LOG(LOG_WARNING, "udp_recv_tmo() timeout!");
WARN("TFTP: UDP receive timeout!");
} else {
if (len == -ECONNREFUSED) {
DCC_LOG(LOG_WARNING, "udp_recv_tmo() lost peer!");
WARN("TFTP: UDP peer lost!");
} else {
DCC_LOG(LOG_WARNING, "udp_recv_tmo() failed!");
WARN("TFTP: UDP receive failed!");
}
}
/* break the inner loop */
break;
}
opc = htons(hdr->th_opcode);
}
/* disconnect */
DCC_LOG(LOG_TRACE, "disconnecting.");
udp_connect(udp, INADDR_ANY, 0);
}
}
void
tftp_stream_close(int *err)
{
tftp_ack(err);
tftp_stream.open = false;
}
int
tftp_stream_read(char *buf,
int len,
int *err)
{
int total_bytes = 0;
int size, recv_len, data_len;
struct timeval timeout;
struct tftphdr *hdr = (struct tftphdr *)tftp_stream.data;
while (total_bytes < len) {
// Move any bytes which we've already read/buffered
if (tftp_stream.avail > 0) {
size = tftp_stream.avail;
if (size > (len - total_bytes)) size = len - total_bytes;
memcpy(buf, tftp_stream.bufp, size);
buf += size;
tftp_stream.bufp += size;
tftp_stream.avail -= size;
total_bytes += size;
} else {
if (tftp_ack(err) < 0) {
return -1;
}
if ((tftp_stream.actual_len >= 0) && (tftp_stream.actual_len < SEGSIZE)) {
// Out of data
break;
}
timeout.tv_sec = (tftp_stream.last_good_block == 0) ? 10*TFTP_TIMEOUT_PERIOD : TFTP_TIMEOUT_PERIOD;
timeout.tv_usec = 0;
recv_len = sizeof(tftp_stream.data);
if ((data_len = __udp_recvfrom(&tftp_stream.data[0], recv_len, &tftp_stream.from_addr,
&tftp_stream.local_addr, &timeout)) < 0) {
// No data, try again
diag_printf("TFTP timed out %d/%d\n", tftp_stream.total_timeouts+1, TFTP_TIMEOUT_MAX);
if ((++tftp_stream.total_timeouts > TFTP_TIMEOUT_MAX) ||
(tftp_stream.last_good_block == 0)) {
// Timeout - no data received
*err = TFTP_TIMEOUT;
return -1;
}
// Send out the ACK for the last block - maybe server will retry
if (tftp_ack(err) < 0) {
return -1;
}
} else {
tftp_stream.packets_received++;
if (ntohs(hdr->th_opcode) == DATA) {
if (ntohs(hdr->th_block) == (cyg_uint16)((tftp_stream.last_good_block+1) & 0xFFFF)) {
// Consume this data
data_len -= 4; /* Sizeof TFTP header */
tftp_stream.avail = tftp_stream.actual_len = data_len;
tftp_stream.bufp = hdr->th_data;
tftp_stream.last_good_block++;
}
} else {
if (ntohs(hdr->th_opcode) == ERROR) {
*err = ntohs(hdr->th_code);
return -1;
} else {
// What kind of packet is this?
*err = TFTP_PROTOCOL;
return -1;
}
}
}
}
}
return total_bytes;
}
static void upd_flash(tftp *tftp)
{
char str[17];
int tftp_block;
u32 waddr;
u32 sector, last_sector;
int len;
tftp_init(tftp);
tftp->filename = upd_mem;
waddr = 0;
last_sector= 0xFFFFFFFF;
while(1)
{
tftp_run(tftp);
if ( (tftp->state == 2) ||
(tftp->state == 3))
{
if (tftp->lastblock != tftp_block)
{
tftp_block = tftp->lastblock;
strcpy(str, "Load flash ");
b2ds(&str[11], tftp_block);
oled_pos(0, 0);
oled_puts(str);
sector = (tftp_block << 9) & 0xFFFF0000;
if (sector != last_sector)
{
flash_erase(sector);
last_sector = sector;
}
uart_puts(str); uart_putc('\r');
len = 0;
if (tftp->length > 4)
{
u8 *dat;
dat = (u8 *)&tftp->data[4];
len = tftp->length - 4;
if (len > 256)
{
flash_write(waddr, dat, 256);
len -= 256; /* Update remaining data length */
dat += 256; /* Move the source pointer */
waddr += 256; /* Update the dest address */
/* Wait for the write transfer ends */
while(flash_status() & 1)
;
}
flash_write(waddr, dat, len);
/* Wait for the write transfer ends */
while(flash_status() & 1)
;
/* Update write address for next packet */
waddr += len;
}
}
tftp_ack(tftp);
}
if (tftp->state == 3)
{
uart_crlf();
break;
}
if (tftp->state == 98)
{
uart_puts("TFTP timeout, restart\r\n");
oled_pos(0, 0);
oled_puts("TFTP: timeout!");
tftp->timestamp = 0x3000;
tftp->state = 0;
}
if (tftp->state == 99)
{
uart_puts("upd_firmware() TFTP error 99\r\n");
break;
}
}
}