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;
}
static void tftp_close_file(struct inode *inode)
{
struct pxe_pvt_inode *socket = PVT(inode);
if (!socket->tftp_goteof) {
tftp_error(inode, 0, "No error, file close");
}
core_udp_close(socket);
}
/**
* Send a file to a TFTP server
*
* @param:inode, the inode to store our state in
* @param:ip, the ip to contact to get the file
* @param:filename, the file we wanna push
*
* @out: open_file_t structure, stores in file->open_file
* @out: the lenght of this file, stores in file->file_len
*
*/
__export int tftp_put(struct url_info *url, int flags, struct inode *inode,
const char **redir, char *data, int data_length)
{
struct pxe_pvt_inode *socket = PVT(inode);
char *buf;
uint16_t buf_len;
static const char wrq_tail[] = "octet";
char wrq_packet_buf[512+4+6];
char reply_packet_buf[PKTBUF_SIZE];
int err;
int wrq_len;
const uint8_t *timeout_ptr;
jiffies_t timeout;
jiffies_t oldtime;
uint16_t opcode;
uint16_t src_port = url->port;
uint32_t src_ip;
uint16_t seq = 0;
size_t chunk = 0;
int len = data_length;
int return_code = -ntohs(TFTP_EUNDEF);
(void)redir; /* TFTP does not redirect */
(void)flags;
if (url->type != URL_OLD_TFTP) {
/*
* The TFTP URL specification allows the TFTP to end with a
* ;mode= which we just ignore.
*/
url_unescape(url->path, ';');
}
if (!src_port)
src_port = TFTP_PORT;
// socket->ops = &tftp_conn_ops;
if (core_udp_open(socket))
return return_code;
buf = wrq_packet_buf;
*(uint16_t *)buf = TFTP_WRQ; /* TFTP opcode */
buf += 2;
buf += strlcpy(buf, url->path, 512);
buf++; /* Point *past* the final NULL */
memcpy(buf, wrq_tail, sizeof wrq_tail);
buf += sizeof wrq_tail;
wrq_len = buf - wrq_packet_buf;
timeout_ptr = TimeoutTable; /* Reset timeout */
sendreq:
timeout = *timeout_ptr++;
if (!timeout)
return return_code; /* No file available... */
oldtime = jiffies();
core_udp_sendto(socket, wrq_packet_buf, wrq_len, url->ip, src_port);
/* If the WRITE call fails, we let the timeout take care of it... */
for (;;) {
buf_len = sizeof(reply_packet_buf);
err = core_udp_recv(socket, reply_packet_buf, &buf_len,
&src_ip, &src_port);
if (err) {
jiffies_t now = jiffies();
if (now - oldtime >= timeout)
goto sendreq;
} else {
/* Make sure the packet actually came from the server and
is long enough for a TFTP opcode */
dprintf("tftp_put: got packet buflen=%d from server %u.%u.%u.%u(%u.%u.%u.%u)\n",
buf_len,
((uint8_t *)&src_ip)[0],
((uint8_t *)&src_ip)[1],
((uint8_t *)&src_ip)[2],
((uint8_t *)&src_ip)[3],
((uint8_t *)&url->ip)[0],
((uint8_t *)&url->ip)[1],
((uint8_t *)&url->ip)[2],
((uint8_t *)&url->ip)[3]);
if ((src_ip == url->ip) && (buf_len >= 2))
break;
}
}
core_udp_disconnect(socket);
core_udp_connect(socket, src_ip, src_port);
/* filesize <- -1 == unknown */
inode->size = -1;
socket->tftp_blksize = TFTP_BLOCKSIZE;
/*
* Get the opcode type, and parse it
*/
opcode = *(uint16_t *)reply_packet_buf;
switch (opcode) {
case TFTP_ERROR:
dprintf("tftp_push: received a TFTP_ERROR\n");
struct tftp_error *te = (struct tftp_error *)(reply_packet_buf+1);
return_code = -ntohs(te->errcode);
inode->size = 0;
goto done; /* ERROR reply; don't try again */
case TFTP_ACK:
dprintf("tftp_push: received a TFTP_ACK\n");
/* We received a ACK packet, sending the associated data packet */
/* If data was completly sent, we can stop here */
if (len == 0) {
return_code = -ntohs(TFTP_OK);
goto done;
}
/* If the server sequence is not aligned with our, we have an issue
* Let's break the transmission for now but could be improved later */
uint16_t srv_seq = ntohs(*(uint16_t *)(reply_packet_buf+2));
if (srv_seq != seq) {
printf("tftp_push: server sequence (%"PRIu16") is not aligned with our sequence (%"PRIu16"\n", srv_seq, seq);
return_code = -ntohs(TFTP_EBADOP);
goto done;
}
/* Let's transmit the data block */
chunk = len >= 512 ? 512 : len;
buf = wrq_packet_buf;
*(uint16_t *)buf = TFTP_DATA; /* TFTP opcode */
*((uint16_t *)(buf+2)) = htons(++seq);
memcpy(buf+4, data, chunk);
wrq_len = chunk + 4;
data += chunk;
len -= chunk;
timeout_ptr = TimeoutTable; /* Reset timeout */
goto sendreq;
default:
dprintf("tftp_push: unknown opcode %d\n", ntohs(opcode));
return_code = -ntohs(TFTP_EOPTNEG);
goto err_reply;
}
err_reply:
/* Build the TFTP error packet */
dprintf("tftp_push: Failure\n");
tftp_error(inode, TFTP_EOPTNEG, "TFTP protocol error");
inode->size = 0;
done:
if (!inode->size)
core_udp_close(socket);
return return_code;
}
/**
* Open a TFTP connection to the server
*
* @param:inode, the inode to store our state in
* @param:ip, the ip to contact to get the file
* @param:filename, the file we wanna open
*
* @out: open_file_t structure, stores in file->open_file
* @out: the lenght of this file, stores in file->file_len
*
*/
void tftp_open(struct url_info *url, int flags, struct inode *inode,
const char **redir)
{
struct pxe_pvt_inode *socket = PVT(inode);
char *buf;
uint16_t buf_len;
char *p;
char *options;
char *data;
static const char rrq_tail[] = "octet\0""tsize\0""0\0""blksize\0""1408";
char rrq_packet_buf[2+2*FILENAME_MAX+sizeof rrq_tail];
char reply_packet_buf[PKTBUF_SIZE];
int err;
int buffersize;
int rrq_len;
const uint8_t *timeout_ptr;
jiffies_t timeout;
jiffies_t oldtime;
uint16_t opcode;
uint16_t blk_num;
uint64_t opdata;
uint16_t src_port;
uint32_t src_ip;
(void)redir; /* TFTP does not redirect */
(void)flags;
if (url->type != URL_OLD_TFTP) {
/*
* The TFTP URL specification allows the TFTP to end with a
* ;mode= which we just ignore.
*/
url_unescape(url->path, ';');
}
if (!url->port)
url->port = TFTP_PORT;
socket->ops = &tftp_conn_ops;
if (core_udp_open(socket))
return;
buf = rrq_packet_buf;
*(uint16_t *)buf = TFTP_RRQ; /* TFTP opcode */
buf += 2;
buf = stpcpy(buf, url->path);
buf++; /* Point *past* the final NULL */
memcpy(buf, rrq_tail, sizeof rrq_tail);
buf += sizeof rrq_tail;
rrq_len = buf - rrq_packet_buf;
timeout_ptr = TimeoutTable; /* Reset timeout */
sendreq:
timeout = *timeout_ptr++;
if (!timeout)
return; /* No file available... */
oldtime = jiffies();
core_udp_sendto(socket, rrq_packet_buf, rrq_len, url->ip, url->port);
/* If the WRITE call fails, we let the timeout take care of it... */
wait_pkt:
for (;;) {
buf_len = sizeof(reply_packet_buf);
err = core_udp_recv(socket, reply_packet_buf, &buf_len,
&src_ip, &src_port);
if (err) {
jiffies_t now = jiffies();
if (now - oldtime >= timeout)
goto sendreq;
} else {
/* Make sure the packet actually came from the server and
is long enough for a TFTP opcode */
dprintf("tftp_open: got packet buflen=%d from server %u.%u.%u.%u(%u.%u.%u.%u)\n",
buf_len,
((uint8_t *)&src_ip)[0],
((uint8_t *)&src_ip)[1],
((uint8_t *)&src_ip)[2],
((uint8_t *)&src_ip)[3],
((uint8_t *)&url->ip)[0],
((uint8_t *)&url->ip)[1],
((uint8_t *)&url->ip)[2],
((uint8_t *)&url->ip)[3]);
if ((src_ip == url->ip) && (buf_len >= 2))
break;
}
}
core_udp_disconnect(socket);
core_udp_connect(socket, src_ip, src_port);
/* filesize <- -1 == unknown */
inode->size = -1;
socket->tftp_blksize = TFTP_BLOCKSIZE;
buffersize = buf_len - 2; /* bytes after opcode */
/*
* Get the opcode type, and parse it
*/
opcode = *(uint16_t *)reply_packet_buf;
switch (opcode) {
case TFTP_ERROR:
inode->size = 0;
goto done; /* ERROR reply; don't try again */
case TFTP_DATA:
/*
* If the server doesn't support any options, we'll get a
* DATA reply instead of OACK. Stash the data in the file
* buffer and go with the default value for all options...
*
* We got a DATA packet, meaning no options are
* suported. Save the data away and consider the
* length undefined, *unless* this is the only
* data packet...
*/
buffersize -= 2;
if (buffersize < 0)
goto wait_pkt;
data = reply_packet_buf + 2;
blk_num = ntohs(*(uint16_t *)data);
data += 2;
if (blk_num != 1)
goto wait_pkt;
socket->tftp_lastpkt = blk_num;
if (buffersize > TFTP_BLOCKSIZE)
goto err_reply; /* Corrupt */
socket->tftp_pktbuf = malloc(TFTP_BLOCKSIZE + 4);
if (!socket->tftp_pktbuf)
goto err_reply; /* Internal error */
if (buffersize < TFTP_BLOCKSIZE) {
/*
* This is the final EOF packet, already...
* We know the filesize, but we also want to
* ack the packet and set the EOF flag.
*/
inode->size = buffersize;
socket->tftp_goteof = 1;
ack_packet(inode, blk_num);
}
socket->tftp_bytesleft = buffersize;
socket->tftp_dataptr = socket->tftp_pktbuf;
memcpy(socket->tftp_pktbuf, data, buffersize);
goto done;
case TFTP_OACK:
/*
* Now we need to parse the OACK packet to get the transfer
* and packet sizes.
*/
options = reply_packet_buf + 2;
p = options;
while (buffersize) {
const char *opt = p;
/*
* If we find an option which starts with a NUL byte,
* (a null option), we're either seeing garbage that some
* TFTP servers add to the end of the packet, or we have
* no clue how to parse the rest of the packet (what is
* an option name and what is a value?) In either case,
* discard the rest.
*/
if (!*opt)
goto done;
while (buffersize) {
if (!*p)
break; /* Found a final null */
*p++ |= 0x20;
buffersize--;
}
if (!buffersize)
break; /* Unterminated option */
/* Consume the terminal null */
p++;
buffersize--;
if (!buffersize)
break; /* No option data */
opdata = 0;
/* do convert a number-string to decimal number, just like atoi */
while (buffersize--) {
uint8_t d = *p++;
if (d == '\0')
break; /* found a final null */
d -= '0';
if (d > 9)
goto err_reply; /* Not a decimal digit */
opdata = opdata*10 + d;
}
if (!strcmp(opt, "tsize"))
inode->size = opdata;
else if (!strcmp(opt, "blksize"))
socket->tftp_blksize = opdata;
else
goto err_reply; /* Non-negotitated option returned,
no idea what it means ...*/
}
if (socket->tftp_blksize < 64 || socket->tftp_blksize > PKTBUF_SIZE)
goto err_reply;
/* Parsing successful, allocate buffer */
socket->tftp_pktbuf = malloc(socket->tftp_blksize + 4);
if (!socket->tftp_pktbuf)
goto err_reply;
else
goto done;
default:
printf("TFTP unknown opcode %d\n", ntohs(opcode));
goto err_reply;
}
err_reply:
/* Build the TFTP error packet */
tftp_error(inode, TFTP_EOPTNEG, "TFTP protocol error");
inode->size = 0;
done:
if (!inode->size)
core_udp_close(socket);
return;
}
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);
}
}
