STATIC mp_obj_t socket_accept(const mp_obj_t arg0) {
socket_obj_t *self = MP_OBJ_TO_PTR(arg0);
struct sockaddr addr;
socklen_t addr_len = sizeof(addr);
int new_fd = -1;
for (int i=0; i<=self->retries; i++) {
MP_THREAD_GIL_EXIT();
new_fd = lwip_accept_r(self->fd, &addr, &addr_len);
MP_THREAD_GIL_ENTER();
if (new_fd >= 0) break;
if (errno != EAGAIN) exception_from_errno(errno);
check_for_exceptions();
}
if (new_fd < 0) mp_raise_OSError(MP_ETIMEDOUT);
// create new socket object
socket_obj_t *sock = m_new_obj_with_finaliser(socket_obj_t);
sock->base.type = self->base.type;
sock->fd = new_fd;
sock->domain = self->domain;
sock->type = self->type;
sock->proto = self->proto;
_socket_settimeout(sock, UINT64_MAX);
// make the return value
uint8_t *ip = (uint8_t*)&((struct sockaddr_in*)&addr)->sin_addr;
mp_uint_t port = lwip_ntohs(((struct sockaddr_in*)&addr)->sin_port);
mp_obj_tuple_t *client = mp_obj_new_tuple(2, NULL);
client->items[0] = sock;
client->items[1] = netutils_format_inet_addr(ip, port, NETUTILS_BIG);
return client;
}
int lwip_sock_connect(void *conn, unsigned long *addr, uint16_t port) {
int ret;
mutexLock(g_netBH_lock);
ret = netconn_connect(conn, addr, lwip_ntohs(port));
mutexUnLock(g_netBH_lock);
return ret;
}
STATIC mp_obj_t socket_recvfrom(mp_obj_t self_in, mp_obj_t len_in) {
struct sockaddr from;
socklen_t fromlen = sizeof(from);
mp_obj_t tuple[2];
tuple[0] = _socket_recvfrom(self_in, len_in, &from, &fromlen);
uint8_t *ip = (uint8_t*)&((struct sockaddr_in*)&from)->sin_addr;
mp_uint_t port = lwip_ntohs(((struct sockaddr_in*)&from)->sin_port);
tuple[1] = netutils_format_inet_addr(ip, port, NETUTILS_BIG);
return mp_obj_new_tuple(2, tuple);
}
void ss2zta(JNIEnv *env, struct sockaddr_storage *ss, jobject addr)
{
jclass c = env->GetObjectClass(addr);
if (!c) {
return;
}
if(ss->ss_family == AF_INET)
{
struct sockaddr_in *in4 = (struct sockaddr_in*)ss;
jfieldID fid = env->GetFieldID(c, "_port", "I");
env->SetIntField(addr, fid, lwip_ntohs(in4->sin_port));
fid = env->GetFieldID(c,"_family", "I");
env->SetIntField(addr, fid, (in4->sin_family));
fid = env->GetFieldID(c, "_ip4", "[B");
jobject ipData = env->GetObjectField (addr, fid);
jbyteArray * arr = reinterpret_cast<jbyteArray*>(&ipData);
char *data = (char*)env->GetByteArrayElements(*arr, NULL);
memcpy(data, &(in4->sin_addr.s_addr), 4);
env->ReleaseByteArrayElements(*arr, (jbyte*)data, 0);
return;
}
if(ss->ss_family == AF_INET6)
{
struct sockaddr_in6 *in6 = (struct sockaddr_in6*)ss;
jfieldID fid = env->GetFieldID(c, "_port", "I");
env->SetIntField(addr, fid, lwip_ntohs(in6->sin6_port));
fid = env->GetFieldID(c,"_family", "I");
env->SetIntField(addr, fid, (in6->sin6_family));
fid = env->GetFieldID(c, "_ip6", "[B");
jobject ipData = env->GetObjectField (addr, fid);
jbyteArray * arr = reinterpret_cast<jbyteArray*>(&ipData);
char *data = (char*)env->GetByteArrayElements(*arr, NULL);
memcpy(data, &(in6->sin6_addr.s6_addr), 16);
env->ReleaseByteArrayElements(*arr, (jbyte*)data, 0);
return;
}
}
coap_pdu_t *
coap_new_pdu(void) {
coap_pdu_t *pdu;
#ifndef WITH_CONTIKI
pdu = coap_pdu_init(0, 0, lwip_ntohs(COAP_INVALID_TID), COAP_MAX_PDU_SIZE);
#else /* WITH_CONTIKI */
pdu = coap_pdu_init(0, 0, uip_ntohs(COAP_INVALID_TID), COAP_MAX_PDU_SIZE);
#endif /* WITH_CONTIKI */
#ifndef NDEBUG
if (!pdu)
coap_log(LOG_CRIT, "coap_new_pdu: cannot allocate memory for new PDU\n");
#endif
return pdu;
}
/**
* RTP send packets
*/
static void
rtp_send_packets( int sock, struct sockaddr_in* to)
{
struct rtp_hdr* rtphdr;
u8_t* rtp_payload;
int rtp_payload_size;
size_t rtp_data_index;
/* prepare RTP packet */
rtphdr = (struct rtp_hdr*)rtp_send_packet;
rtphdr->version = RTP_VERSION;
rtphdr->payloadtype = 0;
rtphdr->ssrc = PP_HTONL(RTP_SSRC);
rtphdr->timestamp = lwip_htonl(lwip_ntohl(rtphdr->timestamp) + RTP_TIMESTAMP_INCREMENT);
/* send RTP stream packets */
rtp_data_index = 0;
do {
rtp_payload = rtp_send_packet+sizeof(struct rtp_hdr);
rtp_payload_size = LWIP_MIN(RTP_PAYLOAD_SIZE, (sizeof(rtp_data) - rtp_data_index));
MEMCPY(rtp_payload, rtp_data + rtp_data_index, rtp_payload_size);
/* set MARKER bit in RTP header on the last packet of an image */
rtphdr->payloadtype = RTP_PAYLOADTYPE | (((rtp_data_index + rtp_payload_size)
>= sizeof(rtp_data)) ? RTP_MARKER_MASK : 0);
/* send RTP stream packet */
if (sendto(sock, rtp_send_packet, sizeof(struct rtp_hdr) + rtp_payload_size,
0, (struct sockaddr *)to, sizeof(struct sockaddr)) >= 0) {
rtphdr->seqNum = lwip_htons(lwip_ntohs(rtphdr->seqNum) + 1);
rtp_data_index += rtp_payload_size;
} else {
LWIP_DEBUGF(RTP_DEBUG, ("rtp_sender: not sendto==%i\n", errno));
}
}while (rtp_data_index < sizeof(rtp_data));
}
/**
* RTP recv thread
*/
static void
rtp_recv_thread(void *arg)
{
int sock;
struct sockaddr_in local;
struct sockaddr_in from;
int fromlen;
struct ip_mreq ipmreq;
struct rtp_hdr* rtphdr;
u32_t rtp_stream_address;
int timeout;
size_t result;
int recvrtppackets = 0;
int lostrtppackets = 0;
u16_t lastrtpseq = 0;
LWIP_UNUSED_ARG(arg);
/* initialize RTP stream address */
rtp_stream_address = RTP_STREAM_ADDRESS;
/* if we got a valid RTP stream address... */
if (rtp_stream_address != 0) {
/* create new socket */
sock = socket(AF_INET, SOCK_DGRAM, 0);
if (sock >= 0) {
/* prepare local address */
memset(&local, 0, sizeof(local));
local.sin_family = AF_INET;
local.sin_port = PP_HTONS(RTP_STREAM_PORT);
local.sin_addr.s_addr = PP_HTONL(INADDR_ANY);
/* bind to local address */
if (bind(sock, (struct sockaddr *)&local, sizeof(local)) == 0) {
/* set recv timeout */
timeout = RTP_RECV_TIMEOUT;
setsockopt(sock, SOL_SOCKET, SO_RCVTIMEO, (char *)&timeout, sizeof(timeout));
/* prepare multicast "ip_mreq" struct */
ipmreq.imr_multiaddr.s_addr = rtp_stream_address;
ipmreq.imr_interface.s_addr = PP_HTONL(INADDR_ANY);
/* join multicast group */
if (setsockopt(sock, IPPROTO_IP, IP_ADD_MEMBERSHIP, &ipmreq, sizeof(ipmreq)) == 0) {
/* receive RTP packets */
while(1) {
fromlen = sizeof(from);
result = recvfrom(sock, rtp_recv_packet, sizeof(rtp_recv_packet), 0,
(struct sockaddr *)&from, (socklen_t *)&fromlen);
if (result >= sizeof(struct rtp_hdr)) {
rtphdr = (struct rtp_hdr *)rtp_recv_packet;
recvrtppackets++;
if ((lastrtpseq == 0) || ((lastrtpseq + 1) == lwip_ntohs(rtphdr->seqNum))) {
RTP_RECV_PROCESSING((rtp_recv_packet + sizeof(rtp_hdr)),(result-sizeof(rtp_hdr)));
} else {
lostrtppackets++;
}
lastrtpseq = lwip_ntohs(rtphdr->seqNum);
if ((recvrtppackets % RTP_RECV_STATS) == 0) {
LWIP_DEBUGF(RTP_DEBUG, ("rtp_recv_thread: recv %6i packet(s) / lost %4i packet(s) (%.4f%%)...\n", recvrtppackets, lostrtppackets, (lostrtppackets*100.0)/recvrtppackets));
}
} else {
LWIP_DEBUGF(RTP_DEBUG, ("rtp_recv_thread: recv timeout...\n"));
}
}
/* leave multicast group */
setsockopt(sock, IPPROTO_IP, IP_DROP_MEMBERSHIP, &ipmreq, sizeof(ipmreq));
}
}
/* close the socket */
closesocket(sock);
}
}
}
/**
* Process an incoming UDP datagram.
*
* Given an incoming UDP datagram (as a chain of pbufs) this function
* finds a corresponding UDP PCB and hands over the pbuf to the pcbs
* recv function. If no pcb is found or the datagram is incorrect, the
* pbuf is freed.
*
* @param p pbuf to be demultiplexed to a UDP PCB (p->payload pointing to the UDP header)
* @param inp network interface on which the datagram was received.
*
*/
void
udp_input(struct pbuf *p, struct netif *inp)
{
struct udp_hdr *udphdr;
struct udp_pcb *pcb, *prev;
struct udp_pcb *uncon_pcb;
u16_t src, dest;
u8_t broadcast;
u8_t for_us = 0;
LWIP_UNUSED_ARG(inp);
PERF_START;
UDP_STATS_INC(udp.recv);
/* Check minimum length (UDP header) */
if (p->len < UDP_HLEN) {
/* drop short packets */
LWIP_DEBUGF(UDP_DEBUG,
("udp_input: short UDP datagram (%"U16_F" bytes) discarded\n", p->tot_len));
UDP_STATS_INC(udp.lenerr);
UDP_STATS_INC(udp.drop);
MIB2_STATS_INC(mib2.udpinerrors);
pbuf_free(p);
goto end;
}
udphdr = (struct udp_hdr *)p->payload;
/* is broadcast packet ? */
broadcast = ip_addr_isbroadcast(ip_current_dest_addr(), ip_current_netif());
LWIP_DEBUGF(UDP_DEBUG, ("udp_input: received datagram of length %"U16_F"\n", p->tot_len));
/* convert src and dest ports to host byte order */
src = lwip_ntohs(udphdr->src);
dest = lwip_ntohs(udphdr->dest);
udp_debug_print(udphdr);
/* print the UDP source and destination */
LWIP_DEBUGF(UDP_DEBUG, ("udp ("));
ip_addr_debug_print(UDP_DEBUG, ip_current_dest_addr());
LWIP_DEBUGF(UDP_DEBUG, (", %"U16_F") <-- (", lwip_ntohs(udphdr->dest)));
ip_addr_debug_print(UDP_DEBUG, ip_current_src_addr());
LWIP_DEBUGF(UDP_DEBUG, (", %"U16_F")\n", lwip_ntohs(udphdr->src)));
pcb = NULL;
prev = NULL;
uncon_pcb = NULL;
/* Iterate through the UDP pcb list for a matching pcb.
* 'Perfect match' pcbs (connected to the remote port & ip address) are
* preferred. If no perfect match is found, the first unconnected pcb that
* matches the local port and ip address gets the datagram. */
for (pcb = udp_pcbs; pcb != NULL; pcb = pcb->next) {
/* print the PCB local and remote address */
LWIP_DEBUGF(UDP_DEBUG, ("pcb ("));
ip_addr_debug_print(UDP_DEBUG, &pcb->local_ip);
LWIP_DEBUGF(UDP_DEBUG, (", %"U16_F") <-- (", pcb->local_port));
ip_addr_debug_print(UDP_DEBUG, &pcb->remote_ip);
LWIP_DEBUGF(UDP_DEBUG, (", %"U16_F")\n", pcb->remote_port));
/* compare PCB local addr+port to UDP destination addr+port */
if ((pcb->local_port == dest) &&
(udp_input_local_match(pcb, inp, broadcast) != 0)) {
if (((pcb->flags & UDP_FLAGS_CONNECTED) == 0) &&
((uncon_pcb == NULL)
#if SO_REUSE
/* prefer specific IPs over cath-all */
|| !ip_addr_isany(&pcb->local_ip)
#endif /* SO_REUSE */
)) {
/* the first unconnected matching PCB */
uncon_pcb = pcb;
}
/* compare PCB remote addr+port to UDP source addr+port */
if ((pcb->remote_port == src) &&
(ip_addr_isany_val(pcb->remote_ip) ||
ip_addr_cmp(&pcb->remote_ip, ip_current_src_addr()))) {
/* the first fully matching PCB */
if (prev != NULL) {
/* move the pcb to the front of udp_pcbs so that is
found faster next time */
prev->next = pcb->next;
pcb->next = udp_pcbs;
udp_pcbs = pcb;
} else {
UDP_STATS_INC(udp.cachehit);
}
break;
}
}
prev = pcb;
}
/* no fully matching pcb found? then look for an unconnected pcb */
if (pcb == NULL) {
pcb = uncon_pcb;
}
/* Check checksum if this is a match or if it was directed at us. */
if (pcb != NULL) {
for_us = 1;
} else {
#if LWIP_IPV6
if (ip_current_is_v6()) {
for_us = netif_get_ip6_addr_match(inp, ip6_current_dest_addr()) >= 0;
}
#endif /* LWIP_IPV6 */
#if LWIP_IPV4
if (!ip_current_is_v6()) {
for_us = ip4_addr_cmp(netif_ip4_addr(inp), ip4_current_dest_addr());
}
#endif /* LWIP_IPV4 */
}
if (for_us) {
LWIP_DEBUGF(UDP_DEBUG | LWIP_DBG_TRACE, ("udp_input: calculating checksum\n"));
#if CHECKSUM_CHECK_UDP
IF__NETIF_CHECKSUM_ENABLED(inp, CHECKSUM_CHECK_UDP) {
#if LWIP_UDPLITE
if (ip_current_header_proto() == IP_PROTO_UDPLITE) {
/* Do the UDP Lite checksum */
u16_t chklen = lwip_ntohs(udphdr->len);
if (chklen < sizeof(struct udp_hdr)) {
if (chklen == 0) {
/* For UDP-Lite, checksum length of 0 means checksum
over the complete packet (See RFC 3828 chap. 3.1) */
chklen = p->tot_len;
} else {
/* At least the UDP-Lite header must be covered by the
checksum! (Again, see RFC 3828 chap. 3.1) */
goto chkerr;
}
}
if (ip_chksum_pseudo_partial(p, IP_PROTO_UDPLITE,
p->tot_len, chklen,
ip_current_src_addr(), ip_current_dest_addr()) != 0) {
goto chkerr;
}
} else
#endif /* LWIP_UDPLITE */
{
if (udphdr->chksum != 0) {
if (ip_chksum_pseudo(p, IP_PROTO_UDP, p->tot_len,
ip_current_src_addr(),
ip_current_dest_addr()) != 0) {
goto chkerr;
}
}
}
}
#endif /* CHECKSUM_CHECK_UDP */
if (pbuf_header(p, -UDP_HLEN)) {
/* Can we cope with this failing? Just assert for now */
LWIP_ASSERT("pbuf_header failed\n", 0);
UDP_STATS_INC(udp.drop);
MIB2_STATS_INC(mib2.udpinerrors);
pbuf_free(p);
goto end;
}
if (pcb != NULL) {
MIB2_STATS_INC(mib2.udpindatagrams);
#if SO_REUSE && SO_REUSE_RXTOALL
if (ip_get_option(pcb, SOF_REUSEADDR) &&
(broadcast || ip_addr_ismulticast(ip_current_dest_addr()))) {
/* pass broadcast- or multicast packets to all multicast pcbs
if SOF_REUSEADDR is set on the first match */
struct udp_pcb *mpcb;
u8_t p_header_changed = 0;
s16_t hdrs_len = (s16_t)(ip_current_header_tot_len() + UDP_HLEN);
for (mpcb = udp_pcbs; mpcb != NULL; mpcb = mpcb->next) {
if (mpcb != pcb) {
/* compare PCB local addr+port to UDP destination addr+port */
if ((mpcb->local_port == dest) &&
(udp_input_local_match(mpcb, inp, broadcast) != 0)) {
/* pass a copy of the packet to all local matches */
if (mpcb->recv != NULL) {
struct pbuf *q;
/* for that, move payload to IP header again */
if (p_header_changed == 0) {
pbuf_header_force(p, hdrs_len);
p_header_changed = 1;
}
q = pbuf_alloc(PBUF_RAW, p->tot_len, PBUF_RAM);
if (q != NULL) {
err_t err = pbuf_copy(q, p);
if (err == ERR_OK) {
/* move payload to UDP data */
pbuf_header(q, -hdrs_len);
mpcb->recv(mpcb->recv_arg, mpcb, q, ip_current_src_addr(), src);
}
}
}
}
}
}
if (p_header_changed) {
/* and move payload to UDP data again */
pbuf_header(p, -hdrs_len);
}
}
#endif /* SO_REUSE && SO_REUSE_RXTOALL */
/* callback */
if (pcb->recv != NULL) {
/* now the recv function is responsible for freeing p */
pcb->recv(pcb->recv_arg, pcb, p, ip_current_src_addr(), src);
} else {
/* no recv function registered? then we have to free the pbuf! */
pbuf_free(p);
goto end;
}
} else {
LWIP_DEBUGF(UDP_DEBUG | LWIP_DBG_TRACE, ("udp_input: not for us.\n"));
#if LWIP_ICMP || LWIP_ICMP6
/* No match was found, send ICMP destination port unreachable unless
destination address was broadcast/multicast. */
if (!broadcast && !ip_addr_ismulticast(ip_current_dest_addr())) {
/* move payload pointer back to ip header */
pbuf_header_force(p, ip_current_header_tot_len() + UDP_HLEN);
icmp_port_unreach(ip_current_is_v6(), p);
}
#endif /* LWIP_ICMP || LWIP_ICMP6 */
UDP_STATS_INC(udp.proterr);
UDP_STATS_INC(udp.drop);
MIB2_STATS_INC(mib2.udpnoports);
pbuf_free(p);
}
} else {
/**
* Reassembles incoming IPv6 fragments into an IPv6 datagram.
*
* @param p points to the IPv6 Fragment Header
* @return NULL if reassembly is incomplete, pbuf pointing to
* IPv6 Header if reassembly is complete
*/
struct pbuf *
ip6_reass(struct pbuf *p)
{
struct ip6_reassdata *ipr, *ipr_prev;
struct ip6_reass_helper *iprh, *iprh_tmp, *iprh_prev=NULL;
struct ip6_frag_hdr *frag_hdr;
u16_t offset, len;
u16_t clen;
u8_t valid = 1;
struct pbuf *q;
IP6_FRAG_STATS_INC(ip6_frag.recv);
if ((const void*)ip6_current_header() != ((u8_t*)p->payload) - IP6_HLEN) {
/* ip6_frag_hdr must be in the first pbuf, not chained */
IP6_FRAG_STATS_INC(ip6_frag.proterr);
IP6_FRAG_STATS_INC(ip6_frag.drop);
goto nullreturn;
}
frag_hdr = (struct ip6_frag_hdr *) p->payload;
clen = pbuf_clen(p);
offset = lwip_ntohs(frag_hdr->_fragment_offset);
/* Calculate fragment length from IPv6 payload length.
* Adjust for headers before Fragment Header.
* And finally adjust by Fragment Header length. */
len = lwip_ntohs(ip6_current_header()->_plen);
len -= (u16_t)(((u8_t*)p->payload - (const u8_t*)ip6_current_header()) - IP6_HLEN);
len -= IP6_FRAG_HLEN;
/* 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_prev = NULL; 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 ((frag_hdr->_identification == ipr->identification) &&
ip6_addr_cmp(ip6_current_src_addr(), &(IPV6_FRAG_HDRREF(ipr->iphdr)->src)) &&
ip6_addr_cmp(ip6_current_dest_addr(), &(IPV6_FRAG_HDRREF(ipr->iphdr)->dest))) {
IP6_FRAG_STATS_INC(ip6_frag.cachehit);
break;
}
ipr_prev = ipr;
}
if (ipr == NULL) {
/* Enqueue a new datagram into the datagram queue */
ipr = (struct ip6_reassdata *)memp_malloc(MEMP_IP6_REASSDATA);
if (ipr == NULL) {
#if IP_REASS_FREE_OLDEST
/* Make room and try again. */
ip6_reass_remove_oldest_datagram(ipr, clen);
ipr = (struct ip6_reassdata *)memp_malloc(MEMP_IP6_REASSDATA);
if (ipr != NULL) {
/* re-search ipr_prev since it might have been removed */
for (ipr_prev = reassdatagrams; ipr_prev != NULL; ipr_prev = ipr_prev->next) {
if (ipr_prev->next == ipr) {
break;
}
}
} else
#endif /* IP_REASS_FREE_OLDEST */
{
IP6_FRAG_STATS_INC(ip6_frag.memerr);
IP6_FRAG_STATS_INC(ip6_frag.drop);
goto nullreturn;
}
}
memset(ipr, 0, sizeof(struct ip6_reassdata));
ipr->timer = IP_REASS_MAXAGE;
/* enqueue the new structure to the front of the list */
ipr->next = reassdatagrams;
reassdatagrams = ipr;
/* Use the current IPv6 header for src/dest address reference.
* Eventually, we will replace it when we get the first fragment
* (it might be this one, in any case, it is done later). */
#if IPV6_FRAG_COPYHEADER
MEMCPY(&ipr->iphdr, ip6_current_header(), IP6_HLEN);
#else /* IPV6_FRAG_COPYHEADER */
/* need to use the none-const pointer here: */
ipr->iphdr = ip_data.current_ip6_header;
#endif /* IPV6_FRAG_COPYHEADER */
/* copy the fragmented packet id. */
ipr->identification = frag_hdr->_identification;
/* copy the nexth field */
ipr->nexth = frag_hdr->_nexth;
}
/* Check if we are allowed to enqueue more datagrams. */
if ((ip6_reass_pbufcount + clen) > IP_REASS_MAX_PBUFS) {
#if IP_REASS_FREE_OLDEST
ip6_reass_remove_oldest_datagram(ipr, clen);
if ((ip6_reass_pbufcount + clen) <= IP_REASS_MAX_PBUFS) {
/* re-search ipr_prev since it might have been removed */
for (ipr_prev = reassdatagrams; ipr_prev != NULL; ipr_prev = ipr_prev->next) {
if (ipr_prev->next == ipr) {
break;
}
}
} else
#endif /* IP_REASS_FREE_OLDEST */
{
/* @todo: send ICMPv6 time exceeded here? */
/* drop this pbuf */
IP6_FRAG_STATS_INC(ip6_frag.memerr);
IP6_FRAG_STATS_INC(ip6_frag.drop);
goto nullreturn;
}
}
/* Overwrite Fragment Header with our own helper struct. */
#if IPV6_FRAG_COPYHEADER
if (IPV6_FRAG_REQROOM > 0) {
/* Make room for struct ip6_reass_helper (only required if sizeof(void*) > 4).
This cannot fail since we already checked when receiving this fragment. */
u8_t hdrerr = pbuf_header_force(p, IPV6_FRAG_REQROOM);
LWIP_UNUSED_ARG(hdrerr); /* in case of LWIP_NOASSERT */
LWIP_ASSERT("no room for struct ip6_reass_helper", hdrerr == 0);
}
#else /* IPV6_FRAG_COPYHEADER */
LWIP_ASSERT("sizeof(struct ip6_reass_helper) <= IP6_FRAG_HLEN, set IPV6_FRAG_COPYHEADER to 1",
sizeof(struct ip6_reass_helper) <= IP6_FRAG_HLEN);
#endif /* IPV6_FRAG_COPYHEADER */
iprh = (struct ip6_reass_helper *)p->payload;
iprh->next_pbuf = NULL;
iprh->start = (offset & IP6_FRAG_OFFSET_MASK);
iprh->end = (offset & IP6_FRAG_OFFSET_MASK) + len;
/* find the right place to insert this pbuf */
/* 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 ip6_reass_helper*)q->payload;
if (iprh->start < iprh_tmp->start) {
#if IP_REASS_CHECK_OVERLAP
if (iprh->end > iprh_tmp->start) {
/* fragment overlaps with following, throw away */
IP6_FRAG_STATS_INC(ip6_frag.proterr);
IP6_FRAG_STATS_INC(ip6_frag.drop);
goto nullreturn;
}
if (iprh_prev != NULL) {
if (iprh->start < iprh_prev->end) {
/* fragment overlaps with previous, throw away */
IP6_FRAG_STATS_INC(ip6_frag.proterr);
IP6_FRAG_STATS_INC(ip6_frag.drop);
goto nullreturn;
}
}
#endif /* IP_REASS_CHECK_OVERLAP */
/* the new pbuf should be inserted before this */
iprh->next_pbuf = q;
if (iprh_prev != NULL) {
/* not the fragment with the lowest offset */
iprh_prev->next_pbuf = p;
} else {
/* fragment with the lowest offset */
ipr->p = p;
}
break;
} else if (iprh->start == iprh_tmp->start) {
/* received the same datagram twice: no need to keep the datagram */
IP6_FRAG_STATS_INC(ip6_frag.drop);
goto nullreturn;
#if IP_REASS_CHECK_OVERLAP
} else if (iprh->start < iprh_tmp->end) {
/* overlap: no need to keep the new datagram */
IP6_FRAG_STATS_INC(ip6_frag.proterr);
IP6_FRAG_STATS_INC(ip6_frag.drop);
goto nullreturn;
#endif /* IP_REASS_CHECK_OVERLAP */
} else {
/* Check if the fragments received so far have no gaps. */
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 = 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 = p;
}
}
/* 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 */
ip6_reass_pbufcount += clen;
/* Remember IPv6 header if this is the first fragment. */
if (iprh->start == 0) {
#if IPV6_FRAG_COPYHEADER
if (iprh->next_pbuf != NULL) {
MEMCPY(&ipr->iphdr, ip6_current_header(), IP6_HLEN);
}
#else /* IPV6_FRAG_COPYHEADER */
/* need to use the none-const pointer here: */
ipr->iphdr = ip_data.current_ip6_header;
#endif /* IPV6_FRAG_COPYHEADER */
}
/* If this is the last fragment, calculate total packet length. */
if ((offset & IP6_FRAG_MORE_FLAG) == 0) {
ipr->datagram_len = iprh->end;
}
/* Additional validity tests: we have received first and last fragment. */
iprh_tmp = (struct ip6_reass_helper*)ipr->p->payload;
if (iprh_tmp->start != 0) {
valid = 0;
}
if (ipr->datagram_len == 0) {
valid = 0;
}
/* Final validity test: no gaps between current and last fragment. */
iprh_prev = iprh;
q = iprh->next_pbuf;
while ((q != NULL) && valid) {
iprh = (struct ip6_reass_helper*)q->payload;
if (iprh_prev->end != iprh->start) {
valid = 0;
break;
}
iprh_prev = iprh;
q = iprh->next_pbuf;
}
if (valid) {
/* All fragments have been received */
struct ip6_hdr* iphdr_ptr;
/* chain together the pbufs contained within the ip6_reassdata list. */
iprh = (struct ip6_reass_helper*) ipr->p->payload;
while (iprh != NULL) {
struct pbuf* next_pbuf = iprh->next_pbuf;
if (next_pbuf != NULL) {
/* Save next helper struct (will be hidden in next step). */
iprh_tmp = (struct ip6_reass_helper*)next_pbuf->payload;
/* hide the fragment header for every succeeding fragment */
pbuf_header(next_pbuf, -IP6_FRAG_HLEN);
#if IPV6_FRAG_COPYHEADER
if (IPV6_FRAG_REQROOM > 0) {
/* hide the extra bytes borrowed from ip6_hdr for struct ip6_reass_helper */
u8_t hdrerr = pbuf_header(next_pbuf, -(s16_t)(IPV6_FRAG_REQROOM));
LWIP_UNUSED_ARG(hdrerr); /* in case of LWIP_NOASSERT */
LWIP_ASSERT("no room for struct ip6_reass_helper", hdrerr == 0);
}
#endif
pbuf_cat(ipr->p, next_pbuf);
}
else {
iprh_tmp = NULL;
}
iprh = iprh_tmp;
}
#if IPV6_FRAG_COPYHEADER
if (IPV6_FRAG_REQROOM > 0) {
/* get back room for struct ip6_reass_helper (only required if sizeof(void*) > 4) */
u8_t hdrerr = pbuf_header(ipr->p, -(s16_t)(IPV6_FRAG_REQROOM));
LWIP_UNUSED_ARG(hdrerr); /* in case of LWIP_NOASSERT */
LWIP_ASSERT("no room for struct ip6_reass_helper", hdrerr == 0);
}
iphdr_ptr = (struct ip6_hdr*)((u8_t*)ipr->p->payload - IP6_HLEN);
MEMCPY(iphdr_ptr, &ipr->iphdr, IP6_HLEN);
#else
iphdr_ptr = ipr->iphdr;
#endif
/* Adjust datagram length by adding header lengths. */
ipr->datagram_len += (u16_t)(((u8_t*)ipr->p->payload - (u8_t*)iphdr_ptr)
+ IP6_FRAG_HLEN
- IP6_HLEN);
/* Set payload length in ip header. */
iphdr_ptr->_plen = lwip_htons(ipr->datagram_len);
/* Get the first pbuf. */
p = ipr->p;
/* Restore Fragment Header in first pbuf. Mark as "single fragment"
* packet. Restore nexth. */
frag_hdr = (struct ip6_frag_hdr *) p->payload;
frag_hdr->_nexth = ipr->nexth;
frag_hdr->reserved = 0;
frag_hdr->_fragment_offset = 0;
frag_hdr->_identification = 0;
/* release the sources allocate for the fragment queue entry */
if (reassdatagrams == ipr) {
/* it was the first in the list */
reassdatagrams = ipr->next;
} else {
/* it wasn't the first, so it must have a valid 'prev' */
LWIP_ASSERT("sanity check linked list", ipr_prev != NULL);
ipr_prev->next = ipr->next;
}
memp_free(MEMP_IP6_REASSDATA, ipr);
/* adjust the number of pbufs currently queued for reassembly. */
ip6_reass_pbufcount -= pbuf_clen(p);
/* Move pbuf back to IPv6 header.
This cannot fail since we already checked when receiving this fragment. */
if (pbuf_header_force(p, (s16_t)((u8_t*)p->payload - (u8_t*)iphdr_ptr))) {
LWIP_ASSERT("ip6_reass: moving p->payload to ip6 header failed\n", 0);
pbuf_free(p);
return NULL;
}
/* Return the pbuf chain */
return p;
}
/* the datagram is not (yet?) reassembled completely */
return NULL;
nullreturn:
pbuf_free(p);
return NULL;
}
/**
* Reassembles incoming IPv6 fragments into an IPv6 datagram.
*
* @param p points to the IPv6 Fragment Header
* @return NULL if reassembly is incomplete, pbuf pointing to
* IPv6 Header if reassembly is complete
*/
struct pbuf *
ip6_reass(struct pbuf *p)
{
struct ip6_reassdata *ipr, *ipr_prev;
struct ip6_reass_helper *iprh, *iprh_tmp, *iprh_prev=NULL;
struct ip6_frag_hdr *frag_hdr;
u16_t offset, len, start, end;
ptrdiff_t hdrdiff;
u16_t clen;
u8_t valid = 1;
struct pbuf *q, *next_pbuf;
IP6_FRAG_STATS_INC(ip6_frag.recv);
/* ip6_frag_hdr must be in the first pbuf, not chained. Checked by caller. */
LWIP_ASSERT("IPv6 fragment header does not fit in first pbuf",
p->len >= sizeof(struct ip6_frag_hdr));
frag_hdr = (struct ip6_frag_hdr *) p->payload;
clen = pbuf_clen(p);
offset = lwip_ntohs(frag_hdr->_fragment_offset);
/* Calculate fragment length from IPv6 payload length.
* Adjust for headers before Fragment Header.
* And finally adjust by Fragment Header length. */
len = lwip_ntohs(ip6_current_header()->_plen);
hdrdiff = (u8_t*)p->payload - (const u8_t*)ip6_current_header();
LWIP_ASSERT("not a valid pbuf (ip6_input check missing?)", hdrdiff <= 0xFFFF);
LWIP_ASSERT("not a valid pbuf (ip6_input check missing?)", hdrdiff >= IP6_HLEN);
hdrdiff -= IP6_HLEN;
hdrdiff += IP6_FRAG_HLEN;
if (hdrdiff > len) {
IP6_FRAG_STATS_INC(ip6_frag.proterr);
goto nullreturn;
}
len = (u16_t)(len - hdrdiff);
start = (offset & IP6_FRAG_OFFSET_MASK);
if (start > (0xFFFF - len)) {
/* u16_t overflow, cannot handle this */
IP6_FRAG_STATS_INC(ip6_frag.proterr);
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_prev = NULL; 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 ((frag_hdr->_identification == ipr->identification) &&
ip6_addr_cmp_packed(ip6_current_src_addr(), &(IPV6_FRAG_SRC(ipr)), ipr->src_zone) &&
ip6_addr_cmp_packed(ip6_current_dest_addr(), &(IPV6_FRAG_DEST(ipr)), ipr->dest_zone)) {
IP6_FRAG_STATS_INC(ip6_frag.cachehit);
break;
}
ipr_prev = ipr;
}
if (ipr == NULL) {
/* Enqueue a new datagram into the datagram queue */
ipr = (struct ip6_reassdata *)memp_malloc(MEMP_IP6_REASSDATA);
if (ipr == NULL) {
#if IP_REASS_FREE_OLDEST
/* Make room and try again. */
ip6_reass_remove_oldest_datagram(ipr, clen);
ipr = (struct ip6_reassdata *)memp_malloc(MEMP_IP6_REASSDATA);
if (ipr != NULL) {
/* re-search ipr_prev since it might have been removed */
for (ipr_prev = reassdatagrams; ipr_prev != NULL; ipr_prev = ipr_prev->next) {
if (ipr_prev->next == ipr) {
break;
}
}
} else
#endif /* IP_REASS_FREE_OLDEST */
{
IP6_FRAG_STATS_INC(ip6_frag.memerr);
goto nullreturn;
}
}
memset(ipr, 0, sizeof(struct ip6_reassdata));
ipr->timer = IPV6_REASS_MAXAGE;
/* enqueue the new structure to the front of the list */
ipr->next = reassdatagrams;
reassdatagrams = ipr;
/* Use the current IPv6 header for src/dest address reference.
* Eventually, we will replace it when we get the first fragment
* (it might be this one, in any case, it is done later). */
/* need to use the none-const pointer here: */
ipr->iphdr = ip_data.current_ip6_header;
#if IPV6_FRAG_COPYHEADER
MEMCPY(&ipr->src, &ip6_current_header()->src, sizeof(ipr->src));
MEMCPY(&ipr->dest, &ip6_current_header()->dest, sizeof(ipr->dest));
#endif /* IPV6_FRAG_COPYHEADER */
#if LWIP_IPV6_SCOPES
/* Also store the address zone information.
* @todo It is possible that due to netif destruction and recreation, the
* stored zones end up resolving to a different interface. In that case, we
* risk sending a "time exceeded" ICMP response over the wrong link.
* Ideally, netif destruction would clean up matching pending reassembly
* structures, but custom zone mappings would make that non-trivial. */
ipr->src_zone = ip6_addr_zone(ip6_current_src_addr());
ipr->dest_zone = ip6_addr_zone(ip6_current_dest_addr());
#endif /* LWIP_IPV6_SCOPES */
/* copy the fragmented packet id. */
ipr->identification = frag_hdr->_identification;
/* copy the nexth field */
ipr->nexth = frag_hdr->_nexth;
}
/* Check if we are allowed to enqueue more datagrams. */
if ((ip6_reass_pbufcount + clen) > IP_REASS_MAX_PBUFS) {
#if IP_REASS_FREE_OLDEST
ip6_reass_remove_oldest_datagram(ipr, clen);
if ((ip6_reass_pbufcount + clen) <= IP_REASS_MAX_PBUFS) {
/* re-search ipr_prev since it might have been removed */
for (ipr_prev = reassdatagrams; ipr_prev != NULL; ipr_prev = ipr_prev->next) {
if (ipr_prev->next == ipr) {
break;
}
}
} else
#endif /* IP_REASS_FREE_OLDEST */
{
/* @todo: send ICMPv6 time exceeded here? */
/* drop this pbuf */
IP6_FRAG_STATS_INC(ip6_frag.memerr);
goto nullreturn;
}
}
/* Overwrite Fragment Header with our own helper struct. */
#if IPV6_FRAG_COPYHEADER
if (IPV6_FRAG_REQROOM > 0) {
/* Make room for struct ip6_reass_helper (only required if sizeof(void*) > 4).
This cannot fail since we already checked when receiving this fragment. */
u8_t hdrerr = pbuf_header_force(p, IPV6_FRAG_REQROOM);
LWIP_UNUSED_ARG(hdrerr); /* in case of LWIP_NOASSERT */
LWIP_ASSERT("no room for struct ip6_reass_helper", hdrerr == 0);
}
#else /* IPV6_FRAG_COPYHEADER */
LWIP_ASSERT("sizeof(struct ip6_reass_helper) <= IP6_FRAG_HLEN, set IPV6_FRAG_COPYHEADER to 1",
sizeof(struct ip6_reass_helper) <= IP6_FRAG_HLEN);
#endif /* IPV6_FRAG_COPYHEADER */
/* Prepare the pointer to the helper structure, and its initial values.
* Do not yet write to the structure itself, as we still have to make a
* backup of the original data, and we should not do that until we know for
* sure that we are going to add this packet to the list. */
iprh = (struct ip6_reass_helper *)p->payload;
next_pbuf = NULL;
end = (u16_t)(start + len);
/* find the right place to insert this pbuf */
/* 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 ip6_reass_helper*)q->payload;
if (start < iprh_tmp->start) {
#if IP_REASS_CHECK_OVERLAP
if (end > iprh_tmp->start) {
/* fragment overlaps with following, throw away */
IP6_FRAG_STATS_INC(ip6_frag.proterr);
goto nullreturn;
}
if (iprh_prev != NULL) {
if (start < iprh_prev->end) {
/* fragment overlaps with previous, throw away */
IP6_FRAG_STATS_INC(ip6_frag.proterr);
goto nullreturn;
}
}
#endif /* IP_REASS_CHECK_OVERLAP */
/* the new pbuf should be inserted before this */
next_pbuf = q;
if (iprh_prev != NULL) {
/* not the fragment with the lowest offset */
iprh_prev->next_pbuf = p;
} else {
/* fragment with the lowest offset */
ipr->p = p;
}
break;
} else if (start == iprh_tmp->start) {
/* received the same datagram twice: no need to keep the datagram */
goto nullreturn;
#if IP_REASS_CHECK_OVERLAP
} else if (start < iprh_tmp->end) {
/* overlap: no need to keep the new datagram */
IP6_FRAG_STATS_INC(ip6_frag.proterr);
goto nullreturn;
#endif /* IP_REASS_CHECK_OVERLAP */
} else {
/* Check if the fragments received so far have no gaps. */
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 <= start);
#endif /* IP_REASS_CHECK_OVERLAP */
iprh_prev->next_pbuf = p;
if (iprh_prev->end != 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 = p;
}
}
/* 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 */
ip6_reass_pbufcount = (u16_t)(ip6_reass_pbufcount + clen);
/* Remember IPv6 header if this is the first fragment. */
if (start == 0) {
/* need to use the none-const pointer here: */
ipr->iphdr = ip_data.current_ip6_header;
/* Make a backup of the part of the packet data that we are about to
* overwrite, so that we can restore the original later. */
MEMCPY(ipr->orig_hdr, p->payload, sizeof(*iprh));
/* For IPV6_FRAG_COPYHEADER there is no need to copy src/dst again, as they
* will be the same as they were. With LWIP_IPV6_SCOPES, the same applies
* to the source/destination zones. */
}
/* Only after the backup do we get to fill in the actual helper structure. */
iprh->next_pbuf = next_pbuf;
iprh->start = start;
iprh->end = end;
/* If this is the last fragment, calculate total packet length. */
if ((offset & IP6_FRAG_MORE_FLAG) == 0) {
ipr->datagram_len = iprh->end;
}
/* Additional validity tests: we have received first and last fragment. */
iprh_tmp = (struct ip6_reass_helper*)ipr->p->payload;
if (iprh_tmp->start != 0) {
valid = 0;
}
if (ipr->datagram_len == 0) {
valid = 0;
}
/* Final validity test: no gaps between current and last fragment. */
iprh_prev = iprh;
q = iprh->next_pbuf;
while ((q != NULL) && valid) {
iprh = (struct ip6_reass_helper*)q->payload;
if (iprh_prev->end != iprh->start) {
valid = 0;
break;
}
iprh_prev = iprh;
q = iprh->next_pbuf;
}
if (valid) {
/* All fragments have been received */
struct ip6_hdr* iphdr_ptr;
/* chain together the pbufs contained within the ip6_reassdata list. */
iprh = (struct ip6_reass_helper*) ipr->p->payload;
while (iprh != NULL) {
next_pbuf = iprh->next_pbuf;
if (next_pbuf != NULL) {
/* Save next helper struct (will be hidden in next step). */
iprh_tmp = (struct ip6_reass_helper*)next_pbuf->payload;
/* hide the fragment header for every succeeding fragment */
pbuf_remove_header(next_pbuf, IP6_FRAG_HLEN);
#if IPV6_FRAG_COPYHEADER
if (IPV6_FRAG_REQROOM > 0) {
/* hide the extra bytes borrowed from ip6_hdr for struct ip6_reass_helper */
u8_t hdrerr = pbuf_remove_header(next_pbuf, IPV6_FRAG_REQROOM);
LWIP_UNUSED_ARG(hdrerr); /* in case of LWIP_NOASSERT */
LWIP_ASSERT("no room for struct ip6_reass_helper", hdrerr == 0);
}
#endif
pbuf_cat(ipr->p, next_pbuf);
}
else {
iprh_tmp = NULL;
}
iprh = iprh_tmp;
}
/* Get the first pbuf. */
p = ipr->p;
#if IPV6_FRAG_COPYHEADER
if (IPV6_FRAG_REQROOM > 0) {
u8_t hdrerr;
/* Restore (only) the bytes that we overwrote beyond the fragment header.
* Those bytes may belong to either the IPv6 header or an extension
* header placed before the fragment header. */
MEMCPY(p->payload, ipr->orig_hdr, IPV6_FRAG_REQROOM);
/* get back room for struct ip6_reass_helper (only required if sizeof(void*) > 4) */
hdrerr = pbuf_remove_header(p, IPV6_FRAG_REQROOM);
LWIP_UNUSED_ARG(hdrerr); /* in case of LWIP_NOASSERT */
LWIP_ASSERT("no room for struct ip6_reass_helper", hdrerr == 0);
}
#endif
/* We need to get rid of the fragment header itself, which is somewhere in
* the middle of the packet (but still in the first pbuf of the chain).
* Getting rid of the header is required by RFC 2460 Sec. 4.5 and necessary
* in order to be able to reassemble packets that are close to full size
* (i.e., around 65535 bytes). We simply move up all the headers before the
* fragment header, including the IPv6 header, and adjust the payload start
* accordingly. This works because all these headers are in the first pbuf
* of the chain, and because the caller adjusts all its pointers on
* successful reassembly. */
MEMMOVE((u8_t*)ipr->iphdr + sizeof(struct ip6_frag_hdr), ipr->iphdr,
(size_t)((u8_t*)p->payload - (u8_t*)ipr->iphdr));
/* This is where the IPv6 header is now. */
iphdr_ptr = (struct ip6_hdr*)((u8_t*)ipr->iphdr +
sizeof(struct ip6_frag_hdr));
/* Adjust datagram length by adding header lengths. */
ipr->datagram_len = (u16_t)(ipr->datagram_len + ((u8_t*)p->payload - (u8_t*)iphdr_ptr)
- IP6_HLEN);
/* Set payload length in ip header. */
iphdr_ptr->_plen = lwip_htons(ipr->datagram_len);
/* With the fragment header gone, we now need to adjust the next-header
* field of whatever header was originally before it. Since the packet made
* it through the original header processing routines at least up to the
* fragment header, we do not need any further sanity checks here. */
if (IP6H_NEXTH(iphdr_ptr) == IP6_NEXTH_FRAGMENT) {
iphdr_ptr->_nexth = ipr->nexth;
} else {
u8_t *ptr = (u8_t *)iphdr_ptr + IP6_HLEN;
while (*ptr != IP6_NEXTH_FRAGMENT) {
ptr += 8 * (1 + ptr[1]);
}
*ptr = ipr->nexth;
}
/* release the resources allocated for the fragment queue entry */
if (reassdatagrams == ipr) {
/* it was the first in the list */
reassdatagrams = ipr->next;
} else {
/* it wasn't the first, so it must have a valid 'prev' */
LWIP_ASSERT("sanity check linked list", ipr_prev != NULL);
ipr_prev->next = ipr->next;
}
memp_free(MEMP_IP6_REASSDATA, ipr);
/* adjust the number of pbufs currently queued for reassembly. */
clen = pbuf_clen(p);
LWIP_ASSERT("ip6_reass_pbufcount >= clen", ip6_reass_pbufcount >= clen);
ip6_reass_pbufcount = (u16_t)(ip6_reass_pbufcount - clen);
/* Move pbuf back to IPv6 header. This should never fail. */
if (pbuf_header_force(p, (s16_t)((u8_t*)p->payload - (u8_t*)iphdr_ptr))) {
LWIP_ASSERT("ip6_reass: moving p->payload to ip6 header failed\n", 0);
pbuf_free(p);
return NULL;
}
/* Return the pbuf chain */
return p;
}
/* the datagram is not (yet?) reassembled completely */
return NULL;
nullreturn:
IP6_FRAG_STATS_INC(ip6_frag.drop);
pbuf_free(p);
return NULL;
}
static void
recv(void *arg, struct udp_pcb *upcb, struct pbuf *p, const ip_addr_t *addr, u16_t port)
{
u16_t *sbuf = (u16_t *) p->payload;
int opcode;
LWIP_UNUSED_ARG(arg);
LWIP_UNUSED_ARG(upcb);
if (((tftp_state.port != 0) && (port != tftp_state.port)) ||
(!ip_addr_isany_val(tftp_state.addr) && !ip_addr_cmp(&tftp_state.addr, addr))) {
send_error(addr, port, TFTP_ERROR_ACCESS_VIOLATION, "Only one connection at a time is supported");
pbuf_free(p);
return;
}
opcode = sbuf[0];
tftp_state.last_pkt = tftp_state.timer;
tftp_state.retries = 0;
switch (opcode) {
case PP_HTONS(TFTP_RRQ): /* fall through */
case PP_HTONS(TFTP_WRQ):
{
const char tftp_null = 0;
char filename[TFTP_MAX_FILENAME_LEN];
char mode[TFTP_MAX_MODE_LEN];
u16_t filename_end_offset;
u16_t mode_end_offset;
if(tftp_state.handle != NULL) {
send_error(addr, port, TFTP_ERROR_ACCESS_VIOLATION, "Only one connection at a time is supported");
break;
}
sys_timeout(TFTP_TIMER_MSECS, tftp_tmr, NULL);
/* find \0 in pbuf -> end of filename string */
filename_end_offset = pbuf_memfind(p, &tftp_null, sizeof(tftp_null), 2);
if((u16_t)(filename_end_offset-2) > sizeof(filename)) {
send_error(addr, port, TFTP_ERROR_ACCESS_VIOLATION, "Filename too long/not NULL terminated");
break;
}
pbuf_copy_partial(p, filename, filename_end_offset-2, 2);
/* find \0 in pbuf -> end of mode string */
mode_end_offset = pbuf_memfind(p, &tftp_null, sizeof(tftp_null), filename_end_offset+1);
if((u16_t)(mode_end_offset-filename_end_offset) > sizeof(mode)) {
send_error(addr, port, TFTP_ERROR_ACCESS_VIOLATION, "Mode too long/not NULL terminated");
break;
}
pbuf_copy_partial(p, mode, mode_end_offset-filename_end_offset, filename_end_offset+1);
tftp_state.handle = tftp_state.ctx->open(filename, mode, opcode == PP_HTONS(TFTP_WRQ));
tftp_state.blknum = 1;
if (!tftp_state.handle) {
send_error(addr, port, TFTP_ERROR_FILE_NOT_FOUND, "Unable to open requested file.");
break;
}
LWIP_DEBUGF(TFTP_DEBUG | LWIP_DBG_STATE, ("tftp: %s request from ", (opcode == PP_HTONS(TFTP_WRQ)) ? "write" : "read"));
ip_addr_debug_print(TFTP_DEBUG | LWIP_DBG_STATE, addr);
LWIP_DEBUGF(TFTP_DEBUG | LWIP_DBG_STATE, (" for '%s' mode '%s'\n", filename, mode));
ip_addr_copy(tftp_state.addr, *addr);
tftp_state.port = port;
if (opcode == PP_HTONS(TFTP_WRQ)) {
tftp_state.mode_write = 1;
send_ack(0);
} else {
tftp_state.mode_write = 0;
send_data();
}
break;
}
case PP_HTONS(TFTP_DATA):
{
int ret;
u16_t blknum;
if (tftp_state.handle == NULL) {
send_error(addr, port, TFTP_ERROR_ACCESS_VIOLATION, "No connection");
break;
}
if (tftp_state.mode_write != 1) {
send_error(addr, port, TFTP_ERROR_ACCESS_VIOLATION, "Not a write connection");
break;
}
blknum = lwip_ntohs(sbuf[1]);
pbuf_header(p, -TFTP_HEADER_LENGTH);
ret = tftp_state.ctx->write(tftp_state.handle, p);
if (ret < 0) {
send_error(addr, port, TFTP_ERROR_ACCESS_VIOLATION, "error writing file");
close_handle();
} else {
send_ack(blknum);
}
if (p->tot_len < TFTP_MAX_PAYLOAD_SIZE) {
close_handle();
}
break;
}
case PP_HTONS(TFTP_ACK):
{
u16_t blknum;
int lastpkt;
if (tftp_state.handle == NULL) {
send_error(addr, port, TFTP_ERROR_ACCESS_VIOLATION, "No connection");
break;
}
if (tftp_state.mode_write != 0) {
send_error(addr, port, TFTP_ERROR_ACCESS_VIOLATION, "Not a read connection");
break;
}
blknum = lwip_ntohs(sbuf[1]);
if (blknum != tftp_state.blknum) {
send_error(addr, port, TFTP_ERROR_UNKNOWN_TRFR_ID, "Wrong block number");
break;
}
lastpkt = 0;
if (tftp_state.last_data != NULL) {
lastpkt = tftp_state.last_data->tot_len != (TFTP_MAX_PAYLOAD_SIZE + TFTP_HEADER_LENGTH);
}
if (!lastpkt) {
tftp_state.blknum++;
send_data();
} else {
close_handle();
}
break;
}
default:
send_error(addr, port, TFTP_ERROR_ILLEGAL_OPERATION, "Unknown operation");
break;
}
pbuf_free(p);
}
