void NAT_ALG_TCP_checksum
(
IP_HEADER_PTR ip_header_ptr /* [IN] pointer to IP header */
)
{ /* Body */
TRANSPORT_UNION transport;
uint16_t checksum;
uint16_t protocol;
uint16_t iplen = IPH_LEN(ip_header_ptr);
uint16_t len = mqx_ntohs(ip_header_ptr->LENGTH) - iplen;
/* Get TCP header */
transport.PTR = TRANSPORT_PTR(ip_header_ptr);
mqx_htons(transport.TCP_PTR->checksum, 0); /* Clear checksum field */
protocol = mqx_ntohc(ip_header_ptr->PROTOCOL); /* PROTOCOL */
checksum = (uint16_t) _mem_sum_ip(protocol, 8, ip_header_ptr->SOURCE); /* IP SRC and DST ADDR */
checksum = (uint16_t) _mem_sum_ip(checksum, len, transport.PTR);
/* TCP LENGTH */
checksum = IP_Sum_immediate(checksum, len);
checksum = IP_Sum_invert(checksum);
mqx_htons(transport.TCP_PTR->checksum, checksum);
} /* Endbody */
/*
* Validate a TCP segment's checksum,return TRUE if pass the validation,
* FALSE will be returned otherwise.
*/
static BOOL validateTCPChksum(struct ip_hdr* p, struct pbuf* pb)
{
struct tcp_hdr* pTcpHdr = NULL;
int iph_len = IPH_HL(p);
int ip_len = ntohs(IPH_LEN(p)); /* Total IP packet length. */
int chksum_old = 0, chksum_new = 0;
ip_addr_t src, dest;
iph_len *= 4;
/* Locate TCP header. */
pTcpHdr = (struct tcp_hdr*)((char*)p + iph_len);
/* Validate pbuf object. */
if (pb->tot_len < (iph_len + sizeof(struct tcp_hdr)))
{
return FALSE;
}
/* Validate the length of this packet. */
if (ip_len < (iph_len + sizeof(struct tcp_hdr)))
{
return FALSE;
}
/*
* Preserve TCP segment's check sum value before re-calculate
* the value.
*/
chksum_old = pTcpHdr->chksum;
ip_addr_copy(src, p->src);
ip_addr_copy(dest, p->dest);
pbuf_header(pb, -iph_len); /* move to TCP header. */
pTcpHdr->chksum = 0; /* Reset the original check sum. */
/* Recalculate the segment's checksum value. */
chksum_new = inet_chksum_pseudo(pb, &src, &dest, IP_PROTO_TCP,
/* IP payload length,e.i,TCP header plus data. */
ip_len - iph_len);
//pb->tot_len);
pbuf_header(pb, iph_len); /* move back. */
if (chksum_new != chksum_old)
{
__LOG("TCP checksum fail:ip_len = %d,pbuf_tot_len = %d,src_addr:%s\r\n",
ip_len,
pb->tot_len,
inet_ntoa(p->src.addr));
IP_STATS_INC(tcp.chkerr);
IP_STATS_INC(tcp.drop);
return FALSE;
}
return TRUE;
}
void
ip_debug_print(struct pbuf *p)
{
struct ip_hdr *iphdr = p->payload;
u8_t *payload;
payload = (u8_t *)iphdr + IP_HLEN;
LWIP_DEBUGF(IP_DEBUG, ("IP header:\n"));
LWIP_DEBUGF(IP_DEBUG, ("+-------------------------------+\n"));
LWIP_DEBUGF(IP_DEBUG, ("|%2"S16_F" |%2"S16_F" | 0x%02"X16_F" | %5"U16_F" | (v, hl, tos, len)\n",
IPH_V(iphdr),
IPH_HL(iphdr),
IPH_TOS(iphdr),
ntohs(IPH_LEN(iphdr))));
LWIP_DEBUGF(IP_DEBUG, ("+-------------------------------+\n"));
LWIP_DEBUGF(IP_DEBUG, ("| %5"U16_F" |%"U16_F"%"U16_F"%"U16_F"| %4"U16_F" | (id, flags, offset)\n",
ntohs(IPH_ID(iphdr)),
ntohs(IPH_OFFSET(iphdr)) >> 15 & 1,
ntohs(IPH_OFFSET(iphdr)) >> 14 & 1,
ntohs(IPH_OFFSET(iphdr)) >> 13 & 1,
ntohs(IPH_OFFSET(iphdr)) & IP_OFFMASK));
LWIP_DEBUGF(IP_DEBUG, ("+-------------------------------+\n"));
LWIP_DEBUGF(IP_DEBUG, ("| %3"U16_F" | %3"U16_F" | 0x%04"X16_F" | (ttl, proto, chksum)\n",
IPH_TTL(iphdr),
IPH_PROTO(iphdr),
ntohs(IPH_CHKSUM(iphdr))));
LWIP_DEBUGF(IP_DEBUG, ("+-------------------------------+\n"));
LWIP_DEBUGF(IP_DEBUG, ("| %3"U16_F" | %3"U16_F" | %3"U16_F" | %3"U16_F" | (src)\n",
ip4_addr1(&iphdr->src),
ip4_addr2(&iphdr->src),
ip4_addr3(&iphdr->src),
ip4_addr4(&iphdr->src)));
LWIP_DEBUGF(IP_DEBUG, ("+-------------------------------+\n"));
LWIP_DEBUGF(IP_DEBUG, ("| %3"U16_F" | %3"U16_F" | %3"U16_F" | %3"U16_F" | (dest)\n",
ip4_addr1(&iphdr->dest),
ip4_addr2(&iphdr->dest),
ip4_addr3(&iphdr->dest),
ip4_addr4(&iphdr->dest)));
LWIP_DEBUGF(IP_DEBUG, ("+-------------------------------+\n"));
}
/* Update TCP segment's check sum. */
static void _tcp_check_sum(struct ip_hdr* p, struct pbuf* pb)
{
struct tcp_hdr* pTcpHdr = NULL;
int iph_len = IPH_HL(p);
int ip_len = ntohs(IPH_LEN(p)); /* Total IP packet's length. */
int chksum = 0;
ip_addr_t src, dest;
iph_len *= 4;
/* Locate TCP header. */
pTcpHdr = (struct tcp_hdr*)((char*)p + iph_len);
/* Validate pbuf object. */
if (pb->tot_len < (iph_len + sizeof(struct tcp_hdr)))
{
return;
}
/* Validate IP packet's total length. */
if (ip_len < (iph_len + sizeof(struct tcp_hdr)))
{
return;
}
/*
* Reset TCP header's check sum since it's source address
* or source port is changed after NATing.
*/
chksum = pTcpHdr->chksum;
ip_addr_copy(src, p->src);
ip_addr_copy(dest, p->dest);
pbuf_header(pb, -iph_len); /* move to TCP header. */
pTcpHdr->chksum = 0; /* Reset the original check sum. */
pTcpHdr->chksum = inet_chksum_pseudo(pb, &src, &dest, IP_PROTO_TCP,
ip_len - iph_len);
pbuf_header(pb, iph_len); /* move back. */
__NATDEBUG("%s: TCP check sum updated[%X] -> [%X]\r\n",
__func__,
chksum,
pTcpHdr->chksum);
}
void
ip_debug_print(struct pbuf *p)
{
struct ip_hdr *iphdr = p->payload;
u8_t *payload;
payload = (u8_t *)iphdr + IP_HLEN/sizeof(u8_t);
DEBUGF(IP_DEBUG, ("IP header:\n"));
DEBUGF(IP_DEBUG, ("+-------------------------------+\n"));
DEBUGF(IP_DEBUG, ("|%2d |%2d | %2d | %4d | (v, hl, tos, len)\n",
IPH_V(iphdr),
IPH_HL(iphdr),
IPH_TOS(iphdr),
ntohs(IPH_LEN(iphdr))));
DEBUGF(IP_DEBUG, ("+-------------------------------+\n"));
DEBUGF(IP_DEBUG, ("| %5d |%d%d%d| %4d | (id, flags, offset)\n",
ntohs(IPH_ID(iphdr)),
ntohs(IPH_OFFSET(iphdr)) >> 15 & 1,
ntohs(IPH_OFFSET(iphdr)) >> 14 & 1,
ntohs(IPH_OFFSET(iphdr)) >> 13 & 1,
ntohs(IPH_OFFSET(iphdr)) & IP_OFFMASK));
DEBUGF(IP_DEBUG, ("+-------------------------------+\n"));
DEBUGF(IP_DEBUG, ("| %2d | %2d | 0x%04x | (ttl, proto, chksum)\n",
IPH_TTL(iphdr),
IPH_PROTO(iphdr),
ntohs(IPH_CHKSUM(iphdr))));
DEBUGF(IP_DEBUG, ("+-------------------------------+\n"));
DEBUGF(IP_DEBUG, ("| %3ld | %3ld | %3ld | %3ld | (src)\n",
ntohl(iphdr->src.addr) >> 24 & 0xff,
ntohl(iphdr->src.addr) >> 16 & 0xff,
ntohl(iphdr->src.addr) >> 8 & 0xff,
ntohl(iphdr->src.addr) & 0xff));
DEBUGF(IP_DEBUG, ("+-------------------------------+\n"));
DEBUGF(IP_DEBUG, ("| %3ld | %3ld | %3ld | %3ld | (dest)\n",
ntohl(iphdr->dest.addr) >> 24 & 0xff,
ntohl(iphdr->dest.addr) >> 16 & 0xff,
ntohl(iphdr->dest.addr) >> 8 & 0xff,
ntohl(iphdr->dest.addr) & 0xff));
DEBUGF(IP_DEBUG, ("+-------------------------------+\n"));
}
static void show_ip_pkt(struct pbuf *p)
{
struct ip_hdr *iphdr = (struct ip_hdr *)p->payload;
u8_t *payload;
payload = (u8_t *)iphdr + IP_HLEN;
_hx_printf("[%s]IP header:\r\n",__func__);
_hx_printf("+-------------------------------+\r\n");
_hx_printf("|%2"S16_F" |%2"S16_F" | 0x%02"X16_F" | %5"U16_F" | (v, hl, tos, len)\r\n",
IPH_V(iphdr),
IPH_HL(iphdr),
IPH_TOS(iphdr),
ntohs(IPH_LEN(iphdr)));
_hx_printf("+-------------------------------+\r\n");
_hx_printf("| %5"U16_F" |%"U16_F"%"U16_F"%"U16_F"| %4"U16_F" | (id, flags, offset)\r\n",
ntohs(IPH_ID(iphdr)),
ntohs(IPH_OFFSET(iphdr)) >> 15 & 1,
ntohs(IPH_OFFSET(iphdr)) >> 14 & 1,
ntohs(IPH_OFFSET(iphdr)) >> 13 & 1,
ntohs(IPH_OFFSET(iphdr)) & IP_OFFMASK);
_hx_printf("+-------------------------------+\r\n");
_hx_printf("| %3"U16_F" | %3"U16_F" | 0x%04"X16_F" | (ttl, proto, chksum)\r\n",
IPH_TTL(iphdr),
IPH_PROTO(iphdr),
ntohs(IPH_CHKSUM(iphdr)));
_hx_printf("+-------------------------------+\r\n");
_hx_printf("| %3"U16_F" | %3"U16_F" | %3"U16_F" | %3"U16_F" | (src)\r\n",
ip4_addr1_16(&iphdr->src),
ip4_addr2_16(&iphdr->src),
ip4_addr3_16(&iphdr->src),
ip4_addr4_16(&iphdr->src));
_hx_printf("+-------------------------------+\r\n");
_hx_printf("| %3"U16_F" | %3"U16_F" | %3"U16_F" | %3"U16_F" | (dest)\r\n",
ip4_addr1_16(&iphdr->dest),
ip4_addr2_16(&iphdr->dest),
ip4_addr3_16(&iphdr->dest),
ip4_addr4_16(&iphdr->dest));
_hx_printf("+-------------------------------+\r\n");
}
/**
* Receive callback function for UDP netconns.
* Posts the packet to conn->recvmbox or deletes it on memory error.
*
* @see udp.h (struct udp_pcb.recv) for parameters
*/
static void
recv_udp(void *arg, struct udp_pcb *pcb, struct pbuf *p,
ip_addr_t *addr, u16_t port)
{
struct netbuf *buf;
struct netconn *conn;
u16_t len;
#if LWIP_SO_RCVBUF
int recv_avail;
#endif /* LWIP_SO_RCVBUF */
LWIP_UNUSED_ARG(pcb); /* only used for asserts... */
LWIP_ASSERT("recv_udp must have a pcb argument", pcb != NULL);
LWIP_ASSERT("recv_udp must have an argument", arg != NULL);
conn = (struct netconn *)arg;
LWIP_ASSERT("recv_udp: recv for wrong pcb!", conn->pcb.udp == pcb);
#if LWIP_SO_RCVBUF
SYS_ARCH_GET(conn->recv_avail, recv_avail);
if ((conn == NULL) || !sys_mbox_valid(&conn->recvmbox) ||
((recv_avail + (int)(p->tot_len)) > conn->recv_bufsize)) {
#else /* LWIP_SO_RCVBUF */
if ((conn == NULL) || !sys_mbox_valid(&conn->recvmbox)) {
#endif /* LWIP_SO_RCVBUF */
pbuf_free(p);
return;
}
buf = (struct netbuf *)memp_malloc(MEMP_NETBUF);
if (buf == NULL) {
pbuf_free(p);
return;
} else {
buf->p = p;
buf->ptr = p;
ip_addr_set(&buf->addr, addr);
buf->port = port;
#if LWIP_NETBUF_RECVINFO
{
const struct ip_hdr* iphdr = ip_current_header();
/* get the UDP header - always in the first pbuf, ensured by udp_input */
const struct udp_hdr* udphdr = (void*)(((char*)iphdr) + IPH_LEN(iphdr));
#if LWIP_CHECKSUM_ON_COPY
buf->flags = NETBUF_FLAG_DESTADDR;
#endif /* LWIP_CHECKSUM_ON_COPY */
ip_addr_set(&buf->toaddr, ip_current_dest_addr());
buf->toport_chksum = udphdr->dest;
}
#endif /* LWIP_NETBUF_RECVINFO */
}
len = p->tot_len;
if (sys_mbox_trypost(&conn->recvmbox, buf) != ERR_OK) {
netbuf_delete(buf);
return;
} else {
#if LWIP_SO_RCVBUF
SYS_ARCH_INC(conn->recv_avail, len);
#endif /* LWIP_SO_RCVBUF */
/* Register event with callback */
API_EVENT(conn, NETCONN_EVT_RCVPLUS, len);
}
}
#endif /* LWIP_UDP */
#if LWIP_TCP
/**
* Receive callback function for TCP netconns.
* Posts the packet to conn->recvmbox, but doesn't delete it on errors.
*
* @see tcp.h (struct tcp_pcb.recv) for parameters and return value
*/
static err_t
recv_tcp(void *arg, struct tcp_pcb *pcb, struct pbuf *p, err_t err)
{
struct netconn *conn;
u16_t len;
LWIP_UNUSED_ARG(pcb);
LWIP_ASSERT("recv_tcp must have a pcb argument", pcb != NULL);
LWIP_ASSERT("recv_tcp must have an argument", arg != NULL);
conn = (struct netconn *)arg;
LWIP_ASSERT("recv_tcp: recv for wrong pcb!", conn->pcb.tcp == pcb);
if (conn == NULL) {
return ERR_VAL;
}
if (!sys_mbox_valid(&conn->recvmbox)) {
/* recvmbox already deleted */
if (p != NULL) {
tcp_recved(pcb, p->tot_len);
pbuf_free(p);
}
return ERR_OK;
}
/* Unlike for UDP or RAW pcbs, don't check for available space
using recv_avail since that could break the connection
(data is already ACKed) */
/* don't overwrite fatal errors! */
NETCONN_SET_SAFE_ERR(conn, err);
if (p != NULL) {
len = p->tot_len;
} else {
len = 0;
}
if (sys_mbox_trypost(&conn->recvmbox, p) != ERR_OK) {
/* don't deallocate p: it is presented to us later again from tcp_fasttmr! */
return ERR_MEM;
} else {
#if LWIP_SO_RCVBUF
SYS_ARCH_INC(conn->recv_avail, len);
#endif /* LWIP_SO_RCVBUF */
/* Register event with callback */
API_EVENT(conn, NETCONN_EVT_RCVPLUS, len);
}
return ERR_OK;
}
/**
* Poll callback function for TCP netconns.
* Wakes up an application thread that waits for a connection to close
* or data to be sent. The application thread then takes the
* appropriate action to go on.
*
* Signals the conn->sem.
* netconn_close waits for conn->sem if closing failed.
*
* @see tcp.h (struct tcp_pcb.poll) for parameters and return value
*/
static err_t
poll_tcp(void *arg, struct tcp_pcb *pcb)
{
struct netconn *conn = (struct netconn *)arg;
LWIP_UNUSED_ARG(pcb);
LWIP_ASSERT("conn != NULL", (conn != NULL));
if (conn->state == NETCONN_WRITE) {
do_writemore(conn);
} else if (conn->state == NETCONN_CLOSE) {
do_close_internal(conn);
}
/* @todo: implement connect timeout here? */
/* Did a nonblocking write fail before? Then check available write-space. */
if (conn->flags & NETCONN_FLAG_CHECK_WRITESPACE) {
/* If the queued byte- or pbuf-count drops below the configured low-water limit,
let select mark this pcb as writable again. */
if ((conn->pcb.tcp != NULL) && (tcp_sndbuf(conn->pcb.tcp) > TCP_SNDLOWAT) &&
(tcp_sndqueuelen(conn->pcb.tcp) < TCP_SNDQUEUELOWAT)) {
conn->flags &= ~NETCONN_FLAG_CHECK_WRITESPACE;
API_EVENT(conn, NETCONN_EVT_SENDPLUS, 0);
}
}
return ERR_OK;
}
/**
* Sent callback function for TCP netconns.
* Signals the conn->sem and calls API_EVENT.
* netconn_write waits for conn->sem if send buffer is low.
*
* @see tcp.h (struct tcp_pcb.sent) for parameters and return value
*/
static err_t
sent_tcp(void *arg, struct tcp_pcb *pcb, u16_t len)
{
struct netconn *conn = (struct netconn *)arg;
LWIP_UNUSED_ARG(pcb);
LWIP_ASSERT("conn != NULL", (conn != NULL));
if (conn->state == NETCONN_WRITE) {
do_writemore(conn);
} else if (conn->state == NETCONN_CLOSE) {
do_close_internal(conn);
}
if (conn) {
/* If the queued byte- or pbuf-count drops below the configured low-water limit,
let select mark this pcb as writable again. */
if ((conn->pcb.tcp != NULL) && (tcp_sndbuf(conn->pcb.tcp) > TCP_SNDLOWAT) &&
(tcp_sndqueuelen(conn->pcb.tcp) < TCP_SNDQUEUELOWAT)) {
conn->flags &= ~NETCONN_FLAG_CHECK_WRITESPACE;
API_EVENT(conn, NETCONN_EVT_SENDPLUS, len);
}
}
return ERR_OK;
}
/**
* Error callback function for TCP netconns.
* Signals conn->sem, posts to all conn mboxes and calls API_EVENT.
* The application thread has then to decide what to do.
*
* @see tcp.h (struct tcp_pcb.err) for parameters
*/
static void
err_tcp(void *arg, err_t err)
{
struct netconn *conn;
enum netconn_state old_state;
SYS_ARCH_DECL_PROTECT(lev);
conn = (struct netconn *)arg;
LWIP_ASSERT("conn != NULL", (conn != NULL));
conn->pcb.tcp = NULL;
/* no check since this is always fatal! */
SYS_ARCH_PROTECT(lev);
conn->last_err = err;
SYS_ARCH_UNPROTECT(lev);
/* reset conn->state now before waking up other threads */
old_state = conn->state;
conn->state = NETCONN_NONE;
/* Notify the user layer about a connection error. Used to signal
select. */
API_EVENT(conn, NETCONN_EVT_ERROR, 0);
/* Try to release selects pending on 'read' or 'write', too.
They will get an error if they actually try to read or write. */
API_EVENT(conn, NETCONN_EVT_RCVPLUS, 0);
API_EVENT(conn, NETCONN_EVT_SENDPLUS, 0);
/* pass NULL-message to recvmbox to wake up pending recv */
if (sys_mbox_valid(&conn->recvmbox)) {
/* use trypost to prevent deadlock */
sys_mbox_trypost(&conn->recvmbox, NULL);
}
/* pass NULL-message to acceptmbox to wake up pending accept */
if (sys_mbox_valid(&conn->acceptmbox)) {
/* use trypost to preven deadlock */
sys_mbox_trypost(&conn->acceptmbox, NULL);
}
if ((old_state == NETCONN_WRITE) || (old_state == NETCONN_CLOSE) ||
(old_state == NETCONN_CONNECT)) {
/* calling do_writemore/do_close_internal is not necessary
since the pcb has already been deleted! */
int was_nonblocking_connect = IN_NONBLOCKING_CONNECT(conn);
SET_NONBLOCKING_CONNECT(conn, 0);
if (!was_nonblocking_connect) {
/* set error return code */
LWIP_ASSERT("conn->current_msg != NULL", conn->current_msg != NULL);
conn->current_msg->err = err;
conn->current_msg = NULL;
/* wake up the waiting task */
sys_sem_signal(&conn->op_completed);
}
} else {
LWIP_ASSERT("conn->current_msg == NULL", conn->current_msg == NULL);
}
}
/**
* Setup a tcp_pcb with the correct callback function pointers
* and their arguments.
*
* @param conn the TCP netconn to setup
*/
static void
setup_tcp(struct netconn *conn)
{
struct tcp_pcb *pcb;
pcb = conn->pcb.tcp;
tcp_arg(pcb, conn);
tcp_recv(pcb, recv_tcp);
tcp_sent(pcb, sent_tcp);
tcp_poll(pcb, poll_tcp, 4);
tcp_err(pcb, err_tcp);
}
/**
* Accept callback function for TCP netconns.
* Allocates a new netconn and posts that to conn->acceptmbox.
*
* @see tcp.h (struct tcp_pcb_listen.accept) for parameters and return value
*/
static err_t
accept_function(void *arg, struct tcp_pcb *newpcb, err_t err)
{
struct netconn *newconn;
struct netconn *conn = (struct netconn *)arg;
LWIP_DEBUGF(API_MSG_DEBUG, ("accept_function: newpcb->tate: %s\n", tcp_debug_state_str(newpcb->state)));
if (!sys_mbox_valid(&conn->acceptmbox)) {
LWIP_DEBUGF(API_MSG_DEBUG, ("accept_function: acceptmbox already deleted\n"));
return ERR_VAL;
}
/* We have to set the callback here even though
* the new socket is unknown. conn->socket is marked as -1. */
newconn = netconn_alloc(conn->type, conn->callback);
if (newconn == NULL) {
return ERR_MEM;
}
newconn->pcb.tcp = newpcb;
setup_tcp(newconn);
/* no protection: when creating the pcb, the netconn is not yet known
to the application thread */
newconn->last_err = err;
if (sys_mbox_trypost(&conn->acceptmbox, newconn) != ERR_OK) {
/* When returning != ERR_OK, the pcb is aborted in tcp_process(),
so do nothing here! */
/* remove all references to this netconn from the pcb */
struct tcp_pcb* pcb = newconn->pcb.tcp;
tcp_arg(pcb, NULL);
tcp_recv(pcb, NULL);
tcp_sent(pcb, NULL);
tcp_poll(pcb, NULL, 4);
tcp_err(pcb, NULL);
/* remove reference from to the pcb from this netconn */
newconn->pcb.tcp = NULL;
/* no need to drain since we know the recvmbox is empty. */
sys_mbox_free(&newconn->recvmbox);
sys_mbox_set_invalid(&newconn->recvmbox);
netconn_free(newconn);
return ERR_MEM;
} else {
/* Register event with callback */
API_EVENT(conn, NETCONN_EVT_RCVPLUS, 0);
}
return ERR_OK;
}
#endif /* LWIP_TCP */
/**
* Create a new pcb of a specific type.
* Called from do_newconn().
*
* @param msg the api_msg_msg describing the connection type
* @return msg->conn->err, but the return value is currently ignored
*/
static void
pcb_new(struct api_msg_msg *msg)
{
LWIP_ASSERT("pcb_new: pcb already allocated", msg->conn->pcb.tcp == NULL);
/* Allocate a PCB for this connection */
switch(NETCONNTYPE_GROUP(msg->conn->type)) {
#if LWIP_RAW
case NETCONN_RAW:
msg->conn->pcb.raw = raw_new(msg->msg.n.proto);
if(msg->conn->pcb.raw == NULL) {
msg->err = ERR_MEM;
break;
}
raw_recv(msg->conn->pcb.raw, recv_raw, msg->conn);
break;
#endif /* LWIP_RAW */
#if LWIP_UDP
case NETCONN_UDP:
msg->conn->pcb.udp = udp_new();
if(msg->conn->pcb.udp == NULL) {
msg->err = ERR_MEM;
break;
}
#if LWIP_UDPLITE
if (msg->conn->type==NETCONN_UDPLITE) {
udp_setflags(msg->conn->pcb.udp, UDP_FLAGS_UDPLITE);
}
#endif /* LWIP_UDPLITE */
if (msg->conn->type==NETCONN_UDPNOCHKSUM) {
udp_setflags(msg->conn->pcb.udp, UDP_FLAGS_NOCHKSUM);
}
udp_recv(msg->conn->pcb.udp, recv_udp, msg->conn);
break;
#endif /* LWIP_UDP */
#if LWIP_TCP
case NETCONN_TCP:
msg->conn->pcb.tcp = tcp_new();
if(msg->conn->pcb.tcp == NULL) {
msg->err = ERR_MEM;
break;
}
setup_tcp(msg->conn);
break;
#endif /* LWIP_TCP */
default:
/* Unsupported netconn type, e.g. protocol disabled */
msg->err = ERR_VAL;
break;
}
}
/**
* Reassembles incoming IP fragments into an IP datagram.
*
* @param p points to a pbuf chain of the fragment
* @return NULL if reassembly is incomplete, ? otherwise
*/
struct pbuf *
ip_reass(struct pbuf *p)
{
struct pbuf *r;
struct ip_hdr *fraghdr;
struct ip_reassdata *ipr;
struct ip_reass_helper *iprh;
u16_t offset, len;
u8_t clen;
struct ip_reassdata *ipr_prev = NULL;
IPFRAG_STATS_INC(ip_frag.recv);
snmp_inc_ipreasmreqds();
fraghdr = (struct ip_hdr*)p->payload;
if ((IPH_HL(fraghdr) * 4) != IP_HLEN) {
LWIP_DEBUGF(IP_REASS_DEBUG,("ip_reass: IP options currently not supported!\n"));
IPFRAG_STATS_INC(ip_frag.err);
goto nullreturn;
}
offset = (ntohs(IPH_OFFSET(fraghdr)) & IP_OFFMASK) * 8;
len = ntohs(IPH_LEN(fraghdr)) - IPH_HL(fraghdr) * 4;
/* Check if we are allowed to enqueue more datagrams. */
clen = pbuf_clen(p);
if ((ip_reass_pbufcount + clen) > IP_REASS_MAX_PBUFS) {
#if IP_REASS_FREE_OLDEST
if (!ip_reass_remove_oldest_datagram(fraghdr, clen) ||
((ip_reass_pbufcount + clen) > IP_REASS_MAX_PBUFS))
#endif /* IP_REASS_FREE_OLDEST */
{
/* No datagram could be freed and still too many pbufs enqueued */
LWIP_DEBUGF(IP_REASS_DEBUG,("ip_reass: Overflow condition: pbufct=%d, clen=%d, MAX=%d\n",
ip_reass_pbufcount, clen, IP_REASS_MAX_PBUFS));
IPFRAG_STATS_INC(ip_frag.memerr);
/* @todo: send ICMP time exceeded here? */
/* drop this pbuf */
goto nullreturn;
}
}
/* Look for the datagram the fragment belongs to in the current datagram queue,
* remembering the previous in the queue for later dequeueing. */
for (ipr = reassdatagrams; ipr != NULL; ipr = ipr->next) {
/* Check if the incoming fragment matches the one currently present
in the reassembly buffer. If so, we proceed with copying the
fragment into the buffer. */
if (IP_ADDRESSES_AND_ID_MATCH(&ipr->iphdr, fraghdr)) {
LWIP_DEBUGF(IP_REASS_DEBUG, ("ip_reass: matching previous fragment ID=%"X16_F"\n",
ntohs(IPH_ID(fraghdr))));
IPFRAG_STATS_INC(ip_frag.cachehit);
break;
}
ipr_prev = ipr;
}
if (ipr == NULL) {
/* Enqueue a new datagram into the datagram queue */
ipr = ip_reass_enqueue_new_datagram(fraghdr, clen);
/* Bail if unable to enqueue */
if(ipr == NULL) {
goto nullreturn;
}
} else {
if (((ntohs(IPH_OFFSET(fraghdr)) & IP_OFFMASK) == 0) &&
((ntohs(IPH_OFFSET(&ipr->iphdr)) & IP_OFFMASK) != 0)) {
/* ipr->iphdr is not the header from the first fragment, but fraghdr is
* -> copy fraghdr into ipr->iphdr since we want to have the header
* of the first fragment (for ICMP time exceeded and later, for copying
* all options, if supported)*/
SMEMCPY(&ipr->iphdr, fraghdr, IP_HLEN);
}
}
/* Track the current number of pbufs current 'in-flight', in order to limit
the number of fragments that may be enqueued at any one time */
ip_reass_pbufcount += clen;
/* At this point, we have either created a new entry or pointing
* to an existing one */
/* check for 'no more fragments', and update queue entry*/
if ((IPH_OFFSET(fraghdr) & PP_NTOHS(IP_MF)) == 0) {
ipr->flags |= IP_REASS_FLAG_LASTFRAG;
ipr->datagram_len = offset + len;
LWIP_DEBUGF(IP_REASS_DEBUG,
("ip_reass: last fragment seen, total len %"S16_F"\n",
ipr->datagram_len));
}
/* find the right place to insert this pbuf */
/* @todo: trim pbufs if fragments are overlapping */
if (ip_reass_chain_frag_into_datagram_and_validate(ipr, p)) {
/* the totally last fragment (flag more fragments = 0) was received at least
* once AND all fragments are received */
ipr->datagram_len += IP_HLEN;
/* save the second pbuf before copying the header over the pointer */
r = ((struct ip_reass_helper*)ipr->p->payload)->next_pbuf;
/* copy the original ip header back to the first pbuf */
fraghdr = (struct ip_hdr*)(ipr->p->payload);
SMEMCPY(fraghdr, &ipr->iphdr, IP_HLEN);
IPH_LEN_SET(fraghdr, htons(ipr->datagram_len));
IPH_OFFSET_SET(fraghdr, 0);
IPH_CHKSUM_SET(fraghdr, 0);
/* @todo: do we need to set calculate the correct checksum? */
IPH_CHKSUM_SET(fraghdr, inet_chksum(fraghdr, IP_HLEN));
p = ipr->p;
/* chain together the pbufs contained within the reass_data list. */
while(r != NULL) {
iprh = (struct ip_reass_helper*)r->payload;
/* hide the ip header for every succeding fragment */
pbuf_header(r, -IP_HLEN);
pbuf_cat(p, r);
r = iprh->next_pbuf;
}
/* release the sources allocate for the fragment queue entry */
ip_reass_dequeue_datagram(ipr, ipr_prev);
/* and adjust the number of pbufs currently queued for reassembly. */
ip_reass_pbufcount -= pbuf_clen(p);
/* Return the pbuf chain */
return p;
}
/* the datagram is not (yet?) reassembled completely */
LWIP_DEBUGF(IP_REASS_DEBUG,("ip_reass_pbufcount: %d out\n", ip_reass_pbufcount));
return NULL;
nullreturn:
LWIP_DEBUGF(IP_REASS_DEBUG,("ip_reass: nullreturn\n"));
IPFRAG_STATS_INC(ip_frag.drop);
pbuf_free(p);
return NULL;
}
/**
* Chain a new pbuf into the pbuf list that composes the datagram. The pbuf list
* will grow over time as new pbufs are rx.
* Also checks that the datagram passes basic continuity checks (if the last
* fragment was received at least once).
* @param root_p points to the 'root' pbuf for the current datagram being assembled.
* @param new_p points to the pbuf for the current fragment
* @return 0 if invalid, >0 otherwise
*/
static int
ip_reass_chain_frag_into_datagram_and_validate(struct ip_reassdata *ipr, struct pbuf *new_p)
{
struct ip_reass_helper *iprh, *iprh_tmp, *iprh_prev=NULL;
struct pbuf *q;
u16_t offset,len;
struct ip_hdr *fraghdr;
int valid = 1;
/* Extract length and fragment offset from current fragment */
fraghdr = (struct ip_hdr*)new_p->payload;
len = ntohs(IPH_LEN(fraghdr)) - IPH_HL(fraghdr) * 4;
offset = (ntohs(IPH_OFFSET(fraghdr)) & IP_OFFMASK) * 8;
/* overwrite the fragment's ip header from the pbuf with our helper struct,
* and setup the embedded helper structure. */
/* make sure the struct ip_reass_helper fits into the IP header */
LWIP_ASSERT("sizeof(struct ip_reass_helper) <= IP_HLEN",
sizeof(struct ip_reass_helper) <= IP_HLEN);
iprh = (struct ip_reass_helper*)new_p->payload;
iprh->next_pbuf = NULL;
iprh->start = offset;
iprh->end = offset + len;
/* Iterate through until we either get to the end of the list (append),
* or we find on with a larger offset (insert). */
for (q = ipr->p; q != NULL;) {
iprh_tmp = (struct ip_reass_helper*)q->payload;
if (iprh->start < iprh_tmp->start) {
/* the new pbuf should be inserted before this */
iprh->next_pbuf = q;
if (iprh_prev != NULL) {
/* not the fragment with the lowest offset */
#if IP_REASS_CHECK_OVERLAP
if ((iprh->start < iprh_prev->end) || (iprh->end > iprh_tmp->start)) {
/* fragment overlaps with previous or following, throw away */
goto freepbuf;
}
#endif /* IP_REASS_CHECK_OVERLAP */
iprh_prev->next_pbuf = new_p;
} else {
/* fragment with the lowest offset */
ipr->p = new_p;
}
break;
} else if(iprh->start == iprh_tmp->start) {
/* received the same datagram twice: no need to keep the datagram */
goto freepbuf;
#if IP_REASS_CHECK_OVERLAP
} else if(iprh->start < iprh_tmp->end) {
/* overlap: no need to keep the new datagram */
goto freepbuf;
#endif /* IP_REASS_CHECK_OVERLAP */
} else {
/* Check if the fragments received so far have no wholes. */
if (iprh_prev != NULL) {
if (iprh_prev->end != iprh_tmp->start) {
/* There is a fragment missing between the current
* and the previous fragment */
valid = 0;
}
}
}
q = iprh_tmp->next_pbuf;
iprh_prev = iprh_tmp;
}
/* If q is NULL, then we made it to the end of the list. Determine what to do now */
if (q == NULL) {
if (iprh_prev != NULL) {
/* this is (for now), the fragment with the highest offset:
* chain it to the last fragment */
#if IP_REASS_CHECK_OVERLAP
LWIP_ASSERT("check fragments don't overlap", iprh_prev->end <= iprh->start);
#endif /* IP_REASS_CHECK_OVERLAP */
iprh_prev->next_pbuf = new_p;
if (iprh_prev->end != iprh->start) {
valid = 0;
}
} else {
#if IP_REASS_CHECK_OVERLAP
LWIP_ASSERT("no previous fragment, this must be the first fragment!",
ipr->p == NULL);
#endif /* IP_REASS_CHECK_OVERLAP */
/* this is the first fragment we ever received for this ip datagram */
ipr->p = new_p;
}
}
/* At this point, the validation part begins: */
/* If we already received the last fragment */
if ((ipr->flags & IP_REASS_FLAG_LASTFRAG) != 0) {
/* and had no wholes so far */
if (valid) {
/* then check if the rest of the fragments is here */
/* Check if the queue starts with the first datagram */
if (((struct ip_reass_helper*)ipr->p->payload)->start != 0) {
valid = 0;
} else {
/* and check that there are no wholes after this datagram */
iprh_prev = iprh;
q = iprh->next_pbuf;
while (q != NULL) {
iprh = (struct ip_reass_helper*)q->payload;
if (iprh_prev->end != iprh->start) {
valid = 0;
break;
}
iprh_prev = iprh;
q = iprh->next_pbuf;
}
/* if still valid, all fragments are received
* (because to the MF==0 already arrived */
if (valid) {
LWIP_ASSERT("sanity check", ipr->p != NULL);
LWIP_ASSERT("sanity check",
((struct ip_reass_helper*)ipr->p->payload) != iprh);
LWIP_ASSERT("validate_datagram:next_pbuf!=NULL",
iprh->next_pbuf == NULL);
LWIP_ASSERT("validate_datagram:datagram end!=datagram len",
iprh->end == ipr->datagram_len);
}
}
}
/* If valid is 0 here, there are some fragments missing in the middle
* (since MF == 0 has already arrived). Such datagrams simply time out if
* no more fragments are received... */
return valid;
}
/* If we come here, not all fragments were received, yet! */
return 0; /* not yet valid! */
#if IP_REASS_CHECK_OVERLAP
freepbuf:
ip_reass_pbufcount -= pbuf_clen(new_p);
pbuf_free(new_p);
return 0;
#endif /* IP_REASS_CHECK_OVERLAP */
}
/*-----------------------------------------------------------------------------------*/
void
tcpdump(struct pbuf *p)
{
struct ip_hdr *iphdr;
#if LWIP_UDP
struct udp_hdr *udphdr;
#endif
#if LWIP_TCP
struct tcp_hdr *tcphdr;
char flags[5];
int i;
int len;
int offset;
#endif
if (file == NULL) {
return;
}
iphdr = (struct ip_hdr *)p->payload;
switch (IPH_PROTO(iphdr)) {
#if LWIP_TCP
case IP_PROTO_TCP:
tcphdr = (struct tcp_hdr *)((char *)iphdr + IP_HLEN);
pbuf_header(p, -IP_HLEN);
if (inet_chksum_pseudo(p, IP_PROTO_TCP, p->tot_len,
(ip4_addr_t *)&(iphdr->src),
(ip4_addr_t *)&(iphdr->dest)) != 0) {
LWIP_DEBUGF(TCPDUMP_DEBUG, ("tcpdump: IP checksum failed!\n"));
/*
fprintf(file, "chksum 0x%lx ", tcphdr->chksum);
tcphdr->chksum = 0;
fprintf(file, "should be 0x%lx ", inet_chksum_pseudo(p, (ip_addr_t *)&(iphdr->src),
(ip_addr_t *)&(iphdr->dest), IP_PROTO_TCP, p->tot_len));*/
fprintf(file, "!chksum ");
}
i = 0;
if (TCPH_FLAGS(tcphdr) & TCP_SYN) {
flags[i++] = 'S';
}
if (TCPH_FLAGS(tcphdr) & TCP_PSH) {
flags[i++] = 'P';
}
if (TCPH_FLAGS(tcphdr) & TCP_FIN) {
flags[i++] = 'F';
}
if (TCPH_FLAGS(tcphdr) & TCP_RST) {
flags[i++] = 'R';
}
if (i == 0) {
flags[i++] = '.';
}
flags[i++] = 0;
fprintf(file, "%d.%d.%d.%d.%u > %d.%d.%d.%d.%u: ",
(int)(ntohl(iphdr->src.addr) >> 24) & 0xff,
(int)(ntohl(iphdr->src.addr) >> 16) & 0xff,
(int)(ntohl(iphdr->src.addr) >> 8) & 0xff,
(int)(ntohl(iphdr->src.addr) >> 0) & 0xff,
ntohs(tcphdr->src),
(int)(ntohl(iphdr->dest.addr) >> 24) & 0xff,
(int)(ntohl(iphdr->dest.addr) >> 16) & 0xff,
(int)(ntohl(iphdr->dest.addr) >> 8) & 0xff,
(int)(ntohl(iphdr->dest.addr) >> 0) & 0xff,
ntohs(tcphdr->dest));
offset = TCPH_HDRLEN(tcphdr);
len = ntohs(IPH_LEN(iphdr)) - offset * 4 - IP_HLEN;
if (len != 0 || flags[0] != '.') {
fprintf(file, "%s %u:%u(%u) ", flags, ntohl(tcphdr->seqno),
ntohl(tcphdr->seqno) + len, len);
}
if (TCPH_FLAGS(tcphdr) & TCP_ACK) {
fprintf(file, "ack %u ", ntohl(tcphdr->ackno));
}
fprintf(file, "wnd %u\n", ntohs(tcphdr->wnd));
fflush(file);
pbuf_header(p, IP_HLEN);
break;
#endif /* LWIP_TCP */
#if LWIP_UDP
case IP_PROTO_UDP:
udphdr = (struct udp_hdr *)((char *)iphdr + IP_HLEN);
pbuf_header(p, -IP_HLEN);
if (inet_chksum_pseudo(p, IP_PROTO_UDP, p->tot_len,
(ip4_addr_t *)&(iphdr->src),
(ip4_addr_t *)&(iphdr->dest)) != 0) {
LWIP_DEBUGF(TCPDUMP_DEBUG, ("tcpdump: IP checksum failed!\n"));
/*
fprintf(file, "chksum 0x%lx ", tcphdr->chksum);
tcphdr->chksum = 0;
fprintf(file, "should be 0x%lx ", inet_chksum_pseudo(p, (ip_addr_t *)&(iphdr->src),
(ip_addr_t *)&(iphdr->dest), IP_PROTO_TCP, p->tot_len));*/
fprintf(file, "!chksum ");
}
fprintf(file, "%d.%d.%d.%d.%u > %d.%d.%d.%d.%u: ",
(int)(ntohl(iphdr->src.addr) >> 24) & 0xff,
(int)(ntohl(iphdr->src.addr) >> 16) & 0xff,
(int)(ntohl(iphdr->src.addr) >> 8) & 0xff,
(int)(ntohl(iphdr->src.addr) >> 0) & 0xff,
(int)(ntohl(iphdr->dest.addr) >> 24) & 0xff,
ntohs(udphdr->src),
(int)(ntohl(iphdr->dest.addr) >> 16) & 0xff,
(int)(ntohl(iphdr->dest.addr) >> 8) & 0xff,
(int)(ntohl(iphdr->dest.addr) >> 0) & 0xff,
ntohs(udphdr->dest));
fprintf(file, "U ");
len = ntohs(IPH_LEN(iphdr)) - sizeof(struct udp_hdr) - IP_HLEN;
fprintf(file, " %d\n", len);
fflush(file);
pbuf_header(p, IP_HLEN);
break;
#endif /* LWIP_UDP */
default:
LWIP_DEBUGF(TCPDUMP_DEBUG, ("unhandled IP protocol: %d\n", (int)IPH_PROTO(iphdr)));
break;
}
}
/**
* This function is called by the network interface device driver when
* an IP packet is received. The function does the basic checks of the
* IP header such as packet size being at least larger than the header
* size etc. If the packet was not destined for us, the packet is
* forwarded (using ip_forward). The IP checksum is always checked.
*
* Finally, the packet is sent to the upper layer protocol input function.
*
* @param p the received IP packet (p->payload points to IP header)
* @param inp the netif on which this packet was received
* @return ERR_OK if the packet was processed (could return ERR_* if it wasn't
* processed, but currently always returns ERR_OK)
*/
err_t
ip_input(struct pbuf *p, struct netif *inp)
{
struct ip_hdr *iphdr;
struct netif *netif;
u16_t iphdr_hlen;
u16_t iphdr_len;
#if LWIP_DHCP
int check_ip_src=1;
#endif /* LWIP_DHCP */
IP_STATS_INC(ip.recv);
snmp_inc_ipinreceives();
/* identify the IP header */
iphdr = p->payload;
if (IPH_V(iphdr) != 4) {
LWIP_DEBUGF(IP_DEBUG | LWIP_DBG_LEVEL_WARNING, ("IP packet dropped due to bad version number %"U16_F"\n", IPH_V(iphdr)));
ip_debug_print(p);
pbuf_free(p);
IP_STATS_INC(ip.err);
IP_STATS_INC(ip.drop);
snmp_inc_ipinhdrerrors();
return ERR_OK;
}
/* obtain IP header length in number of 32-bit words */
iphdr_hlen = IPH_HL(iphdr);
/* calculate IP header length in bytes */
iphdr_hlen *= 4;
/* obtain ip length in bytes */
iphdr_len = ntohs(IPH_LEN(iphdr));
/* header length exceeds first pbuf length, or ip length exceeds total pbuf length? */
if ((iphdr_hlen > p->len) || (iphdr_len > p->tot_len)) {
if (iphdr_hlen > p->len) {
LWIP_DEBUGF(IP_DEBUG | LWIP_DBG_LEVEL_SERIOUS,
("IP header (len %"U16_F") does not fit in first pbuf (len %"U16_F"), IP packet dropped.\n",
iphdr_hlen, p->len));
}
if (iphdr_len > p->tot_len) {
LWIP_DEBUGF(IP_DEBUG | LWIP_DBG_LEVEL_SERIOUS,
("IP (len %"U16_F") is longer than pbuf (len %"U16_F"), IP packet dropped.\n",
iphdr_len, p->tot_len));
}
/* free (drop) packet pbufs */
pbuf_free(p);
IP_STATS_INC(ip.lenerr);
IP_STATS_INC(ip.drop);
snmp_inc_ipindiscards();
return ERR_OK;
}
/* verify checksum */
#if CHECKSUM_CHECK_IP
if (inet_chksum(iphdr, iphdr_hlen) != 0) {
LWIP_DEBUGF(IP_DEBUG | LWIP_DBG_LEVEL_SERIOUS,
("Checksum (0x%"X16_F") failed, IP packet dropped.\n", inet_chksum(iphdr, iphdr_hlen)));
ip_debug_print(p);
pbuf_free(p);
IP_STATS_INC(ip.chkerr);
IP_STATS_INC(ip.drop);
snmp_inc_ipinhdrerrors();
return ERR_OK;
}
#endif
/* Trim pbuf. This should have been done at the netif layer,
* but we'll do it anyway just to be sure that its done. */
pbuf_realloc(p, iphdr_len);
/* match packet against an interface, i.e. is this packet for us? */
#if LWIP_IGMP
if (ip_addr_ismulticast(&(iphdr->dest))) {
if ((inp->flags & NETIF_FLAG_IGMP) && (igmp_lookfor_group(inp, &(iphdr->dest)))) {
netif = inp;
} else {
netif = NULL;
}
} else
#endif /* LWIP_IGMP */
{
/* start trying with inp. if that's not acceptable, start walking the
list of configured netifs.
'first' is used as a boolean to mark whether we started walking the list */
int first = 1;
netif = inp;
do {
LWIP_DEBUGF(IP_DEBUG, ("ip_input: iphdr->dest 0x%"X32_F" netif->ip_addr 0x%"X32_F" (0x%"X32_F", 0x%"X32_F", 0x%"X32_F")\n",
iphdr->dest.addr, netif->ip_addr.addr,
iphdr->dest.addr & netif->netmask.addr,
netif->ip_addr.addr & netif->netmask.addr,
iphdr->dest.addr & ~(netif->netmask.addr)));
/* interface is up and configured? */
if ((netif_is_up(netif)) && (!ip_addr_isany(&(netif->ip_addr)))) {
/* unicast to this interface address? */
if (ip_addr_cmp(&(iphdr->dest), &(netif->ip_addr)) ||
/* or broadcast on this interface network address? */
ip_addr_isbroadcast(&(iphdr->dest), netif)) {
LWIP_DEBUGF(IP_DEBUG, ("ip_input: packet accepted on interface %c%c\n",
netif->name[0], netif->name[1]));
/* break out of for loop */
break;
}
}
if (first) {
first = 0;
netif = netif_list;
} else {
netif = netif->next;
}
if (netif == inp) {
netif = netif->next;
}
} while(netif != NULL);
}
#if LWIP_DHCP
/* Pass DHCP messages regardless of destination address. DHCP traffic is addressed
* using link layer addressing (such as Ethernet MAC) so we must not filter on IP.
* According to RFC 1542 section 3.1.1, referred by RFC 2131).
*/
if (netif == NULL) {
/* remote port is DHCP server? */
if (IPH_PROTO(iphdr) == IP_PROTO_UDP) {
LWIP_DEBUGF(IP_DEBUG | LWIP_DBG_TRACE, ("ip_input: UDP packet to DHCP client port %"U16_F"\n",
ntohs(((struct udp_hdr *)((u8_t *)iphdr + iphdr_hlen))->dest)));
if (ntohs(((struct udp_hdr *)((u8_t *)iphdr + iphdr_hlen))->dest) == DHCP_CLIENT_PORT) {
LWIP_DEBUGF(IP_DEBUG | LWIP_DBG_TRACE, ("ip_input: DHCP packet accepted.\n"));
netif = inp;
check_ip_src = 0;
}
}
}
#endif /* LWIP_DHCP */
/* broadcast or multicast packet source address? Compliant with RFC 1122: 3.2.1.3 */
#if LWIP_DHCP
/* DHCP servers need 0.0.0.0 to be allowed as source address (RFC 1.1.2.2: 3.2.1.3/a) */
if (check_ip_src && (iphdr->src.addr != 0))
#endif /* LWIP_DHCP */
{ if ((ip_addr_isbroadcast(&(iphdr->src), inp)) ||
(ip_addr_ismulticast(&(iphdr->src)))) {
/* packet source is not valid */
LWIP_DEBUGF(IP_DEBUG | LWIP_DBG_TRACE | LWIP_DBG_LEVEL_WARNING, ("ip_input: packet source is not valid.\n"));
/* free (drop) packet pbufs */
pbuf_free(p);
IP_STATS_INC(ip.drop);
snmp_inc_ipinaddrerrors();
snmp_inc_ipindiscards();
return ERR_OK;
}
}
/* packet not for us? */
if (netif == NULL) {
/* packet not for us, route or discard */
LWIP_DEBUGF(IP_DEBUG | LWIP_DBG_TRACE, ("ip_input: packet not for us.\n"));
#if IP_FORWARD
/* non-broadcast packet? */
if (!ip_addr_isbroadcast(&(iphdr->dest), inp)) {
/* try to forward IP packet on (other) interfaces */
ip_forward(p, iphdr, inp);
} else
#endif /* IP_FORWARD */
{
snmp_inc_ipinaddrerrors();
snmp_inc_ipindiscards();
}
pbuf_free(p);
return ERR_OK;
}
/* packet consists of multiple fragments? */
if ((IPH_OFFSET(iphdr) & htons(IP_OFFMASK | IP_MF)) != 0) {
#if IP_REASSEMBLY /* packet fragment reassembly code present? */
LWIP_DEBUGF(IP_DEBUG, ("IP packet is a fragment (id=0x%04"X16_F" tot_len=%"U16_F" len=%"U16_F" MF=%"U16_F" offset=%"U16_F"), calling ip_reass()\n",
ntohs(IPH_ID(iphdr)), p->tot_len, ntohs(IPH_LEN(iphdr)), !!(IPH_OFFSET(iphdr) & htons(IP_MF)), (ntohs(IPH_OFFSET(iphdr)) & IP_OFFMASK)*8));
/* reassemble the packet*/
p = ip_reass(p);
/* packet not fully reassembled yet? */
if (p == NULL) {
return ERR_OK;
}
iphdr = p->payload;
#else /* IP_REASSEMBLY == 0, no packet fragment reassembly code present */
pbuf_free(p);
LWIP_DEBUGF(IP_DEBUG | LWIP_DBG_LEVEL_SERIOUS, ("IP packet dropped since it was fragmented (0x%"X16_F") (while IP_REASSEMBLY == 0).\n",
ntohs(IPH_OFFSET(iphdr))));
IP_STATS_INC(ip.opterr);
IP_STATS_INC(ip.drop);
/* unsupported protocol feature */
snmp_inc_ipinunknownprotos();
return ERR_OK;
#endif /* IP_REASSEMBLY */
}
#if IP_OPTIONS_ALLOWED == 0 /* no support for IP options in the IP header? */
#if LWIP_IGMP
/* there is an extra "router alert" option in IGMP messages which we allow for but do not police */
if((iphdr_hlen > IP_HLEN && (IPH_PROTO(iphdr) != IP_PROTO_IGMP)) {
#else
if (iphdr_hlen > IP_HLEN) {
#endif /* LWIP_IGMP */
LWIP_DEBUGF(IP_DEBUG | LWIP_DBG_LEVEL_SERIOUS, ("IP packet dropped since there were IP options (while IP_OPTIONS_ALLOWED == 0).\n"));
pbuf_free(p);
IP_STATS_INC(ip.opterr);
IP_STATS_INC(ip.drop);
/* unsupported protocol feature */
snmp_inc_ipinunknownprotos();
return ERR_OK;
}
#endif /* IP_OPTIONS_ALLOWED == 0 */
/* send to upper layers */
LWIP_DEBUGF(IP_DEBUG, ("ip_input: \n"));
ip_debug_print(p);
LWIP_DEBUGF(IP_DEBUG, ("ip_input: p->len %"U16_F" p->tot_len %"U16_F"\n", p->len, p->tot_len));
current_netif = inp;
current_header = iphdr;
#if LWIP_RAW
/* raw input did not eat the packet? */
if (raw_input(p, inp) == 0)
#endif /* LWIP_RAW */
{
switch (IPH_PROTO(iphdr)) {
#if LWIP_UDP
case IP_PROTO_UDP:
#if LWIP_UDPLITE
case IP_PROTO_UDPLITE:
#endif /* LWIP_UDPLITE */
snmp_inc_ipindelivers();
udp_input(p, inp);
break;
#endif /* LWIP_UDP */
#if LWIP_TCP
case IP_PROTO_TCP:
snmp_inc_ipindelivers();
tcp_input(p, inp);
break;
#endif /* LWIP_TCP */
#if LWIP_ICMP
case IP_PROTO_ICMP:
snmp_inc_ipindelivers();
icmp_input(p, inp);
break;
#endif /* LWIP_ICMP */
#if LWIP_IGMP
case IP_PROTO_IGMP:
igmp_input(p,inp,&(iphdr->dest));
break;
#endif /* LWIP_IGMP */
default:
#if LWIP_ICMP
/* send ICMP destination protocol unreachable unless is was a broadcast */
if (!ip_addr_isbroadcast(&(iphdr->dest), inp) &&
!ip_addr_ismulticast(&(iphdr->dest))) {
p->payload = iphdr;
icmp_dest_unreach(p, ICMP_DUR_PROTO);
}
#endif /* LWIP_ICMP */
pbuf_free(p);
LWIP_DEBUGF(IP_DEBUG | LWIP_DBG_LEVEL_SERIOUS, ("Unsupported transport protocol %"U16_F"\n", IPH_PROTO(iphdr)));
IP_STATS_INC(ip.proterr);
IP_STATS_INC(ip.drop);
snmp_inc_ipinunknownprotos();
}
}
current_netif = NULL;
current_header = NULL;
return ERR_OK;
}
/**
* Sends an IP packet on a network interface. This function constructs
* the IP header and calculates the IP header checksum. If the source
* IP address is NULL, the IP address of the outgoing network
* interface is filled in as source address.
* If the destination IP address is IP_HDRINCL, p is assumed to already
* include an IP header and p->payload points to it instead of the data.
*
* @param p the packet to send (p->payload points to the data, e.g. next
protocol header; if dest == IP_HDRINCL, p already includes an IP
header and p->payload points to that IP header)
* @param src the source IP address to send from (if src == IP_ADDR_ANY, the
* IP address of the netif used to send is used as source address)
* @param dest the destination IP address to send the packet to
* @param ttl the TTL value to be set in the IP header
* @param tos the TOS value to be set in the IP header
* @param proto the PROTOCOL to be set in the IP header
* @param netif the netif on which to send this packet
* @return ERR_OK if the packet was sent OK
* ERR_BUF if p doesn't have enough space for IP/LINK headers
* returns errors returned by netif->output
*
* @note ip_id: RFC791 "some host may be able to simply use
* unique identifiers independent of destination"
*/
err_t
ip_output_if(struct pbuf *p, struct ip_addr *src, struct ip_addr *dest,
u8_t ttl, u8_t tos,
u8_t proto, struct netif *netif)
{
#if IP_OPTIONS_SEND
return ip_output_if_opt(p, src, dest, ttl, tos, proto, netif, NULL, 0);
}
/**
* Same as ip_output_if() but with the possibility to include IP options:
*
* @ param ip_options pointer to the IP options, copied into the IP header
* @ param optlen length of ip_options
*/
err_t ip_output_if_opt(struct pbuf *p, struct ip_addr *src, struct ip_addr *dest,
u8_t ttl, u8_t tos, u8_t proto, struct netif *netif, void *ip_options,
u16_t optlen)
{
#endif /* IP_OPTIONS_SEND */
struct ip_hdr *iphdr;
static u16_t ip_id = 0;
snmp_inc_ipoutrequests();
/* Should the IP header be generated or is it already included in p? */
if (dest != IP_HDRINCL) {
u16_t ip_hlen = IP_HLEN;
#if IP_OPTIONS_SEND
u16_t optlen_aligned = 0;
if (optlen != 0) {
/* round up to a multiple of 4 */
optlen_aligned = ((optlen + 3) & ~3);
ip_hlen += optlen_aligned;
/* First write in the IP options */
if (pbuf_header(p, optlen_aligned)) {
LWIP_DEBUGF(IP_DEBUG | LWIP_DBG_LEVEL_SERIOUS, ("ip_output_if_opt: not enough room for IP options in pbuf\n"));
IP_STATS_INC(ip.err);
snmp_inc_ipoutdiscards();
return ERR_BUF;
}
MEMCPY(p->payload, ip_options, optlen);
if (optlen < optlen_aligned) {
/* zero the remaining bytes */
memset(((char*)p->payload) + optlen, 0, optlen_aligned - optlen);
}
}
#endif /* IP_OPTIONS_SEND */
/* generate IP header */
if (pbuf_header(p, IP_HLEN)) {
LWIP_DEBUGF(IP_DEBUG | LWIP_DBG_LEVEL_SERIOUS, ("ip_output: not enough room for IP header in pbuf\n"));
IP_STATS_INC(ip.err);
snmp_inc_ipoutdiscards();
return ERR_BUF;
}
iphdr = p->payload;
LWIP_ASSERT("check that first pbuf can hold struct ip_hdr",
(p->len >= sizeof(struct ip_hdr)));
IPH_TTL_SET(iphdr, ttl);
IPH_PROTO_SET(iphdr, proto);
ip_addr_set(&(iphdr->dest), dest);
IPH_VHLTOS_SET(iphdr, 4, ip_hlen / 4, tos);
IPH_LEN_SET(iphdr, htons(p->tot_len));
IPH_OFFSET_SET(iphdr, 0);
IPH_ID_SET(iphdr, htons(ip_id));
++ip_id;
if (ip_addr_isany(src)) {
ip_addr_set(&(iphdr->src), &(netif->ip_addr));
} else {
ip_addr_set(&(iphdr->src), src);
}
IPH_CHKSUM_SET(iphdr, 0);
#if CHECKSUM_GEN_IP
IPH_CHKSUM_SET(iphdr, inet_chksum(iphdr, ip_hlen));
#endif
} else {
/* IP header already included in p */
iphdr = p->payload;
dest = &(iphdr->dest);
}
IP_STATS_INC(ip.xmit);
LWIP_DEBUGF(IP_DEBUG, ("ip_output_if: %c%c%"U16_F"\n", netif->name[0], netif->name[1], netif->num));
ip_debug_print(p);
#if ENABLE_LOOPBACK
if (ip_addr_cmp(dest, &netif->ip_addr)) {
/* Packet to self, enqueue it for loopback */
LWIP_DEBUGF(IP_DEBUG, ("netif_loop_output()"));
return netif_loop_output(netif, p, dest);
}
#endif /* ENABLE_LOOPBACK */
#if IP_FRAG
/* don't fragment if interface has mtu set to 0 [loopif] */
if (netif->mtu && (p->tot_len > netif->mtu)) {
return ip_frag(p,netif,dest);
}
#endif
LWIP_DEBUGF(IP_DEBUG, ("netif->output()"));
return netif->output(netif, p, dest);
}
/**
* Simple interface to ip_output_if. It finds the outgoing network
* interface and calls upon ip_output_if to do the actual work.
*
* @param p the packet to send (p->payload points to the data, e.g. next
protocol header; if dest == IP_HDRINCL, p already includes an IP
header and p->payload points to that IP header)
* @param src the source IP address to send from (if src == IP_ADDR_ANY, the
* IP address of the netif used to send is used as source address)
* @param dest the destination IP address to send the packet to
* @param ttl the TTL value to be set in the IP header
* @param tos the TOS value to be set in the IP header
* @param proto the PROTOCOL to be set in the IP header
*
* @return ERR_RTE if no route is found
* see ip_output_if() for more return values
*/
err_t
ip_output(struct pbuf *p, struct ip_addr *src, struct ip_addr *dest,
u8_t ttl, u8_t tos, u8_t proto)
{
struct netif *netif;
if ((netif = ip_route(dest)) == NULL) {
LWIP_DEBUGF(IP_DEBUG, ("ip_output: No route to 0x%"X32_F"\n", dest->addr));
IP_STATS_INC(ip.rterr);
return ERR_RTE;
}
return ip_output_if(p, src, dest, ttl, tos, proto, netif);
}
#if LWIP_NETIF_HWADDRHINT
/** Like ip_output, but takes and addr_hint pointer that is passed on to netif->addr_hint
* before calling ip_output_if.
*
* @param p the packet to send (p->payload points to the data, e.g. next
protocol header; if dest == IP_HDRINCL, p already includes an IP
header and p->payload points to that IP header)
* @param src the source IP address to send from (if src == IP_ADDR_ANY, the
* IP address of the netif used to send is used as source address)
* @param dest the destination IP address to send the packet to
* @param ttl the TTL value to be set in the IP header
* @param tos the TOS value to be set in the IP header
* @param proto the PROTOCOL to be set in the IP header
* @param addr_hint address hint pointer set to netif->addr_hint before
* calling ip_output_if()
*
* @return ERR_RTE if no route is found
* see ip_output_if() for more return values
*/
err_t
ip_output_hinted(struct pbuf *p, struct ip_addr *src, struct ip_addr *dest,
u8_t ttl, u8_t tos, u8_t proto, u8_t *addr_hint)
{
struct netif *netif;
err_t err;
if ((netif = ip_route(dest)) == NULL) {
LWIP_DEBUGF(IP_DEBUG, ("ip_output: No route to 0x%"X32_F"\n", dest->addr));
IP_STATS_INC(ip.rterr);
return ERR_RTE;
}
netif->addr_hint = addr_hint;
err = ip_output_if(p, src, dest, ttl, tos, proto, netif);
netif->addr_hint = NULL;
return err;
}
#endif /* LWIP_NETIF_HWADDRHINT*/
#if IP_DEBUG
/* Print an IP header by using LWIP_DEBUGF
* @param p an IP packet, p->payload pointing to the IP header
*/
void
ip_debug_print(struct pbuf *p)
{
struct ip_hdr *iphdr = p->payload;
u8_t *payload;
payload = (u8_t *)iphdr + IP_HLEN;
LWIP_DEBUGF(IP_DEBUG, ("IP header:\n"));
LWIP_DEBUGF(IP_DEBUG, ("+-------------------------------+\n"));
LWIP_DEBUGF(IP_DEBUG, ("|%2"S16_F" |%2"S16_F" | 0x%02"X16_F" | %5"U16_F" | (v, hl, tos, len)\n",
IPH_V(iphdr),
IPH_HL(iphdr),
IPH_TOS(iphdr),
ntohs(IPH_LEN(iphdr))));
LWIP_DEBUGF(IP_DEBUG, ("+-------------------------------+\n"));
LWIP_DEBUGF(IP_DEBUG, ("| %5"U16_F" |%"U16_F"%"U16_F"%"U16_F"| %4"U16_F" | (id, flags, offset)\n",
ntohs(IPH_ID(iphdr)),
ntohs(IPH_OFFSET(iphdr)) >> 15 & 1,
ntohs(IPH_OFFSET(iphdr)) >> 14 & 1,
ntohs(IPH_OFFSET(iphdr)) >> 13 & 1,
ntohs(IPH_OFFSET(iphdr)) & IP_OFFMASK));
LWIP_DEBUGF(IP_DEBUG, ("+-------------------------------+\n"));
LWIP_DEBUGF(IP_DEBUG, ("| %3"U16_F" | %3"U16_F" | 0x%04"X16_F" | (ttl, proto, chksum)\n",
IPH_TTL(iphdr),
IPH_PROTO(iphdr),
ntohs(IPH_CHKSUM(iphdr))));
LWIP_DEBUGF(IP_DEBUG, ("+-------------------------------+\n"));
LWIP_DEBUGF(IP_DEBUG, ("| %3"U16_F" | %3"U16_F" | %3"U16_F" | %3"U16_F" | (src)\n",
ip4_addr1(&iphdr->src),
ip4_addr2(&iphdr->src),
ip4_addr3(&iphdr->src),
ip4_addr4(&iphdr->src)));
LWIP_DEBUGF(IP_DEBUG, ("+-------------------------------+\n"));
LWIP_DEBUGF(IP_DEBUG, ("| %3"U16_F" | %3"U16_F" | %3"U16_F" | %3"U16_F" | (dest)\n",
ip4_addr1(&iphdr->dest),
ip4_addr2(&iphdr->dest),
ip4_addr3(&iphdr->dest),
ip4_addr4(&iphdr->dest)));
LWIP_DEBUGF(IP_DEBUG, ("+-------------------------------+\n"));
}
int net_send(spdid_t spdid, net_connection_t nc, void *data, int sz)
{
struct intern_connection *ic;
u16_t tid = cos_get_thd_id();
int ret = sz;
// if (!cos_argreg_buff_intern(data, sz)) return -EFAULT;
if (!net_conn_valid(nc)) return -EINVAL;
if (sz > MAX_SEND) return -EMSGSIZE;
// NET_LOCK_TAKE();
ic = net_conn_get_internal(nc);
if (NULL == ic) {
ret = -EINVAL;
goto err;
}
if (tid != ic->tid) {
ret = -EPERM;
goto err;
}
switch (ic->conn_type) {
case UDP:
{
struct udp_pcb *up;
struct pbuf *p;
/* There's no blocking in the UDP case, so this is simple */
up = ic->conn.up;
p = pbuf_alloc(PBUF_TRANSPORT, sz, PBUF_ROM);
if (NULL == p) {
ret = -ENOMEM;
goto err;
}
p->payload = data;
if (ERR_OK != udp_send(up, p)) {
pbuf_free(p);
/* IP/port must not be set */
ret = -ENOTCONN;
goto err;
}
pbuf_free(p);
break;
}
case TCP:
{
struct tcp_pcb *tp;
#define TCP_SEND_COPY
#ifdef TCP_SEND_COPY
void *d;
struct packet_queue *pq;
#endif
tp = ic->conn.tp;
if (tcp_sndbuf(tp) < sz) {
ret = 0;
break;
}
#ifdef TCP_SEND_COPY
pq = malloc(sizeof(struct packet_queue) + sz);
if (unlikely(NULL == pq)) {
ret = -ENOMEM;
goto err;
}
#ifdef TEST_TIMING
pq->ts_start = timing_record(APP_PROC, ic->ts_start);
#endif
pq->headers = NULL;
d = net_packet_data(pq);
memcpy(d, data, sz);
if (ERR_OK != (ret = tcp_write(tp, d, sz, 0))) {
#else
if (ERR_OK != (ret = tcp_write(tp, data, sz, TCP_WRITE_FLAG_COPY))) {
#endif
free(pq);
printc("tcp_write returned %d (sz %d, tcp_sndbuf %d, ERR_MEM: %d)",
ret, sz, tcp_sndbuf(tp), ERR_MEM);
BUG();
}
/* No implementation of nagle's algorithm yet. Send
* out the packet immediately if possible. */
if (ERR_OK != (ret = tcp_output(tp))) {
printc("tcp_output returned %d, ERR_MEM: %d", ret, ERR_MEM);
BUG();
}
ret = sz;
break;
}
case TCP_CLOSED:
ret = -EPIPE;
break;
default:
BUG();
}
err:
// NET_LOCK_RELEASE();
return ret;
}
/************************ LWIP integration: **************************/
struct ip_addr ip, mask, gw;
struct netif cos_if;
static void cos_net_interrupt(char *packet, int sz)
{
void *d;
int len;
struct pbuf *p;
struct ip_hdr *ih;
struct packet_queue *pq;
#ifdef TEST_TIMING
unsigned long long ts;
#endif
// printc(">>> %d\n", net_lock.lock_id);
NET_LOCK_TAKE();
// printc("<<< %d\n", net_lock.lock_id);
assert(packet);
ih = (struct ip_hdr*)packet;
if (unlikely(4 != IPH_V(ih))) goto done;
len = ntohs(IPH_LEN(ih));
if (unlikely(len != sz || len > MTU)) {
printc("len %d != %d or > %d", len, sz, MTU);
goto done;
}
p = pbuf_alloc(PBUF_IP, len, PBUF_ROM);
if (unlikely(!p)) {
prints("OOM in interrupt: allocation of pbuf failed.\n");
goto done;
}
/* For now, we're going to do an additional copy. Currently,
* packets should be small, so this shouldn't hurt that badly.
* This is done because 1) we are freeing the packet
* elsewhere, 2) we want to malloc some (small) packets to
* save space and free up the ring buffers, 3) it is difficult
* to know in (1) which deallocation method (free or return to
* ring buff) to use */
pq = malloc(len + sizeof(struct packet_queue));
if (unlikely(NULL == pq)) {
printc("OOM in interrupt: allocation of packet data (%d bytes) failed.\n", len);
pbuf_free(p);
goto done;
}
pq->headers = d = net_packet_data(pq);
#ifdef TEST_TIMING
#ifdef TCP_SEND_COPY
ts = pq->ts_start = timing_timestamp();
#endif
#endif
memcpy(d, packet, len);
p->payload = p->alloc_track = d;
/* hand off packet ownership here... */
if (ERR_OK != cos_if.input(p, &cos_if)) {
prints("net: failure in IP input.");
pbuf_free(p);
goto done;
}
#ifdef TEST_TIMING
timing_record(UPCALL_PROC, ts);
#endif
done:
NET_LOCK_RELEASE();
return;
}
/**
* Reassembles incoming IP fragments into an IP datagram.
*
* @param p points to a pbuf chain of the fragment
* @return NULL if reassembly is incomplete, ? otherwise
*/
struct pbuf *
ip_reass(struct pbuf *p)
{
struct pbuf *q;
struct ip_hdr *fraghdr, *iphdr;
u16_t offset, len;
u16_t i;
IPFRAG_STATS_INC(ip_frag.recv);
iphdr = (struct ip_hdr *) ip_reassbuf;
fraghdr = (struct ip_hdr *) p->payload;
/* If ip_reasstmr is zero, no packet is present in the buffer, so we
write the IP header of the fragment into the reassembly
buffer. The timer is updated with the maximum age. */
if (ip_reasstmr == 0) {
LWIP_DEBUGF(IP_REASS_DEBUG, ("ip_reass: new packet\n"));
memcpy(iphdr, fraghdr, IP_HLEN);
ip_reasstmr = IP_REASS_MAXAGE;
ip_reassflags = 0;
/* Clear the bitmap. */
memset(ip_reassbitmap, 0, sizeof(ip_reassbitmap));
}
/* Check if the incoming fragment matches the one currently present
in the reasembly buffer. If so, we proceed with copying the
fragment into the buffer. */
if (ip_addr_cmp(&iphdr->src, &fraghdr->src) &&
ip_addr_cmp(&iphdr->dest, &fraghdr->dest) &&
IPH_ID(iphdr) == IPH_ID(fraghdr)) {
LWIP_DEBUGF(IP_REASS_DEBUG, ("ip_reass: matching previous fragment ID=%"X16_F"\n",
ntohs(IPH_ID(fraghdr))));
IPFRAG_STATS_INC(ip_frag.cachehit);
/* Find out the offset in the reassembly buffer where we should
copy the fragment. */
len = ntohs(IPH_LEN(fraghdr)) - IPH_HL(fraghdr) * 4;
offset = (ntohs(IPH_OFFSET(fraghdr)) & IP_OFFMASK) * 8;
/* If the offset or the offset + fragment length overflows the
reassembly buffer, we discard the entire packet. */
if (offset > IP_REASS_BUFSIZE || offset + len > IP_REASS_BUFSIZE) {
LWIP_DEBUGF(IP_REASS_DEBUG,
("ip_reass: fragment outside of buffer (%"S16_F":%"S16_F"/%"S16_F").\n", offset,
offset + len, IP_REASS_BUFSIZE));
ip_reasstmr = 0;
goto nullreturn;
}
/* Copy the fragment into the reassembly buffer, at the right
offset. */
LWIP_DEBUGF(IP_REASS_DEBUG,
("ip_reass: copying with offset %"S16_F" into %"S16_F":%"S16_F"\n", offset,
IP_HLEN + offset, IP_HLEN + offset + len));
i = IPH_HL(fraghdr) * 4;
copy_from_pbuf(p, &i, &ip_reassbuf[IP_HLEN + offset], len);
/* Update the bitmap. */
if (offset / (8 * 8) == (offset + len) / (8 * 8)) {
LWIP_DEBUGF(IP_REASS_DEBUG,
("ip_reass: updating single byte in bitmap.\n"));
/* If the two endpoints are in the same byte, we only update that byte. */
LWIP_ASSERT("offset / (8 * 8) < sizeof(ip_reassbitmap)",
offset / (8 * 8) < sizeof(ip_reassbitmap));
ip_reassbitmap[offset / (8 * 8)] |=
bitmap_bits[(offset / 8) & 7] &
~bitmap_bits[((offset + len) / 8) & 7];
} else {
/* If the two endpoints are in different bytes, we update the
bytes in the endpoints and fill the stuff inbetween with
0xff. */
LWIP_ASSERT("offset / (8 * 8) < sizeof(ip_reassbitmap)",
offset / (8 * 8) < sizeof(ip_reassbitmap));
ip_reassbitmap[offset / (8 * 8)] |= bitmap_bits[(offset / 8) & 7];
LWIP_DEBUGF(IP_REASS_DEBUG,
("ip_reass: updating many bytes in bitmap (%"S16_F":%"S16_F").\n",
1 + offset / (8 * 8), (offset + len) / (8 * 8)));
for (i = 1 + offset / (8 * 8); i < (offset + len) / (8 * 8); ++i) {
ip_reassbitmap[i] = 0xff;
}
LWIP_ASSERT("(offset + len) / (8 * 8) < sizeof(ip_reassbitmap)",
(offset + len) / (8 * 8) < sizeof(ip_reassbitmap));
ip_reassbitmap[(offset + len) / (8 * 8)] |=
~bitmap_bits[((offset + len) / 8) & 7];
}
/* If this fragment has the More Fragments flag set to zero, we
know that this is the last fragment, so we can calculate the
size of the entire packet. We also set the
IP_REASS_FLAG_LASTFRAG flag to indicate that we have received
the final fragment. */
if ((ntohs(IPH_OFFSET(fraghdr)) & IP_MF) == 0) {
ip_reassflags |= IP_REASS_FLAG_LASTFRAG;
ip_reasslen = offset + len;
LWIP_DEBUGF(IP_REASS_DEBUG,
("ip_reass: last fragment seen, total len %"S16_F"\n",
ip_reasslen));
}
/* Finally, we check if we have a full packet in the buffer. We do
this by checking if we have the last fragment and if all bits
in the bitmap are set. */
if (ip_reassflags & IP_REASS_FLAG_LASTFRAG) {
/* Check all bytes up to and including all but the last byte in
the bitmap. */
LWIP_ASSERT("ip_reasslen / (8 * 8) - 1 < sizeof(ip_reassbitmap)",
ip_reasslen / (8 * 8) - 1 < sizeof(ip_reassbitmap));
for (i = 0; i < ip_reasslen / (8 * 8) - 1; ++i) {
if (ip_reassbitmap[i] != 0xff) {
LWIP_DEBUGF(IP_REASS_DEBUG,
("ip_reass: last fragment seen, bitmap %"S16_F"/%"S16_F" failed (%"X16_F")\n",
i, ip_reasslen / (8 * 8) - 1, ip_reassbitmap[i]));
goto nullreturn;
}
}
/* Check the last byte in the bitmap. It should contain just the
right amount of bits. */
LWIP_ASSERT("ip_reasslen / (8 * 8) < sizeof(ip_reassbitmap)",
ip_reasslen / (8 * 8) < sizeof(ip_reassbitmap));
if (ip_reassbitmap[ip_reasslen / (8 * 8)] !=
(u8_t) ~ bitmap_bits[ip_reasslen / 8 & 7]) {
LWIP_DEBUGF(IP_REASS_DEBUG,
("ip_reass: last fragment seen, bitmap %"S16_F" didn't contain %"X16_F" (%"X16_F")\n",
ip_reasslen / (8 * 8), ~bitmap_bits[ip_reasslen / 8 & 7],
ip_reassbitmap[ip_reasslen / (8 * 8)]));
goto nullreturn;
}
/* Pretend to be a "normal" (i.e., not fragmented) IP packet
from now on. */
ip_reasslen += IP_HLEN;
IPH_LEN_SET(iphdr, htons(ip_reasslen));
IPH_OFFSET_SET(iphdr, 0);
IPH_CHKSUM_SET(iphdr, 0);
IPH_CHKSUM_SET(iphdr, inet_chksum(iphdr, IP_HLEN));
/* If we have come this far, we have a full packet in the
buffer, so we allocate a pbuf and copy the packet into it. We
also reset the timer. */
ip_reasstmr = 0;
pbuf_free(p);
p = pbuf_alloc(PBUF_LINK, ip_reasslen, PBUF_POOL);
if (p != NULL) {
i = 0;
for (q = p; q != NULL; q = q->next) {
/* Copy enough bytes to fill this pbuf in the chain. The
available data in the pbuf is given by the q->len variable. */
LWIP_DEBUGF(IP_REASS_DEBUG,
("ip_reass: memcpy from %p (%"S16_F") to %p, %"S16_F" bytes\n",
(void *)&ip_reassbuf[i], i, q->payload,
q->len > ip_reasslen - i ? ip_reasslen - i : q->len));
memcpy(q->payload, &ip_reassbuf[i],
q->len > ip_reasslen - i ? ip_reasslen - i : q->len);
i += q->len;
}
IPFRAG_STATS_INC(ip_frag.fw);
} else {
IPFRAG_STATS_INC(ip_frag.memerr);
}
LWIP_DEBUGF(IP_REASS_DEBUG, ("ip_reass: p %p\n", (void*)p));
return p;
}
}
nullreturn:
IPFRAG_STATS_INC(ip_frag.drop);
pbuf_free(p);
return NULL;
}
err_t
ip_input(struct pbuf *p, struct netif *inp) {
struct ip_hdr *iphdr;
struct netif *netif;
u16_t iphdrlen;
IP_STATS_INC(ip.recv);
snmp_inc_ipinreceives();
/* identify the IP header */
iphdr = p->payload;
if (IPH_V(iphdr) != 4) {
LWIP_DEBUGF(IP_DEBUG | 1, ("IP packet dropped due to bad version number %"U16_F"\n", IPH_V(iphdr)));
ip_debug_print(p);
pbuf_free(p);
IP_STATS_INC(ip.err);
IP_STATS_INC(ip.drop);
snmp_inc_ipinhdrerrors();
return ERR_OK;
}
/* obtain IP header length in number of 32-bit words */
iphdrlen = IPH_HL(iphdr);
/* calculate IP header length in bytes */
iphdrlen *= 4;
/* header length exceeds first pbuf length? */
if (iphdrlen > p->len) {
LWIP_DEBUGF(IP_DEBUG | 2, ("IP header (len %"U16_F") does not fit in first pbuf (len %"U16_F"), IP packet droppped.\n",
iphdrlen, p->len));
/* free (drop) packet pbufs */
pbuf_free(p);
IP_STATS_INC(ip.lenerr);
IP_STATS_INC(ip.drop);
snmp_inc_ipindiscards();
return ERR_OK;
}
/* verify checksum */
#if CHECKSUM_CHECK_IP
if (inet_chksum(iphdr, iphdrlen) != 0) {
LWIP_DEBUGF(IP_DEBUG | 2, ("Checksum (0x%"X16_F") failed, IP packet dropped.\n", inet_chksum(iphdr, iphdrlen)));
ip_debug_print(p);
pbuf_free(p);
IP_STATS_INC(ip.chkerr);
IP_STATS_INC(ip.drop);
snmp_inc_ipinhdrerrors();
return ERR_OK;
}
#endif
/* Trim pbuf. This should have been done at the netif layer,
* but we'll do it anyway just to be sure that its done. */
pbuf_realloc(p, ntohs(IPH_LEN(iphdr)));
/* match packet against an interface, i.e. is this packet for us? */
for (netif = netif_list; netif != NULL; netif = netif->next) {
LWIP_DEBUGF(IP_DEBUG, ("ip_input: iphdr->dest 0x%"X32_F" netif->ip_addr 0x%"X32_F" (0x%"X32_F", 0x%"X32_F", 0x%"X32_F")\n",
iphdr->dest.addr, netif->ip_addr.addr,
iphdr->dest.addr & netif->netmask.addr,
netif->ip_addr.addr & netif->netmask.addr,
iphdr->dest.addr & ~(netif->netmask.addr)));
/* interface is up and configured? */
if ((netif_is_up(netif)) && (!ip_addr_isany(&(netif->ip_addr))))
{
/* unicast to this interface address? */
if (ip_addr_cmp(&(iphdr->dest), &(netif->ip_addr)) ||
/* or broadcast on this interface network address? */
ip_addr_isbroadcast(&(iphdr->dest), netif)) {
LWIP_DEBUGF(IP_DEBUG, ("ip_input: packet accepted on interface %c%c\n",
netif->name[0], netif->name[1]));
/* break out of for loop */
break;
}
}
}
#if LWIP_DHCP
/* Pass DHCP messages regardless of destination address. DHCP traffic is addressed
* using link layer addressing (such as Ethernet MAC) so we must not filter on IP.
* According to RFC 1542 section 3.1.1, referred by RFC 2131).
*/
if (netif == NULL) {
/* remote port is DHCP server? */
if (IPH_PROTO(iphdr) == IP_PROTO_UDP) {
LWIP_DEBUGF(IP_DEBUG | DBG_TRACE | 1, ("ip_input: UDP packet to DHCP client port %"U16_F"\n",
ntohs(((struct udp_hdr *)((u8_t *)iphdr + iphdrlen))->dest)));
if (ntohs(((struct udp_hdr *)((u8_t *)iphdr + iphdrlen))->dest) == DHCP_CLIENT_PORT) {
LWIP_DEBUGF(IP_DEBUG | DBG_TRACE | 1, ("ip_input: DHCP packet accepted.\n"));
netif = inp;
}
}
}
#endif /* LWIP_DHCP */
/* packet not for us? */
if (netif == NULL) {
/* packet not for us, route or discard */
LWIP_DEBUGF(IP_DEBUG | DBG_TRACE | 1, ("ip_input: packet not for us.\n"));
#if IP_FORWARD
/* non-broadcast packet? */
if (!ip_addr_isbroadcast(&(iphdr->dest), inp)) {
/* try to forward IP packet on (other) interfaces */
ip_forward(p, iphdr, inp);
}
else
#endif /* IP_FORWARD */
{
snmp_inc_ipinaddrerrors();
snmp_inc_ipindiscards();
}
pbuf_free(p);
return ERR_OK;
}
/* packet consists of multiple fragments? */
if ((IPH_OFFSET(iphdr) & htons(IP_OFFMASK | IP_MF)) != 0) {
#if IP_REASSEMBLY /* packet fragment reassembly code present? */
LWIP_DEBUGF(IP_DEBUG, ("IP packet is a fragment (id=0x%04"X16_F" tot_len=%"U16_F" len=%"U16_F" MF=%"U16_F" offset=%"U16_F"), calling ip_reass()\n",
ntohs(IPH_ID(iphdr)), p->tot_len, ntohs(IPH_LEN(iphdr)), !!(IPH_OFFSET(iphdr) & htons(IP_MF)), (ntohs(IPH_OFFSET(iphdr)) & IP_OFFMASK)*8));
/* reassemble the packet*/
p = ip_reass(p);
/* packet not fully reassembled yet? */
if (p == NULL) {
return ERR_OK;
}
iphdr = p->payload;
#else /* IP_REASSEMBLY == 0, no packet fragment reassembly code present */
pbuf_free(p);
LWIP_DEBUGF(IP_DEBUG | 2, ("IP packet dropped since it was fragmented (0x%"X16_F") (while IP_REASSEMBLY == 0).\n",
ntohs(IPH_OFFSET(iphdr))));
IP_STATS_INC(ip.opterr);
IP_STATS_INC(ip.drop);
/* unsupported protocol feature */
snmp_inc_ipinunknownprotos();
return ERR_OK;
#endif /* IP_REASSEMBLY */
}
#if IP_OPTIONS == 0 /* no support for IP options in the IP header? */
if (iphdrlen > IP_HLEN) {
LWIP_DEBUGF(IP_DEBUG | 2, ("IP packet dropped since there were IP options (while IP_OPTIONS == 0).\n"));
pbuf_free(p);
IP_STATS_INC(ip.opterr);
IP_STATS_INC(ip.drop);
/* unsupported protocol feature */
snmp_inc_ipinunknownprotos();
return ERR_OK;
}
#endif /* IP_OPTIONS == 0 */
/* send to upper layers */
LWIP_DEBUGF(IP_DEBUG, ("ip_input: \n"));
ip_debug_print(p);
LWIP_DEBUGF(IP_DEBUG, ("ip_input: p->len %"U16_F" p->tot_len %"U16_F"\n", p->len, p->tot_len));
#if LWIP_RAW
/* raw input did not eat the packet? */
if (raw_input(p, inp) == 0) {
#endif /* LWIP_RAW */
switch (IPH_PROTO(iphdr)) {
#if LWIP_UDP
case IP_PROTO_UDP:
case IP_PROTO_UDPLITE:
snmp_inc_ipindelivers();
udp_input(p, inp);
break;
#endif /* LWIP_UDP */
#if LWIP_TCP
case IP_PROTO_TCP:
snmp_inc_ipindelivers();
tcp_input(p, inp);
break;
#endif /* LWIP_TCP */
case IP_PROTO_ICMP:
snmp_inc_ipindelivers();
icmp_input(p, inp);
break;
default:
/* send ICMP destination protocol unreachable unless is was a broadcast */
if (!ip_addr_isbroadcast(&(iphdr->dest), inp) &&
!ip_addr_ismulticast(&(iphdr->dest))) {
p->payload = iphdr;
icmp_dest_unreach(p, ICMP_DUR_PROTO);
}
pbuf_free(p);
LWIP_DEBUGF(IP_DEBUG | 2, ("Unsupported transport protocol %"U16_F"\n", IPH_PROTO(iphdr)));
IP_STATS_INC(ip.proterr);
IP_STATS_INC(ip.drop);
snmp_inc_ipinunknownprotos();
}
#if LWIP_RAW
} /* LWIP_RAW */
#endif
return ERR_OK;
}
/**
* This function is called by the network interface device driver when
* an IP packet is received. The function does the basic checks of the
* IP header such as packet size being at least larger than the header
* size etc. If the packet was not destined for us, the packet is
* forwarded (using ip_forward). The IP checksum is always checked.
*
* Finally, the packet is sent to the upper layer protocol input function.
*
* @param p the received IP packet (p->payload points to IP header)
* @param inp the netif on which this packet was received
* @return ERR_OK if the packet was processed (could return ERR_* if it wasn't
* processed, but currently always returns ERR_OK)
*/
uint8 ip_input(PBUF *p, NETIF *inp)
{
IP_HDR *iphdr;
NETIF *netif;
uint16 iphdr_hlen;
uint16 iphdr_len;
/* identify the IP header */
iphdr = p->payload;
if (IPH_V(iphdr) != 4)
{
pbuf_free(p);//this packet is not ipv4
return ERR_OK;
}
/* obtain IP header length in number of 32-bit words */
iphdr_hlen = IPH_HL(iphdr);
/* calculate IP header length in bytes */
iphdr_hlen *= 4;
/* obtain ip pocket total length in bytes */
iphdr_len = ntohs(IPH_LEN(iphdr));
/* header length exceeds first pbuf length, or ip length exceeds total pbuf length? */
if ((iphdr_hlen > p->len) || (iphdr_len > p->len))
{
/* free (drop) packet pbufs */
pbuf_free(p);
return ERR_OK;
}
if (inet_chksum(iphdr, iphdr_hlen) != 0)
{
pbuf_free(p);//ip header checksum error!
return ERR_OK;
}
netif = inp;
/* interface is up and configured? */
if (ip_addr_isany(&(netif->ip_addr)))
{
netif = NULL;
}
else
{
/* unicast to this interface address? */
if (!ip_addr_cmp(&(iphdr->dest), &(netif->ip_addr)))
{
netif = NULL;
}
}
/* packet consists of multiple fragments? */
if ((IPH_OFFSET(iphdr) & htons(IP_OFFMASK | IP_MF)) != 0)
{
printf("received splited IP packet!! \n");
pbuf_free(p);
return ERR_OK;
}
if (iphdr_hlen > IP_HLEN)
{
pbuf_free(p);
return ERR_OK;
}
switch (IPH_PROTO(iphdr))
{
case IP_PROTO_TCP://(6)
//printf("get a tcp packet\n");
tcp_input(p, inp);
break;
case IP_PROTO_UDP://(17)
printf("UDP\n");
udp_input(p, inp);
break;
case IP_PROTO_ICMP://(1)
printf("gICMP\n");
icmp_input(p, inp);
break;
default:
/* send ICMP destination protocol unreachable unless is was a broadcast */
printf("gerr\n");
if (!ip_addr_isbroadcast(&(iphdr->dest), inp) && !ip_addr_ismulticast(&(iphdr->dest)))
{
p->payload = iphdr;
icmp_dest_unreach(p, ICMP_DUR_PROTO);
}
pbuf_free(p);
}
return ERR_OK;
}
/*-----------------------------------------------------------------------------------*/
err_t
ip_input(struct pbuf *p, struct netif *inp) {
static struct ip_hdr *iphdr;
static struct netif *netif;
static u8_t hl;
#ifdef IP_STATS
++stats.ip.recv;
#endif /* IP_STATS */
/* identify the IP header */
iphdr = p->payload;
if(IPH_V(iphdr) != 4) {
DEBUGF(IP_DEBUG, ("IP packet dropped due to bad version number %d\n", IPH_V(iphdr)));
#if IP_DEBUG
ip_debug_print(p);
#endif /* IP_DEBUG */
pbuf_free(p);
#ifdef IP_STATS
++stats.ip.err;
++stats.ip.drop;
#endif /* IP_STATS */
return ERR_OK;
}
hl = IPH_HL(iphdr);
if(hl * 4 > p->len) {
DEBUGF(IP_DEBUG, ("IP packet dropped due to too short packet %d\n", p->len));
pbuf_free(p);
#ifdef IP_STATS
++stats.ip.lenerr;
++stats.ip.drop;
#endif /* IP_STATS */
return ERR_OK;
}
/* verify checksum */
if(inet_chksum(iphdr, hl * 4) != 0) {
DEBUGF(IP_DEBUG, ("IP packet dropped due to failing checksum 0x%x\n", inet_chksum(iphdr, hl * 4)));
#if IP_DEBUG
ip_debug_print(p);
#endif /* IP_DEBUG */
pbuf_free(p);
#ifdef IP_STATS
++stats.ip.chkerr;
++stats.ip.drop;
#endif /* IP_STATS */
return ERR_OK;
}
/* Trim pbuf. This should have been done at the netif layer,
but we'll do it anyway just to be sure that its done. */
pbuf_realloc(p, ntohs(IPH_LEN(iphdr)));
/* is this packet for us? */
for(netif = netif_list; netif != NULL; netif = netif->next) {
DEBUGF(IP_DEBUG, ("ip_input: iphdr->dest 0x%lx netif->ip_addr 0x%lx (0x%lx, 0x%lx, 0x%lx)\n",
iphdr->dest.addr, netif->ip_addr.addr,
iphdr->dest.addr & netif->netmask.addr,
netif->ip_addr.addr & netif->netmask.addr,
iphdr->dest.addr & ~(netif->netmask.addr)));
if(ip_addr_isany(&(netif->ip_addr)) ||
ip_addr_cmp(&(iphdr->dest), &(netif->ip_addr)) ||
(ip_addr_isbroadcast(&(iphdr->dest), &(netif->netmask)) &&
ip_addr_maskcmp(&(iphdr->dest), &(netif->ip_addr), &(netif->netmask))) ||
ip_addr_cmp(&(iphdr->dest), IP_ADDR_BROADCAST)) {
break;
}
}
#if LWIP_DHCP
/* If a DHCP packet has arrived on the interface, we pass it up the
stack regardless of destination IP address. The reason is that
DHCP replies are sent to the IP adress that will be given to this
node (as recommended by RFC 1542 section 3.1.1, referred by RFC
2131). */
if(IPH_PROTO(iphdr) == IP_PROTO_UDP &&
((struct udp_hdr *)((u8_t *)iphdr + IPH_HL(iphdr) * 4/sizeof(u8_t)))->src ==
DHCP_SERVER_PORT) {
netif = inp;
}
#endif /* LWIP_DHCP */
if(netif == NULL) {
/* packet not for us, route or discard */
DEBUGF(IP_DEBUG, ("ip_input: packet not for us.\n"));
#if IP_FORWARD
if(!ip_addr_isbroadcast(&(iphdr->dest), &(inp->netmask))) {
ip_forward(p, iphdr, inp);
}
#endif /* IP_FORWARD */
pbuf_free(p);
return ERR_OK;
}
#if IP_REASSEMBLY
if((IPH_OFFSET(iphdr) & htons(IP_OFFMASK | IP_MF)) != 0) {
p = ip_reass(p);
if(p == NULL) {
return ERR_OK;
}
iphdr = p->payload;
}
#else /* IP_REASSEMBLY */
if((IPH_OFFSET(iphdr) & htons(IP_OFFMASK | IP_MF)) != 0) {
pbuf_free(p);
DEBUGF(IP_DEBUG, ("IP packet dropped since it was fragmented (0x%x).\n",
ntohs(IPH_OFFSET(iphdr))));
#ifdef IP_STATS
++stats.ip.opterr;
++stats.ip.drop;
#endif /* IP_STATS */
return ERR_OK;
}
#endif /* IP_REASSEMBLY */
#if IP_OPTIONS == 0
if(hl * 4 > IP_HLEN) {
DEBUGF(IP_DEBUG, ("IP packet dropped since there were IP options.\n"));
pbuf_free(p);
#ifdef IP_STATS
++stats.ip.opterr;
++stats.ip.drop;
#endif /* IP_STATS */
return ERR_OK;
}
#endif /* IP_OPTIONS == 0 */
/* send to upper layers */
#if IP_DEBUG
DEBUGF(IP_DEBUG, ("ip_input: \n"));
ip_debug_print(p);
DEBUGF(IP_DEBUG, ("ip_input: p->len %d p->tot_len %d\n", p->len, p->tot_len));
#endif /* IP_DEBUG */
switch(IPH_PROTO(iphdr)) {
#if LWIP_UDP > 0
case IP_PROTO_UDP:
udp_input(p, inp);
break;
#endif /* LWIP_UDP */
#if LWIP_TCP > 0
case IP_PROTO_TCP:
tcp_input(p, inp);
break;
#endif /* LWIP_TCP */
case IP_PROTO_ICMP:
icmp_input(p, inp);
break;
default:
/* send ICMP destination protocol unreachable unless is was a broadcast */
if(!ip_addr_isbroadcast(&(iphdr->dest), &(inp->netmask)) &&
!ip_addr_ismulticast(&(iphdr->dest))) {
p->payload = iphdr;
icmp_dest_unreach(p, ICMP_DUR_PROTO);
}
pbuf_free(p);
DEBUGF(IP_DEBUG, ("Unsupported transportation protocol %d\n", IPH_PROTO(iphdr)));
#ifdef IP_STATS
++stats.ip.proterr;
++stats.ip.drop;
#endif /* IP_STATS */
}
return ERR_OK;
}
void ICMP_send_error_internal
(
uint8_t type, /* [IN] the type to send */
uint8_t code, /* [IN] the code to send */
uint32_t param, /* [IN] a parameter */
IP_HEADER_PTR iph, /* [IN] the IP header */
RTCSPCB_PTR origpcb, /* [IN] pcb with bad packet */
uint32_t maxlen /* [IN] the max data len to send, 0 = default */
)
{ /* Body */
ICMP_CFG_STRUCT_PTR ICMP_cfg_ptr = RTCS_getcfg(ICMP);
RTCSPCB_PTR pcb;
ICMP_ERR_HEADER_PTR icmph;
_ip_address ipsrc = mqx_ntohl(iph->SOURCE);
_ip_address ipdst = mqx_ntohl(iph->DEST);
uint16_t iphdrlen = (mqx_ntohc(iph->VERSLEN) & 0x0F) << 2;
uint16_t ippktlen = mqx_ntohs(iph->LENGTH) - iphdrlen;
uint16_t checksum;
_ip_address icmpsrc = IP_is_local(NULL,ipdst) ? ipdst : INADDR_ANY;
uint32_t error;
unsigned char *buffer;
uint32_t temp;
#if RTCSCFG_ENABLE_NAT
TCP_HEADER_PTR tcp_hdr;
UDP_HEADER_PTR udp_hdr;
IP_HEADER_PTR ip_hdr;
uint32_t protocol;
uint16_t src_port, dest_port;
uint32_t (_CODE_PTR_ *nat_exec)(RTCSPCB_PTR *);
#endif
#if BSPCFG_ENET_HW_TX_PROTOCOL_CHECKSUM
_ip_address if_addr;
IP_IF_PTR if_ptr;
#endif
/*
** Only include up to a maximum of maxlen bytes of data from the
** original IP datagram
*/
if (!maxlen) {
maxlen = IP_DEFAULT_MTU - sizeof(IP_HEADER) - sizeof(ICMP_HEADER) - 4;
} /* Endif */
if (origpcb) {
temp = RTCSPCB_DATA(origpcb) - RTCSPCB_DATA_NETWORK(origpcb);
if (maxlen > origpcb->HEADER_FRAG_USED + temp) {
maxlen = origpcb->HEADER_FRAG_USED + temp;
} /* Endif */
} /* Endif */
if (ippktlen + iphdrlen > maxlen) {
ippktlen = maxlen - iphdrlen;
} /* Endif */
/* Don't send an error in response to an ICMP error */
if (mqx_ntohc(iph->PROTOCOL) == IPPROTO_ICMP) {
/* Make sure the packet has at least a 'TYPE' field */
if (ippktlen == 0) {
return;
} /* Endif */
icmph = (ICMP_ERR_HEADER_PTR)((unsigned char *)iph + iphdrlen);
if (!ICMPTYPE_ISQUERY(mqx_ntohc(icmph->HEAD.TYPE))) {
return;
} /* Endif */
} /* Endif */
IF_ICMP_STATS_ENABLED(ICMP_cfg_ptr->STATS.COMMON.ST_TX_TOTAL++);
/* Allocate a PCB */
pcb = RTCSPCB_alloc_send();
if (pcb == NULL) {
IF_ICMP_STATS_ENABLED(ICMP_cfg_ptr->STATS.COMMON.ST_TX_MISSED++);
return;
} /* Endif */
//RTCSLOG_PCB_ALLOC(pcb);
if (origpcb) {
/* Add a dependency and a pointer to the ICMP data */
RTCSPCB_depend(pcb, origpcb);
error = RTCSPCB_append_fragment(pcb, iphdrlen + ippktlen, (unsigned char *)iph);
} else {
/* Reserve space for the ICMP data */
buffer = RTCS_mem_alloc_system(iphdrlen + ippktlen);
if (!buffer) {
IF_ICMP_STATS_ENABLED(ICMP_cfg_ptr->STATS.COMMON.ST_TX_MISSED++);
IF_ICMP_STATS_ENABLED(RTCS_seterror(&ICMP_cfg_ptr->STATS.ERR_TX, RTCSERR_OUT_OF_MEMORY, (uint32_t)pcb));
RTCSLOG_PCB_FREE(pcb, RTCSERR_OUT_OF_MEMORY);
RTCSPCB_free(pcb);
return;
} /* Endif */
_mem_set_type(buffer, MEM_TYPE_ICMP_DATA);
_mem_copy(iph, buffer, iphdrlen + ippktlen);
error = RTCSPCB_append_fragment_autofree(pcb, iphdrlen + ippktlen, buffer);
if (error) {
_mem_free(buffer);
} /* Endif */
} /* Endif */
if (!error) {
error = RTCSPCB_insert_header(pcb, sizeof(ICMP_HEADER) + 4);
} /* Endif */
if (error) {
IF_ICMP_STATS_ENABLED(ICMP_cfg_ptr->STATS.COMMON.ST_TX_MISSED++);
IF_ICMP_STATS_ENABLED(RTCS_seterror(&ICMP_cfg_ptr->STATS.ERR_TX, error, (uint32_t)pcb));
RTCSLOG_PCB_FREE(pcb, error);
RTCSPCB_free(pcb);
return;
} /* Endif */
RTCSLOG_PCB_WRITE(pcb, RTCS_LOGCTRL_PROTO(IPPROTO_ICMP), 0);
/* Build the header */
icmph = (ICMP_ERR_HEADER_PTR)RTCSPCB_DATA(pcb);
mqx_htonc(icmph->HEAD.TYPE, type);
mqx_htonc(icmph->HEAD.CODE, code);
mqx_htons(icmph->HEAD.CHECKSUM, 0);
mqx_htonl(icmph->DATA, param);
#if BSPCFG_ENET_HW_TX_PROTOCOL_CHECKSUM
/* HW-offload.*/
if_addr = IP_route_find(ipsrc /* Destination*/, 1);
if_ptr = IP_find_if(if_addr);
if( (if_ptr
&& (if_ptr->FEATURES & IP_IF_FEATURE_HW_TX_PROTOCOL_CHECKSUM)
&& (IP_will_fragment(if_ptr, RTCSPCB_SIZE(pcb)) == FALSE))
#if RTCSCFG_LINKOPT_8023
&& (pcb->LINK_OPTIONS.TX.OPT_8023 == 0)
#endif
)
{
pcb->TYPE |= RTCSPCB_TYPE_HW_PROTOCOL_CHECKSUM;
}
else
#endif
{
checksum = IP_Sum_PCB (0, pcb);
checksum = IP_Sum_invert(checksum);
mqx_htons(icmph->HEAD.CHECKSUM, checksum);
pcb->TYPE &= ~RTCSPCB_TYPE_HW_PROTOCOL_CHECKSUM;
}
#if RTCSCFG_ENABLE_ICMP_STATS
/* Update the statistics */
switch (type) {
case ICMPTYPE_DESTUNREACH: ICMP_cfg_ptr->STATS.ST_TX_DESTUNREACH++; break;
case ICMPTYPE_TIMEEXCEED: ICMP_cfg_ptr->STATS.ST_TX_TIMEEXCEED++; break;
case ICMPTYPE_PARMPROB: ICMP_cfg_ptr->STATS.ST_TX_PARMPROB++; break;
case ICMPTYPE_SRCQUENCH: ICMP_cfg_ptr->STATS.ST_TX_SRCQUENCH++; break;
case ICMPTYPE_REDIRECT: ICMP_cfg_ptr->STATS.ST_TX_REDIRECT++; break;
case ICMPTYPE_ECHO_REQ: ICMP_cfg_ptr->STATS.ST_TX_ECHO_REQ++; break;
case ICMPTYPE_ECHO_REPLY: ICMP_cfg_ptr->STATS.ST_TX_ECHO_REPLY++; break;
case ICMPTYPE_TIME_REQ: ICMP_cfg_ptr->STATS.ST_TX_TIME_REQ++; break;
case ICMPTYPE_TIME_REPLY: ICMP_cfg_ptr->STATS.ST_TX_TIME_REPLY++; break;
case ICMPTYPE_INFO_REQ: ICMP_cfg_ptr->STATS.ST_TX_INFO_REQ++; break;
case ICMPTYPE_INFO_REPLY: ICMP_cfg_ptr->STATS.ST_TX_INFO_REPLY++; break;
default: ICMP_cfg_ptr->STATS.ST_TX_OTHER++; break;
} /* Endswitch */
#endif
#if RTCSCFG_ENABLE_NAT
/* Reverse NAT (if it is installed) on the origpcb,
otherwise the icmp error will not get sent to the
original src */
nat_exec = RTCS_getcfg(NAT);
if (origpcb && nat_exec && *nat_exec) {
// swap src and dst IPs and ports so NAT_apply
// will process the pcb
ip_hdr = (IP_HEADER_PTR)RTCSPCB_DATA(origpcb);
protocol = mqx_ntohc(ip_hdr->PROTOCOL);
// Swap ports if it is udp or tcp
if ((protocol == IPPROTO_TCP) || (protocol == IPPROTO_UDP)) {
switch(protocol)
{
case IPPROTO_TCP:
tcp_hdr = (TCP_HEADER_PTR)((unsigned char *)ip_hdr + IPH_LEN(ip_hdr));
dest_port = mqx_ntohs(tcp_hdr->dest_port);
src_port = mqx_ntohs(tcp_hdr->source_port);
mqx_htons(tcp_hdr->dest_port, src_port);
mqx_htons(tcp_hdr->source_port, dest_port);
break;
case IPPROTO_UDP:
udp_hdr = (UDP_HEADER_PTR)((unsigned char *)ip_hdr + IPH_LEN(ip_hdr));
dest_port = mqx_ntohs(udp_hdr->DEST_PORT);
src_port = mqx_ntohs(udp_hdr->SRC_PORT);
mqx_htons(udp_hdr->DEST_PORT, src_port);
mqx_htons(udp_hdr->SRC_PORT, dest_port);
break;
default:
// should not get here
break;
}
}
// swap IPs
ipsrc = mqx_ntohl(ip_hdr->SOURCE);
ipdst = mqx_ntohl(ip_hdr->DEST);
mqx_htonl(ip_hdr->SOURCE, ipdst);
mqx_htonl(ip_hdr->DEST,ipsrc);
// call NAT
error = (*nat_exec)(&origpcb);
if (!error) {
// swap IPs and ports back
ip_hdr = (IP_HEADER_PTR)RTCSPCB_DATA(origpcb);
protocol = mqx_ntohc(ip_hdr->PROTOCOL);
// swap ports if it is udp or tcp
if ((protocol == IPPROTO_TCP) || (protocol == IPPROTO_UDP)) {
switch(protocol)
{
case IPPROTO_TCP:
tcp_hdr = (TCP_HEADER_PTR)((unsigned char *)ip_hdr + IPH_LEN(ip_hdr));
dest_port = mqx_ntohs(tcp_hdr->dest_port);
src_port = mqx_ntohs(tcp_hdr->source_port);
mqx_htons(tcp_hdr->dest_port, src_port);
mqx_htons(tcp_hdr->source_port, dest_port);
break;
case IPPROTO_UDP:
udp_hdr = (UDP_HEADER_PTR)((unsigned char *)ip_hdr + IPH_LEN(ip_hdr));
dest_port = mqx_ntohs(udp_hdr->DEST_PORT);
src_port = mqx_ntohs(udp_hdr->SRC_PORT);
mqx_htons(udp_hdr->DEST_PORT, src_port);
mqx_htons(udp_hdr->SRC_PORT, dest_port);
break;
default:
// should not get here
break;
}
}
// swap IPs
ipsrc = mqx_ntohl(ip_hdr->SOURCE);
ipdst = mqx_ntohl(ip_hdr->DEST);
mqx_htonl(ip_hdr->SOURCE, ipdst);
mqx_htonl(ip_hdr->DEST,ipsrc);
// Recalculate the cksum
mqx_htons(icmph->HEAD.CHECKSUM, 0);
checksum = IP_Sum_PCB (0, pcb);
checksum = IP_Sum_invert(checksum);
mqx_htons(icmph->HEAD.CHECKSUM, checksum);
// recalculate icmpsrc, and use new ipsrc.
ipdst = mqx_ntohl(ip_hdr->DEST);
ipsrc = mqx_ntohl(ip_hdr->SOURCE);
icmpsrc = IP_is_local(NULL,ipdst) ? ipdst : INADDR_ANY;
}
}
#endif
/* Send it */
IP_send(pcb, IPPROTO_ICMP, icmpsrc, ipsrc, 0);
} /* Endbody */
/**
* Receive callback function for RAW netconns.
* Doesn't 'eat' the packet, only references it and sends it to
* conn->recvmbox
*
* @see raw.h (struct raw_pcb.recv) for parameters and return value
*/
static u8_t
recv_raw(void *arg, struct raw_pcb *pcb, struct pbuf *p,
struct ip_addr *addr)
{
struct pbuf *q;
struct netbuf *buf;
struct netconn *conn;
#if LWIP_SO_RCVBUF
int recv_avail;
#endif /* LWIP_SO_RCVBUF */
LWIP_UNUSED_ARG(addr);
conn = arg;
#if LWIP_SO_RCVBUF
SYS_ARCH_GET(conn->recv_avail, recv_avail);
if ((conn != NULL) && (conn->recvmbox != SYS_MBOX_NULL) &&
((recv_avail + (int)(p->tot_len)) <= conn->recv_bufsize)) {
#else /* LWIP_SO_RCVBUF */
if ((conn != NULL) && (conn->recvmbox != SYS_MBOX_NULL)) {
#endif /* LWIP_SO_RCVBUF */
/* copy the whole packet into new pbufs */
q = pbuf_alloc(PBUF_RAW, p->tot_len, PBUF_RAM);
if(q != NULL) {
if (pbuf_copy(q, p) != ERR_OK) {
pbuf_free(q);
q = NULL;
}
}
if(q != NULL) {
buf = memp_malloc(MEMP_NETBUF);
if (buf == NULL) {
pbuf_free(q);
return 0;
}
buf->p = q;
buf->ptr = q;
buf->addr = &(((struct ip_hdr*)(q->payload))->src);
buf->port = pcb->protocol;
if (sys_mbox_trypost(conn->recvmbox, buf) != ERR_OK) {
netbuf_delete(buf);
return 0;
} else {
SYS_ARCH_INC(conn->recv_avail, q->tot_len);
/* Register event with callback */
API_EVENT(conn, NETCONN_EVT_RCVPLUS, q->tot_len);
}
}
}
return 0; /* do not eat the packet */
}
#endif /* LWIP_RAW*/
#if LWIP_UDP
/**
* Receive callback function for UDP netconns.
* Posts the packet to conn->recvmbox or deletes it on memory error.
*
* @see udp.h (struct udp_pcb.recv) for parameters
*/
static void
recv_udp(void *arg, struct udp_pcb *pcb, struct pbuf *p,
struct ip_addr *addr, u16_t port)
{
struct netbuf *buf;
struct netconn *conn;
#if LWIP_SO_RCVBUF
int recv_avail;
#endif /* LWIP_SO_RCVBUF */
LWIP_UNUSED_ARG(pcb); /* only used for asserts... */
LWIP_ASSERT("recv_udp must have a pcb argument", pcb != NULL);
LWIP_ASSERT("recv_udp must have an argument", arg != NULL);
conn = arg;
LWIP_ASSERT("recv_udp: recv for wrong pcb!", conn->pcb.udp == pcb);
#if LWIP_SO_RCVBUF
SYS_ARCH_GET(conn->recv_avail, recv_avail);
if ((conn == NULL) || (conn->recvmbox == SYS_MBOX_NULL) ||
((recv_avail + (int)(p->tot_len)) > conn->recv_bufsize)) {
#else /* LWIP_SO_RCVBUF */
if ((conn == NULL) || (conn->recvmbox == SYS_MBOX_NULL)) {
#endif /* LWIP_SO_RCVBUF */
pbuf_free(p);
return;
}
buf = memp_malloc(MEMP_NETBUF);
if (buf == NULL) {
pbuf_free(p);
return;
} else {
buf->p = p;
buf->ptr = p;
buf->addr = addr;
buf->port = port;
#if LWIP_NETBUF_RECVINFO
{
const struct ip_hdr* iphdr = ip_current_header();
/* get the UDP header - always in the first pbuf, ensured by udp_input */
const struct udp_hdr* udphdr = (void*)(((char*)iphdr) + IPH_LEN(iphdr));
buf->toaddr = (struct ip_addr*)&iphdr->dest;
buf->toport = udphdr->dest;
}
#endif /* LWIP_NETBUF_RECVINFO */
}
if (sys_mbox_trypost(conn->recvmbox, buf) != ERR_OK) {
netbuf_delete(buf);
return;
} else {
SYS_ARCH_INC(conn->recv_avail, p->tot_len);
/* Register event with callback */
API_EVENT(conn, NETCONN_EVT_RCVPLUS, p->tot_len);
}
}
#endif /* LWIP_UDP */
#if LWIP_TCP
/**
* Receive callback function for TCP netconns.
* Posts the packet to conn->recvmbox, but doesn't delete it on errors.
*
* @see tcp.h (struct tcp_pcb.recv) for parameters and return value
*/
static err_t
recv_tcp(void *arg, struct tcp_pcb *pcb, struct pbuf *p, err_t err)
{
struct netconn *conn;
u16_t len;
LWIP_UNUSED_ARG(pcb);
LWIP_ASSERT("recv_tcp must have a pcb argument", pcb != NULL);
LWIP_ASSERT("recv_tcp must have an argument", arg != NULL);
conn = arg;
LWIP_ASSERT("recv_tcp: recv for wrong pcb!", conn->pcb.tcp == pcb);
if ((conn == NULL) || (conn->recvmbox == SYS_MBOX_NULL)) {
return ERR_VAL;
}
conn->err = err;
if (p != NULL) {
len = p->tot_len;
SYS_ARCH_INC(conn->recv_avail, len);
} else {
len = 0;
}
if (sys_mbox_trypost(conn->recvmbox, p) != ERR_OK) {
return ERR_MEM;
} else {
/* Register event with callback */
API_EVENT(conn, NETCONN_EVT_RCVPLUS, len);
}
return ERR_OK;
}
/**
* Poll callback function for TCP netconns.
* Wakes up an application thread that waits for a connection to close
* or data to be sent. The application thread then takes the
* appropriate action to go on.
*
* Signals the conn->sem.
* netconn_close waits for conn->sem if closing failed.
*
* @see tcp.h (struct tcp_pcb.poll) for parameters and return value
*/
static err_t
poll_tcp(void *arg, struct tcp_pcb *pcb)
{
struct netconn *conn = arg;
LWIP_UNUSED_ARG(pcb);
LWIP_ASSERT("conn != NULL", (conn != NULL));
if (conn->state == NETCONN_WRITE) {
do_writemore(conn);
} else if (conn->state == NETCONN_CLOSE) {
do_close_internal(conn);
}
return ERR_OK;
}
/**
* Sent callback function for TCP netconns.
* Signals the conn->sem and calls API_EVENT.
* netconn_write waits for conn->sem if send buffer is low.
*
* @see tcp.h (struct tcp_pcb.sent) for parameters and return value
*/
static err_t
sent_tcp(void *arg, struct tcp_pcb *pcb, u16_t len)
{
struct netconn *conn = arg;
LWIP_UNUSED_ARG(pcb);
LWIP_ASSERT("conn != NULL", (conn != NULL));
if (conn->state == NETCONN_WRITE) {
LWIP_ASSERT("conn->pcb.tcp != NULL", conn->pcb.tcp != NULL);
do_writemore(conn);
} else if (conn->state == NETCONN_CLOSE) {
do_close_internal(conn);
}
if (conn) {
if ((conn->pcb.tcp != NULL) && (tcp_sndbuf(conn->pcb.tcp) > TCP_SNDLOWAT)) {
API_EVENT(conn, NETCONN_EVT_SENDPLUS, len);
}
}
return ERR_OK;
}
/**
* Error callback function for TCP netconns.
* Signals conn->sem, posts to all conn mboxes and calls API_EVENT.
* The application thread has then to decide what to do.
*
* @see tcp.h (struct tcp_pcb.err) for parameters
*/
static void
err_tcp(void *arg, err_t err)
{
struct netconn *conn;
conn = arg;
LWIP_ASSERT("conn != NULL", (conn != NULL));
conn->pcb.tcp = NULL;
conn->err = err;
if (conn->recvmbox != SYS_MBOX_NULL) {
/* Register event with callback */
API_EVENT(conn, NETCONN_EVT_RCVPLUS, 0);
sys_mbox_post(conn->recvmbox, NULL);
}
if (conn->op_completed != SYS_SEM_NULL && conn->state == NETCONN_CONNECT) {
conn->state = NETCONN_NONE;
sys_sem_signal(conn->op_completed);
}
if (conn->acceptmbox != SYS_MBOX_NULL) {
/* Register event with callback */
API_EVENT(conn, NETCONN_EVT_RCVPLUS, 0);
sys_mbox_post(conn->acceptmbox, NULL);
}
if ((conn->state == NETCONN_WRITE) || (conn->state == NETCONN_CLOSE)) {
/* calling do_writemore/do_close_internal is not necessary
since the pcb has already been deleted! */
conn->state = NETCONN_NONE;
/* wake up the waiting task */
sys_sem_signal(conn->op_completed);
}
}
/**
* Setup a tcp_pcb with the correct callback function pointers
* and their arguments.
*
* @param conn the TCP netconn to setup
*/
static void
setup_tcp(struct netconn *conn)
{
struct tcp_pcb *pcb;
pcb = conn->pcb.tcp;
tcp_arg(pcb, conn);
tcp_recv(pcb, recv_tcp);
tcp_sent(pcb, sent_tcp);
tcp_poll(pcb, poll_tcp, 4);
tcp_err(pcb, err_tcp);
}
/**
* Accept callback function for TCP netconns.
* Allocates a new netconn and posts that to conn->acceptmbox.
*
* @see tcp.h (struct tcp_pcb_listen.accept) for parameters and return value
*/
static err_t
accept_function(void *arg, struct tcp_pcb *newpcb, err_t err)
{
struct netconn *newconn;
struct netconn *conn;
#if API_MSG_DEBUG
#if TCP_DEBUG
tcp_debug_print_state(newpcb->state);
#endif /* TCP_DEBUG */
#endif /* API_MSG_DEBUG */
conn = (struct netconn *)arg;
LWIP_ERROR("accept_function: invalid conn->acceptmbox",
conn->acceptmbox != SYS_MBOX_NULL, return ERR_VAL;);
/* We have to set the callback here even though
* the new socket is unknown. conn->socket is marked as -1. */
newconn = netconn_alloc(conn->type, conn->callback);
if (newconn == NULL) {
return ERR_MEM;
}
newconn->pcb.tcp = newpcb;
setup_tcp(newconn);
newconn->err = err;
if (sys_mbox_trypost(conn->acceptmbox, newconn) != ERR_OK) {
/* When returning != ERR_OK, the connection is aborted in tcp_process(),
so do nothing here! */
newconn->pcb.tcp = NULL;
netconn_free(newconn);
return ERR_MEM;
} else {
/* Register event with callback */
API_EVENT(conn, NETCONN_EVT_RCVPLUS, 0);
}
return ERR_OK;
}
#endif /* LWIP_TCP */
/**
* Create a new pcb of a specific type.
* Called from do_newconn().
*
* @param msg the api_msg_msg describing the connection type
* @return msg->conn->err, but the return value is currently ignored
*/
static err_t
pcb_new(struct api_msg_msg *msg)
{
msg->conn->err = ERR_OK;
LWIP_ASSERT("pcb_new: pcb already allocated", msg->conn->pcb.tcp == NULL);
/* Allocate a PCB for this connection */
switch(NETCONNTYPE_GROUP(msg->conn->type)) {
#if LWIP_RAW
case NETCONN_RAW:
msg->conn->pcb.raw = raw_new(msg->msg.n.proto);
if(msg->conn->pcb.raw == NULL) {
msg->conn->err = ERR_MEM;
break;
}
raw_recv(msg->conn->pcb.raw, recv_raw, msg->conn);
break;
#endif /* LWIP_RAW */
#if LWIP_UDP
case NETCONN_UDP:
msg->conn->pcb.udp = udp_new();
if(msg->conn->pcb.udp == NULL) {
msg->conn->err = ERR_MEM;
break;
}
#if LWIP_UDPLITE
if (msg->conn->type==NETCONN_UDPLITE) {
udp_setflags(msg->conn->pcb.udp, UDP_FLAGS_UDPLITE);
}
#endif /* LWIP_UDPLITE */
if (msg->conn->type==NETCONN_UDPNOCHKSUM) {
udp_setflags(msg->conn->pcb.udp, UDP_FLAGS_NOCHKSUM);
}
udp_recv(msg->conn->pcb.udp, recv_udp, msg->conn);
break;
#endif /* LWIP_UDP */
#if LWIP_TCP
case NETCONN_TCP:
msg->conn->pcb.tcp = tcp_new();
if(msg->conn->pcb.tcp == NULL) {
msg->conn->err = ERR_MEM;
break;
}
setup_tcp(msg->conn);
break;
#endif /* LWIP_TCP */
default:
/* Unsupported netconn type, e.g. protocol disabled */
msg->conn->err = ERR_VAL;
break;
}
return msg->conn->err;
}
