static int _tftp_close(tftp_info_t *info)
{
ebuf_t *buf;
const char *emsg = "transfer cancelled"; /* some error message */
if (info->tftp_socket == -1) return 0;
if (info->tftp_filemode == FILE_MODE_READ) {
buf = udp_alloc();
if (buf) {
/* If we're on the EOF packet, just send an ack */
if (info->tftp_lastblock) {
ebuf_append_u16_be(buf,TFTP_OP_ACK);
ebuf_append_u16_be(buf,info->tftp_blknum);
}
else {
ebuf_append_u16_be(buf,TFTP_OP_ERROR);
ebuf_append_u16_be(buf,TFTP_ERR_DISKFULL);
ebuf_append_bytes(buf,emsg,strlen(emsg)+1);
}
udp_send(info->tftp_socket,buf,info->tftp_ipaddr);
}
}
else {
/* Just flush out the remaining write data, if any */
_tftp_writemore(info);
}
udp_close(info->tftp_socket);
info->tftp_socket = -1;
return 0;
}
extern int send_rescueack(unsigned short no, unsigned short lo)
{
ebuf_t *buf = NULL;
int acksocket;
char tftpnull;
int res, i;
/*
* Open a UDP socket to the TFTP server
*/
acksocket = udp_socket(UDP_PROTO_TFTP);
res = udp_bind(acksocket, 69);
if (res < 0) {
return res;
}
udp_connect(acksocket, ackport);
for (i = 0; i < 1; i++) {
buf = udp_alloc();
if (!buf)
return -1;
/*
* Send the data
*/
ebuf_append_u16_be(buf, no);
ebuf_append_u16_be(buf, lo);
ebuf_append_bytes(buf,&tftpnull, 0);
udp_send(acksocket ,buf, tftpipto);
}
if (buf)
udp_free(buf);
udp_close(acksocket);
return 0;
}
void
tftp_udpbuffer(struct tfconn *cn, int size)
{
PACKET pkt;
if (cn->tf_outbuf.udp_use)
{
pkt = (PACKET)cn->tf_outbuf.udp_use;
if (pkt->nb_plen >= (unsigned)size)
{
/* recycle existing buffer */
pkt->nb_prot = (char *)cn->tf_outbuf.data;
return;
}
else
{
udp_free(pkt); /* old packet was too small */
}
}
pkt = udp_alloc(size, 0);
if (!pkt) /* alloc failed, clear outbuf pointers */
{
cn->tf_outbuf.data = NULL;
cn->tf_outbuf.dlen = 0;
cn->tf_outbuf.udp_use = NULL;
}
else /* return buffer pointer */
{
cn->tf_outbuf.data = pkt->nb_prot;
cn->tf_outbuf.dlen = size;
cn->tf_outbuf.udp_use = (void *)pkt; /* so we can find struct later */
}
}
int
tftp_udpsend(struct tfconn *conn, void *outbuf, int outlen)
{
PACKET pkt;
int e; /* error holder */
pkt = (PACKET)conn->tf_outbuf.udp_use;
/* sanity check buffer pointer */
if ((char *)outbuf != pkt->nb_prot)
{
/* this is OK on retrys, but not initial sends */
if (((char *)outbuf <= pkt->nb_buff) || /* null outbuf == bad */
(conn->tf_tmo == 0)) /* this is not a retry */
{
dtrap();
return ENP_LOGIC;
}
}
pkt->nb_prot = (char*)outbuf;
pkt->nb_plen = outlen;
pkt->fhost = conn->tf_fhost;
pkt->net = NULL; /* force it to route */
#ifdef ZEROCOPY_API /* packet can be marked in use, not freed */
pkt->inuse = 2; /* clone it so it won't pk_free() on send */
#else /* need to copy data in case we have to retry */
{
PACKET pkt2; /* packet to send & free */
pkt2 = udp_alloc(outlen, 0);
if (!pkt2)
return ENP_NOBUFFER;
pkt2->nb_plen = pkt->nb_plen;
pkt2->fhost = pkt->fhost;
pkt2->net = NULL;
MEMCPY(pkt2->nb_prot, pkt->nb_prot, outlen);
pkt = pkt2; /* send duplicate pkt, keep original in tf_conn */
}
#endif
e = udp_send(conn->tf_fport, conn->tf_lport, pkt);
if (e < 0)
{
#ifdef NPDEBUG
dprintf("tftp_udpsend(): udp_send() error %d\n", e);
#endif
return e;
}
else
return 0;
}
int
udp_flood(void * pio)
{
int i; /* index */
int e; /* tmp error holder */
int send_err; /* total send error count */
int no_pkt; /* times packet get failed */
u_short lport; /* local port for send (random) */
PACKET p;
int addr_index; /* index into floodtargs[] */
ns_printf(pio, "sending UDP flood of %ld pkts to %u.%u.%u.%u\n",
pktcount, PUSH_IPADDR(activehost) );
lport = udp_socket();
e = send_err = no_pkt = 0;
addr_index = 0;
for (i = 0; i < pktcount; i++)
{
p = udp_alloc((unsigned)deflength, 0); /* get packet for sending */
if(!p)
{
/* ns_printf(pio, "pkt alloc failed on send #%d\n", i); */
no_pkt++;
continue;
}
p->nb_plen = deflength;
/* if floodtargs array has any entries rotate through them, else send all
* of the packets to activehost
*/
if(floodtargs[0] == 0)
p->fhost = activehost;
else
{
p->fhost = floodtargs[addr_index++]; /* get next host */
if(floodtargs[addr_index++] == 0) /* wrap if at end of list */
addr_index = 0;
}
e = udp_send(lport, fport, p);
if(e)
{
send_err++;
/* ns_printf(pio, "UDP send error %d on %d\n", e, i); */
continue;
}
}
ns_printf(pio, "sent %d pkts, %d errors, %d no-pkt, last return code: %d\n",
i - no_pkt, send_err, no_pkt, e);
return e;
}
static int psock_udp_alloc(FAR struct socket *psock)
{
/* Allocate the UDP connection structure */
FAR struct udp_conn_s *conn = udp_alloc(psock->s_domain);
if (!conn)
{
/* Failed to reserve a connection structure */
return -ENOMEM;
}
/* Set the reference count on the connection structure. This reference
* count will be incremented only if the socket is dup'ed
*/
DEBUGASSERT(conn->crefs == 0);
conn->crefs = 1;
/* Save the pre-allocated connection in the socket structure */
psock->s_conn = conn;
return OK;
}
static int _tftp_writemore(tftp_info_t *info)
{
ebuf_t *buf;
uint16_t cmd,block,error;
int retries;
/*
* If we've already written the last block, there is no more
*/
if (info->tftp_lastblock) return 1;
if (info->tftp_error) return CFE_ERR_TIMEOUT;
/*
* Otherwise, send a block
*/
for (retries = 0; retries < tftp_max_retries; retries++) {
buf = udp_alloc();
if (!buf) return -1;
/*
* Send the data
*/
ebuf_append_u16_be(buf,TFTP_OP_DATA);
ebuf_append_u16_be(buf,info->tftp_blknum);
ebuf_append_bytes(buf,info->tftp_data,info->tftp_blklen);
udp_send(info->tftp_socket,buf,info->tftp_ipaddr);
/*
* Wait for some response, retransmitting as necessary
*/
buf = udp_recv_with_timeout(info->tftp_socket,tftp_recv_timeout);
if (buf == NULL) continue;
/*
* Got a response, make sure it's right
*/
ebuf_get_u16_be(buf,cmd);
if (cmd == TFTP_OP_ERROR) {
ebuf_get_u16_be(buf,error);
xprintf("TFTP write error %d: %s\n",error,ebuf_ptr(buf));
info->tftp_error = 1;
info->tftp_lastblock = 1;
udp_free(buf);
return CFE_ERR_IOERR;
}
if (cmd != TFTP_OP_ACK) {
udp_free(buf);
continue;
}
ebuf_get_u16_be(buf,block);
if (block != (info->tftp_blknum)) {
udp_free(buf);
continue;
}
/*
* It's the correct response. Update the block #
*/
info->tftp_blknum++;
if (info->tftp_blklen != TFTP_BLOCKSIZE) info->tftp_lastblock = 1;
udp_free(buf);
break;
}
/*
* If we had some failure, mark the stream with an error
*/
if (retries == tftp_max_retries) {
info->tftp_error = 1;
return CFE_ERR_TIMEOUT;
}
return 0;
}
static int _tftp_readmore(tftp_info_t *info)
{
ebuf_t *buf;
uint16_t cmd,block;
int retries;
/*
* If we've already read the last block, there is no more
*/
if (info->tftp_lastblock) return 1;
if (info->tftp_error) return CFE_ERR_TIMEOUT;
/*
* Otherwise, ack the current block so another one will come
*/
for (retries = 0; retries < tftp_max_retries; retries++) {
buf = udp_alloc();
if (!buf) return -1;
/*
* Send the ack
*/
ebuf_append_u16_be(buf,TFTP_OP_ACK);
ebuf_append_u16_be(buf,info->tftp_blknum);
udp_send(info->tftp_socket,buf,info->tftp_ipaddr);
/*
* Wait for some response, retransmitting as necessary
*/
buf = udp_recv_with_timeout(info->tftp_socket,tftp_recv_timeout);
if (buf == NULL) continue;
/*
* Got a response, make sure it's right
*/
ebuf_get_u16_be(buf,cmd);
if (cmd != TFTP_OP_DATA) {
udp_free(buf);
continue;
}
ebuf_get_u16_be(buf,block);
if (block != (info->tftp_blknum+1)) {
udp_free(buf);
continue;
}
/*
* It's the correct response. Copy the user data
*/
info->tftp_blknum = block;
info->tftp_blklen = ebuf_length(buf);
ebuf_get_bytes(buf,info->tftp_data,ebuf_length(buf));
udp_free(buf);
break;
}
/*
* If the block is less than 512 bytes long, it's the EOF block.
*/
if (retries == tftp_max_retries) {
info->tftp_error = 1;
return CFE_ERR_TIMEOUT;
}
if (info->tftp_blklen < TFTP_BLOCKSIZE) {
info->tftp_lastblock = 1; /* EOF */
}
return 0;
}
static int _tftp_open(tftp_info_t *info,char *hostname,char *filename,int mode)
{
ebuf_t *buf = NULL;
const char *datamode = "octet";
uint16_t type,error,block;
int res;
int retries;
/*
* Look up the remote host's IP address
*/
res = dns_lookup(hostname,info->tftp_ipaddr);
if (res < 0) return res;
/*
* Open a UDP socket to the TFTP server
*/
info->tftp_socket = udp_socket(UDP_PROTO_TFTP);
info->tftp_lastblock = 0;
info->tftp_error = 0;
info->tftp_filemode = mode;
/*
* Try to send the RRQ packet to open the file
*/
for (retries = 0; retries < tftp_max_retries; retries++) {
buf = udp_alloc();
if (!buf) break;
if (info->tftp_filemode == FILE_MODE_READ) {
ebuf_append_u16_be(buf,TFTP_OP_RRQ); /* read file */
}
else {
ebuf_append_u16_be(buf,TFTP_OP_WRQ); /* write file */
}
ebuf_append_bytes(buf,filename,strlen(filename)+1);
ebuf_append_bytes(buf,datamode,strlen(datamode)+1);
udp_send(info->tftp_socket,buf,info->tftp_ipaddr);
buf = udp_recv_with_timeout(info->tftp_socket,tftp_rrq_timeout);
if (buf) break;
}
/*
* If we got no response, bail now.
*/
if (!buf) {
udp_close(info->tftp_socket);
info->tftp_socket = -1;
return CFE_ERR_TIMEOUT;
}
/*
* Otherwise, process the response.
*/
ebuf_get_u16_be(buf,type);
switch (type) {
case TFTP_OP_ACK:
/*
* Acks are what we get back on a WRQ command,
* but are otherwise unexpected.
*/
if (info->tftp_filemode == FILE_MODE_WRITE) {
udp_connect(info->tftp_socket,(uint16_t) buf->eb_usrdata);
info->tftp_blknum = 1;
info->tftp_blklen = 0;
udp_free(buf);
return 0;
break;
}
/* fall through */
case TFTP_OP_RRQ:
case TFTP_OP_WRQ:
default:
/*
* we aren't expecting any of these messages
*/
udp_free(buf);
udp_close(info->tftp_socket);
info->tftp_socket = -1;
return CFE_ERR_PROTOCOLERR;
case TFTP_OP_ERROR:
ebuf_get_u16_be(buf,error);
xprintf("TFTP error %d: %s\n",error,ebuf_ptr(buf));
udp_free(buf);
udp_close(info->tftp_socket);
info->tftp_socket = -1;
return CFE_ERR_PROTOCOLERR;
case TFTP_OP_DATA:
/*
* Yay, we've got data! Store the first block.
*/
ebuf_get_u16_be(buf,block);
udp_connect(info->tftp_socket,(uint16_t) buf->eb_usrdata);
info->tftp_blknum = block;
info->tftp_blklen = ebuf_length(buf);
ebuf_get_bytes(buf,info->tftp_data,ebuf_length(buf));
udp_free(buf);
if (info->tftp_blklen < TFTP_BLOCKSIZE) {
info->tftp_lastblock = 1; /* EOF */
}
return 0;
break;
}
}
int run_vpn(shadowvpn_args_t *args) {
fd_set readset;
int tun, sock, max_fd;
ssize_t r;
unsigned char *tun_buf;
unsigned char *udp_buf;
struct sockaddr_storage remote_addr;
struct sockaddr *remote_addrp = (struct sockaddr *)&remote_addr;
socklen_t remote_addrlen;
struct sockaddr *src_addrp = NULL;
socklen_t *src_addrlen = NULL;
if (args->mode == SHADOWVPN_MODE_SERVER) {
// if we are running a server, update server address from recv_from
src_addrp = remote_addrp;
src_addrlen = &remote_addrlen;
}
if (-1 == (tun = tun_alloc(args->intf))) {
errf("failed to create tun device");
return -1;
}
if (-1 == (sock = udp_alloc(args->mode == SHADOWVPN_MODE_SERVER,
args->server, args->port,
remote_addrp, &remote_addrlen))) {
errf("failed to create UDP socket");
close(tun);
return -1;
}
shell_up(args);
tun_buf = malloc(args->mtu + SHADOWVPN_ZERO_BYTES);
udp_buf = malloc(args->mtu + SHADOWVPN_ZERO_BYTES);
memset(tun_buf, 0, SHADOWVPN_ZERO_BYTES);
memset(udp_buf, 0, SHADOWVPN_ZERO_BYTES);
for(;;) {
FD_ZERO(&readset);
FD_SET(tun, &readset);
FD_SET(sock, &readset);
max_fd = max(tun, sock) + 1;
if (-1 == select(max_fd, &readset, NULL, NULL, NULL)) {
if (errno == EINTR)
continue;
err("select");
break;
}
if (FD_ISSET(tun, &readset)) {
r = read(tun, tun_buf + SHADOWVPN_ZERO_BYTES, args->mtu);
if (r == -1) {
if (errno == EAGAIN || errno == EWOULDBLOCK) {
// do nothing
} else if (errno == EPERM || errno == EINTR) {
// just log, do nothing
err("read from tun");
} else {
err("read from tun");
break;
}
}
if (remote_addrlen) {
crypto_encrypt(udp_buf, tun_buf, r);
r = sendto(sock, udp_buf + SHADOWVPN_PACKET_OFFSET,
SHADOWVPN_OVERHEAD_LEN + r, 0,
remote_addrp, remote_addrlen);
if (r == -1) {
if (errno == EAGAIN || errno == EWOULDBLOCK) {
// do nothing
} else if (errno == ENETUNREACH || errno == ENETDOWN ||
errno == EPERM || errno == EINTR || errno == EMSGSIZE) {
// just log, do nothing
err("sendto");
} else {
err("sendto");
// TODO rebuild socket
break;
}
}
}
}
if (FD_ISSET(sock, &readset)) {
r = recvfrom(sock, udp_buf + SHADOWVPN_PACKET_OFFSET,
SHADOWVPN_OVERHEAD_LEN + args->mtu, 0,
src_addrp, src_addrlen);
if (r == -1) {
if (errno == EAGAIN || errno == EWOULDBLOCK) {
// do nothing
} else if (errno == ENETUNREACH || errno == ENETDOWN ||
errno == EPERM || errno == EINTR) {
// just log, do nothing
err("recvfrom");
} else {
err("recvfrom");
// TODO rebuild socket
break;
}
}
if (-1 == crypto_decrypt(tun_buf, udp_buf, r - SHADOWVPN_OVERHEAD_LEN)) {
errf("invalid packet, drop");
} else {
if (-1 == write(tun, tun_buf + SHADOWVPN_ZERO_BYTES,
r - SHADOWVPN_OVERHEAD_LEN)) {
if (errno == EAGAIN || errno == EWOULDBLOCK) {
// do nothing
} else if (errno == EPERM || errno == EINTR || errno == EINVAL) {
// just log, do nothing
err("write to tun");
} else {
err("write to tun");
break;
}
}
}
}
}
free(tun_buf);
free(udp_buf);
shell_down(args);
close(tun);
close(sock);
return -1;
}
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);
}
}
static int _tftpd_open(tftp_info_t *info,char *hostname,char *filename,int mode)
{
ebuf_t *buf = NULL;
uint16_t type;
int res;
int retries;
char ch = 0;
#ifdef RESCUE_MODE
uint8_t asuslink[13] = "ASUSSPACELINK";
uint8_t maclink[13]="snxxxxxxxxxxx";
unsigned char tftpmask[4] = { 0xff, 0xff, 0xff, 0x00 };
int i;
char tftpnull;
uint8_t ipaddr[4] = { 0xc0, 0xa8, 0x01, 0x0c };
uint8_t hwaddr[6] = { 0x00, 0xe0, 0x18, 0x00, 0x3e, 0xc4 };
#endif
/*
* Open a UDP socket
*/
info->tftp_socket = udp_socket(UDP_PROTO_TFTP);
info->tftp_lastblock = 0;
info->tftp_error = 0;
info->tftp_filemode = mode;
/*
* Listen for requests
*/
res = udp_bind(info->tftp_socket,UDP_PROTO_TFTP);
if (res < 0) return res;
res = CFE_ERR_TIMEOUT;
for (retries = 0; retries < tftp_max_retries; retries++) {
while (console_status()) {
console_read(&ch,1);
if (ch == 3) break;
}
if (ch == 3) {
res = CFE_ERR_INTR;
break;
}
buf = udp_recv_with_timeout(info->tftp_socket,tftp_recv_timeout);
if (buf == NULL) continue;
/*
* Process the request
*/
ebuf_get_u16_be(buf,type);
switch (type) {
case TFTP_OP_RRQ:
#ifdef RESCUE_MODE
udp_connect(info->tftp_socket,(uint16_t) buf->eb_usrdata);
ackport = buf->eb_usrdata;
memcpy(info->tftp_ipaddr,buf->eb_usrptr,IP_ADDR_LEN);
info->tftp_blknum = 1;
info->tftp_blklen = 0;
for (i=0; i<13; i++) {
if (buf->eb_ptr[i] != asuslink[i])
break;
}
if (i==13) {
tftpipfrom[0]=buf->eb_ptr[16];
tftpipfrom[1]=buf->eb_ptr[15];
tftpipfrom[2]=buf->eb_ptr[14];
tftpipfrom[3]=buf->eb_ptr[13];
tftpipto[0]=buf->eb_usrptr[0];
tftpipto[1]=buf->eb_usrptr[1];
tftpipto[2]=buf->eb_usrptr[2];
tftpipto[3]=buf->eb_usrptr[3];
net_setparam(NET_IPADDR,tftpipfrom);
net_setparam(NET_NETMASK,tftpmask);
net_setparam(NET_GATEWAY,tftpipfrom);
ui_myshowifconfig();
net_setnetvars();
for (i=0; i<4; i++)
ipaddr[i]=tftpipto[i];
for (i=0; i<6; i++)
hwaddr[i]=buf->eb_data[6+i];
buf = udp_alloc();
if (!buf) {
res = CFE_ERR_TIMEOUT;
break;
}
ebuf_append_u16_be(buf, 3);
ebuf_append_u16_be(buf, 1);
ebuf_append_bytes(buf,&tftpnull, 0);
arp_delete(ipaddr);
arp_add(ipaddr,hwaddr);
udp_send(info->tftp_socket, buf, tftpipto);
}
else {
for (i=0; i<13; i++) {
if (buf->eb_ptr[i] != maclink[i])
break;
}
if (i==13) {
tftpipfrom[0]=buf->eb_ptr[16];
tftpipfrom[1]=buf->eb_ptr[15];
tftpipfrom[2]=buf->eb_ptr[14];
tftpipfrom[3]=buf->eb_ptr[13];
tftpipto[0]=buf->eb_usrptr[0];
tftpipto[1]=buf->eb_usrptr[1];
tftpipto[2]=buf->eb_usrptr[2];
tftpipto[3]=buf->eb_usrptr[3];
net_setparam(NET_IPADDR,tftpipfrom);
net_setparam(NET_NETMASK,tftpmask);
net_setparam(NET_GATEWAY,tftpipfrom);
ui_myshowifconfig();
net_setnetvars();
for (i=0; i<4; i++)
ipaddr[i]=tftpipto[i];
for (i=0; i<6; i++)
hwaddr[i]=buf->eb_data[6+i];
buf = udp_alloc();
if (!buf) {
res = CFE_ERR_TIMEOUT;
break;
}
ebuf_append_u16_be(buf, 3);
ebuf_append_u16_be(buf, 1);
ebuf_append_bytes(buf,&tftpnull, 0);
arp_delete(ipaddr);
arp_add(ipaddr,hwaddr);
udp_send(info->tftp_socket, buf, tftpipto);
}
}
res = CFE_ERR_TIMEOUT;
#else
udp_connect(info->tftp_socket,(uint16_t) buf->eb_usrdata);
memcpy(info->tftp_ipaddr,buf->eb_usrptr,IP_ADDR_LEN);
info->tftp_blknum = 1;
info->tftp_blklen = 0;
res = 0;
#endif
break;
case TFTP_OP_WRQ:
udp_connect(info->tftp_socket,(uint16_t) buf->eb_usrdata);
memcpy(info->tftp_ipaddr,buf->eb_usrptr,IP_ADDR_LEN);
info->tftp_blknum = 0;
res = _tftp_readmore(info);
break;
default:
/*
* we aren't expecting any of these messages
*/
res = CFE_ERR_PROTOCOLERR;
break;
}
udp_free(buf);
break;
}
if (res) {
udp_close(info->tftp_socket);
info->tftp_socket = -1;
}
return res;
}
int psock_socket(int domain, int type, int protocol, FAR struct socket *psock)
{
int err;
/* Only PF_INET, PF_INET6 or PF_PACKET domains supported */
if (
#if defined(CONFIG_NET_IPv6)
domain != PF_INET6
#else
domain != PF_INET
#endif
#if defined(CONFIG_NET_PKT)
&& domain != PF_PACKET
#endif
)
{
err = EAFNOSUPPORT;
goto errout;
}
/* Only SOCK_STREAM, SOCK_DGRAM and possible SOCK_RAW are supported */
if (
#if defined(CONFIG_NET_TCP)
(type == SOCK_STREAM && protocol != 0 && protocol != IPPROTO_TCP) ||
#endif
#if defined(CONFIG_NET_UDP)
(type == SOCK_DGRAM && protocol != 0 && protocol != IPPROTO_UDP) ||
#endif
(
#if defined(CONFIG_NET_TCP)
#if defined(CONFIG_NET_UDP) || defined(CONFIG_NET_PKT)
type != SOCK_STREAM &&
#else
type != SOCK_STREAM
#endif
#endif
#if defined(CONFIG_NET_UDP)
#if defined(CONFIG_NET_PKT)
type != SOCK_DGRAM &&
#else
type != SOCK_DGRAM
#endif
#endif
#if defined(CONFIG_NET_PKT)
type != SOCK_RAW
#endif
)
)
{
err = EPROTONOSUPPORT;
goto errout;
}
/* Everything looks good. Initialize the socket structure */
/* Save the protocol type */
psock->s_type = type;
psock->s_conn = NULL;
#ifdef CONFIG_NET_TCP_WRITE_BUFFERS
psock->s_sndcb = NULL;
#endif
/* Allocate the appropriate connection structure. This reserves the
* the connection structure is is unallocated at this point. It will
* not actually be initialized until the socket is connected.
*/
err = ENOMEM; /* Assume failure to allocate connection instance */
switch (type)
{
#ifdef CONFIG_NET_PKT
case SOCK_RAW:
{
/* Allocate the packet socket connection structure and save
* in the new socket instance.
*/
FAR struct pkt_conn_s *conn = pkt_alloc();
if (!conn)
{
/* Failed to reserve a connection structure */
goto errout;
}
/* Set the reference count on the connection structure. This
* reference count will be increment only if the socket is
* dup'ed
*/
DEBUGASSERT(conn->crefs == 0);
psock->s_conn = conn;
conn->crefs = 1;
}
break;
#endif
#ifdef CONFIG_NET_TCP
case SOCK_STREAM:
{
/* Allocate the TCP connection structure and save in the new
* socket instance.
*/
FAR struct tcp_conn_s *conn = tcp_alloc();
if (!conn)
{
/* Failed to reserve a connection structure */
goto errout; /* With err == ENFILE or ENOMEM */
}
/* Set the reference count on the connection structure. This
* reference count will be increment only if the socket is
* dup'ed
*/
DEBUGASSERT(conn->crefs == 0);
psock->s_conn = conn;
conn->crefs = 1;
}
break;
#endif
#ifdef CONFIG_NET_UDP
case SOCK_DGRAM:
{
/* Allocate the UDP connection structure and save in the new
* socket instance.
*/
FAR struct udp_conn_s *conn = udp_alloc();
if (!conn)
{
/* Failed to reserve a connection structure */
goto errout; /* With err == ENFILE or ENOMEM */
}
/* Set the reference count on the connection structure. This
* reference count will be increment only if the socket is
* dup'ed
*/
DEBUGASSERT(conn->crefs == 0);
psock->s_conn = conn;
conn->crefs = 1;
}
break;
#endif
default:
break;
}
return OK;
errout:
set_errno(err);
return ERROR;
}
void __attribute__((noreturn)) supervisor_task(void)
{
struct trace_entry trace;
uint32_t clk;
uint32_t dump_tmo;
int n;
INF("<%d> started...", thinkos_thread_self());
/* set the supervisor stream to stdout */
trace_file = stdout;
/* enable auto flush */
trace_auto_flush = true;
trace_tail(&trace);
#if 0
struct sockaddr_in sin;
struct udp_pcb * udp;
bool udp_enabled;
if ((udp = udp_alloc()) == NULL) {
abort();
}
if (!inet_aton(trace_host, &sin.sin_addr)) {
abort();
}
sin.sin_port = htons(trace_port);
if (udp_bind(udp, INADDR_ANY, htons(10)) < 0) {
abort();
}
// if (udp_connect(udp, sin.sin_addr.s_addr, htons(sin.sin_port)) < 0) {
// abort();
// }
#endif
clk = thinkos_clock();
dump_tmo = clk + 15000;
for (;;) {
char msg[80];
/* 8Hz periodic task */
clk += 125;
thinkos_alarm(clk);
// udp_enabled = trace_udp_enabled;
while (trace_getnext(&trace, msg, sizeof(msg)) >= 0) {
struct timeval tv;
char s[128];
trace_ts2timeval(&tv, trace.dt);
if (trace.ref->lvl <= TRACE_LVL_WARN)
n = sprintf(s, "%s %2d.%06d: %s,%d: %s\n",
trace_lvl_nm[trace.ref->lvl],
(int)tv.tv_sec, (int)tv.tv_usec,
trace.ref->func, trace.ref->line, msg);
else
n = sprintf(s, "%s %2d.%06d: %s\n",
trace_lvl_nm[trace.ref->lvl],
(int)tv.tv_sec, (int)tv.tv_usec, msg);
/* write log to local console */
fwrite(s, n, 1, trace_file);
#if 0
if (udp_enabled) {
/* sent log to remote station */
if (udp_sendto(udp, s, n, &sin) < 0) {
udp_enabled = false;
}
}
#endif
}
if (trace_auto_flush)
trace_flush(&trace);
if ((int32_t)(clk - dump_tmo) >= 0) {
const struct thinkos_thread_inf * infbuf[33];
uint32_t cycbuf[33];
uint32_t cycsum = 0;
uint32_t cycbusy;
uint32_t cycidle;
uint32_t cycdiv;
uint32_t idle;
uint32_t busy;
int n;
int i;
thinkos_critical_enter();
thinkos_thread_inf(infbuf);
n = thinkos_cyccnt(cycbuf);
thinkos_critical_exit();
cycsum = 0;
for (i = 0; i < n; ++i)
cycsum += cycbuf[i];
cycidle = cycbuf[n - 1]; /* The last item is IDLE */
cycbusy = cycsum - cycidle;
cycdiv = (cycsum + 5000) / 10000;
busy = (cycbusy + (cycdiv / 2)) / cycdiv;
idle = 1000 - busy;
printf("CPU usage: %d.%02d%% busy, %d.%02d%% idle\n",
busy / 100, busy % 100, idle / 100, idle % 100);
for (i = 0; i < n; ++i) {
const struct thinkos_thread_inf * inf;
if (((inf = infbuf[i]) != NULL) && (cycbuf[i] != 0)) {
uint32_t usage;
usage = (cycbuf[i] + cycdiv / 2) / cycdiv;
printf("%2d %7s %3d.%02d%%\n", i, inf->tag,
usage / 100, usage % 100);
}
}
dump_tmo = clk + 15000;
}
}
}