STATIC mp_obj_t socket_sendto(mp_obj_t self_in, mp_obj_t data_in, mp_obj_t addr_in) { socket_obj_t *self = MP_OBJ_TO_PTR(self_in); // get the buffer to send mp_buffer_info_t bufinfo; mp_get_buffer_raise(data_in, &bufinfo, MP_BUFFER_READ); // create the destination address struct sockaddr_in to; to.sin_len = sizeof(to); to.sin_family = AF_INET; to.sin_port = lwip_htons(netutils_parse_inet_addr(addr_in, (uint8_t*)&to.sin_addr, NETUTILS_BIG)); // send the data for (int i=0; i<=self->retries; i++) { MP_THREAD_GIL_EXIT(); int ret = lwip_sendto_r(self->fd, bufinfo.buf, bufinfo.len, 0, (struct sockaddr*)&to, sizeof(to)); MP_THREAD_GIL_ENTER(); if (ret > 0) return mp_obj_new_int_from_uint(ret); if (ret == -1 && errno != EWOULDBLOCK) { exception_from_errno(errno); } check_for_exceptions(); } mp_raise_OSError(MP_ETIMEDOUT); }
static void send_data(void) { u16_t *payload; int ret; if(tftp_state.last_data != NULL) { pbuf_free(tftp_state.last_data); } tftp_state.last_data = pbuf_alloc(PBUF_TRANSPORT, TFTP_HEADER_LENGTH + TFTP_MAX_PAYLOAD_SIZE, PBUF_RAM); if(tftp_state.last_data == NULL) { return; } payload = (u16_t *) tftp_state.last_data->payload; payload[0] = PP_HTONS(TFTP_DATA); payload[1] = lwip_htons(tftp_state.blknum); ret = tftp_state.ctx->read(tftp_state.handle, &payload[2], TFTP_MAX_PAYLOAD_SIZE); if (ret < 0) { send_error(&tftp_state.addr, tftp_state.port, TFTP_ERROR_ACCESS_VIOLATION, "Error occured while reading the file."); close_handle(); return; } pbuf_realloc(tftp_state.last_data, (u16_t)(TFTP_HEADER_LENGTH + ret)); resend_data(); }
static struct pbuf * test_udp_create_test_packet(u16_t length, u16_t port, u32_t dst_addr) { err_t err; u8_t ret; struct udp_hdr *uh; struct ip_hdr *ih; struct pbuf *p; const u8_t test_data[16] = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 0xa, 0xb, 0xc, 0xd, 0xe, 0xf}; p = pbuf_alloc(PBUF_TRANSPORT, length, PBUF_POOL); fail_unless(p != NULL); if (p == NULL) { return NULL; } fail_unless(p->next == NULL); err = pbuf_take(p, test_data, length); fail_unless(err == ERR_OK); /* add UDP header */ ret = pbuf_add_header(p, sizeof(struct udp_hdr)); fail_unless(!ret); uh = (struct udp_hdr *)p->payload; uh->chksum = 0; uh->dest = uh->src = lwip_htons(port); uh->len = lwip_htons(p->tot_len); /* add IPv4 header */ ret = pbuf_add_header(p, sizeof(struct ip_hdr)); fail_unless(!ret); ih = (struct ip_hdr *)p->payload; memset(ih, 0, sizeof(*ih)); ih->dest.addr = dst_addr; ih->_len = lwip_htons(p->tot_len); ih->_ttl = 32; ih->_proto = IP_PROTO_UDP; IPH_VHL_SET(ih, 4, sizeof(struct ip_hdr) / 4); IPH_CHKSUM_SET(ih, inet_chksum(ih, sizeof(struct ip_hdr))); return p; }
void zta2ss(JNIEnv *env, struct sockaddr_storage *ss, jobject addr) { jclass c = env->GetObjectClass(addr); if (!c) { return; } jfieldID fid = env->GetFieldID(c, "_family", "I"); int family = env->GetIntField(addr, fid); if (family == AF_INET) { struct sockaddr_in *in4 = (struct sockaddr_in*)ss; fid = env->GetFieldID(c, "_port", "I"); in4->sin_port = lwip_htons(env->GetIntField(addr, fid)); in4->sin_family = AF_INET; 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(&(in4->sin_addr.s_addr), data, 4); env->ReleaseByteArrayElements(*arr, (jbyte*)data, 0); return; } if (family == AF_INET6) { struct sockaddr_in6 *in6 = (struct sockaddr_in6*)ss; jfieldID fid = env->GetFieldID(c, "_port", "I"); in6->sin6_port = lwip_htons(env->GetIntField(addr, fid)); fid = env->GetFieldID(c,"_family", "I"); in6->sin6_family = AF_INET6; 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(&(in6->sin6_addr.s6_addr), data, 16); env->ReleaseByteArrayElements(*arr, (jbyte*)data, 0); return; } }
static void frame_client_create_package_data(rt_uint8_t *pkt_data, struct frame_format_em *frame, rt_uint8_t *data) { rt_uint8_t *tx_pch; tx_pch = pkt_data; *tx_pch++ = frame->head1; *tx_pch++ = frame->head2; *tx_pch++ = frame->head3; *tx_pch++ = frame->head4; *tx_pch++ = frame->ctrl; *tx_pch++ = lwip_htons(frame->data_len) & 0xff; *tx_pch++ = lwip_htons(frame->data_len) >> 8; rt_memcpy(tx_pch, data, frame->data_len); }
static struct pbuf* init_packet(u16_t opcode, u16_t extra, size_t size) { struct pbuf* p = pbuf_alloc(PBUF_TRANSPORT, (u16_t)(TFTP_HEADER_LENGTH + size), PBUF_RAM); u16_t* payload; if (p != NULL) { payload = (u16_t*) p->payload; payload[0] = PP_HTONS(opcode); payload[1] = lwip_htons(extra); } return p; }
/* Send 6LoWPAN TX packets as UDP broadcast */ static err_t zepif_linkoutput(struct netif *netif, struct pbuf *p) { err_t err; struct pbuf *q; struct zep_hdr *zep; struct zepif_state *state; LWIP_ASSERT("invalid netif", netif != NULL); LWIP_ASSERT("invalid pbuf", p != NULL); if (p->tot_len > ZEP_MAX_DATA_LEN) { return ERR_VAL; } LWIP_ASSERT("TODO: support chained pbufs", p->next == NULL); state = (struct zepif_state *)netif->state; LWIP_ASSERT("state->pcb != NULL", state->pcb != NULL); q = pbuf_alloc(PBUF_TRANSPORT, sizeof(struct zep_hdr) + p->tot_len, PBUF_RAM); if (q == NULL) { return ERR_MEM; } zep = (struct zep_hdr *)q->payload; memset(zep, 0, sizeof(struct zep_hdr)); zep->prot_id[0] = 'E'; zep->prot_id[1] = 'X'; zep->prot_version = 2; zep->type = 1; /* Data */ zep->channel_id = 0; /* whatever */ zep->device_id = lwip_htons(1); /* whatever */ zep->crc_mode = 1; zep->unknown_1 = 0xff; zep->seq_num = lwip_htonl(state->seqno); state->seqno++; zep->len = (u8_t)p->tot_len; err = pbuf_take_at(q, p->payload, p->tot_len, sizeof(struct zep_hdr)); if (err == ERR_OK) { #if ZEPIF_LOOPBACK zepif_udp_recv(netif, state->pcb, pbuf_clone(PBUF_RAW, PBUF_RAM, q), NULL, 0); #endif err = udp_sendto(state->pcb, q, state->init.zep_dst_ip_addr, state->init.zep_dst_udp_port); } pbuf_free(q); return err; }
static void send_ack(u16_t blknum) { struct pbuf* p; u16_t* payload; p = pbuf_alloc(PBUF_TRANSPORT, TFTP_HEADER_LENGTH, PBUF_RAM); if(p == NULL) { return; } payload = (u16_t*) p->payload; payload[0] = PP_HTONS(TFTP_ACK); payload[1] = lwip_htons(blknum); udp_sendto(tftp_state.upcb, p, &tftp_state.addr, tftp_state.port); pbuf_free(p); }
static void send_error(const ip_addr_t *addr, u16_t port, enum tftp_error code, const char *str) { int str_length = strlen(str); struct pbuf* p; u16_t* payload; p = pbuf_alloc(PBUF_TRANSPORT, (u16_t)(TFTP_HEADER_LENGTH + str_length + 1), PBUF_RAM); if(p == NULL) { return; } payload = (u16_t*) p->payload; payload[0] = PP_HTONS(TFTP_ERROR); payload[1] = lwip_htons(code); MEMCPY(&payload[2], str, str_length + 1); udp_sendto(tftp_state.upcb, p, addr, port); pbuf_free(p); }
/** * 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)); }
/** * Translates the name of a service location (for example, a host name) and/or * a service name and returns a set of socket addresses and associated * information to be used in creating a socket with which to address the * specified service. * Memory for the result is allocated internally and must be freed by calling * lwip_freeaddrinfo()! * * Due to a limitation in dns_gethostbyname, only the first address of a * host is returned. * Also, service names are not supported (only port numbers)! * * @param nodename descriptive name or address string of the host * (may be NULL -> local address) * @param servname port number as string of NULL * @param hints structure containing input values that set socktype and protocol * @param res pointer to a pointer where to store the result (set to NULL on failure) * @return 0 on success, non-zero on failure * * @todo: implement AI_V4MAPPED, AI_ADDRCONFIG */ int lwip_getaddrinfo(const char *nodename, const char *servname, const struct addrinfo *hints, struct addrinfo **res) { err_t err; ip_addr_t addr; struct addrinfo *ai; struct sockaddr_storage *sa = NULL; int port_nr = 0; size_t total_size; size_t namelen = 0; int ai_family; if (res == NULL) { return EAI_FAIL; } *res = NULL; if ((nodename == NULL) && (servname == NULL)) { return EAI_NONAME; } if (hints != NULL) { ai_family = hints->ai_family; if ((ai_family != AF_UNSPEC) #if LWIP_IPV4 && (ai_family != AF_INET) #endif /* LWIP_IPV4 */ #if LWIP_IPV6 && (ai_family != AF_INET6) #endif /* LWIP_IPV6 */ ) { return EAI_FAMILY; } } else { ai_family = AF_UNSPEC; } if (servname != NULL) { /* service name specified: convert to port number * @todo?: currently, only ASCII integers (port numbers) are supported (AI_NUMERICSERV)! */ port_nr = atoi(servname); if ((port_nr <= 0) || (port_nr > 0xffff)) { return EAI_SERVICE; } } if (nodename != NULL) { /* service location specified, try to resolve */ if ((hints != NULL) && (hints->ai_flags & AI_NUMERICHOST)) { /* no DNS lookup, just parse for an address string */ if (!ipaddr_aton(nodename, &addr)) { return EAI_NONAME; } #if LWIP_IPV4 && LWIP_IPV6 if ((IP_IS_V6_VAL(addr) && ai_family == AF_INET) || (IP_IS_V4_VAL(addr) && ai_family == AF_INET6)) { return EAI_NONAME; } #endif /* LWIP_IPV4 && LWIP_IPV6 */ } else { #if LWIP_IPV4 && LWIP_IPV6 /* AF_UNSPEC: prefer IPv4 */ u8_t type = NETCONN_DNS_IPV4_IPV6; if (ai_family == AF_INET) { type = NETCONN_DNS_IPV4; } else if (ai_family == AF_INET6) { type = NETCONN_DNS_IPV6; } #endif /* LWIP_IPV4 && LWIP_IPV6 */ err = netconn_gethostbyname_addrtype(nodename, &addr, type); if (err != ERR_OK) { return EAI_FAIL; } } } else { /* service location specified, use loopback address */ if ((hints != NULL) && (hints->ai_flags & AI_PASSIVE)) { ip_addr_set_any(ai_family == AF_INET6, &addr); } else { ip_addr_set_loopback(ai_family == AF_INET6, &addr); } } total_size = sizeof(struct addrinfo) + sizeof(struct sockaddr_storage); if (nodename != NULL) { namelen = strlen(nodename); if (namelen > DNS_MAX_NAME_LENGTH) { /* invalid name length */ return EAI_FAIL; } LWIP_ASSERT("namelen is too long", total_size + namelen + 1 > total_size); total_size += namelen + 1; } /* If this fails, please report to lwip-devel! :-) */ LWIP_ASSERT("total_size <= NETDB_ELEM_SIZE: please report this!", total_size <= NETDB_ELEM_SIZE); ai = (struct addrinfo *)memp_malloc(MEMP_NETDB); if (ai == NULL) { return EAI_MEMORY; } memset(ai, 0, total_size); /* cast through void* to get rid of alignment warnings */ sa = (struct sockaddr_storage *)(void*)((u8_t*)ai + sizeof(struct addrinfo)); if (IP_IS_V6_VAL(addr)) { #if LWIP_IPV6 struct sockaddr_in6 *sa6 = (struct sockaddr_in6*)sa; /* set up sockaddr */ inet6_addr_from_ip6addr(&sa6->sin6_addr, ip_2_ip6(&addr)); sa6->sin6_family = AF_INET6; sa6->sin6_len = sizeof(struct sockaddr_in6); sa6->sin6_port = lwip_htons((u16_t)port_nr); ai->ai_family = AF_INET6; #endif /* LWIP_IPV6 */ } else { #if LWIP_IPV4 struct sockaddr_in *sa4 = (struct sockaddr_in*)sa; /* set up sockaddr */ inet4_addr_from_ip4addr(&sa4->sin_addr, ip_2_ip4(&addr)); sa4->sin_family = AF_INET; sa4->sin_len = sizeof(struct sockaddr_in); sa4->sin_port = lwip_htons((u16_t)port_nr); ai->ai_family = AF_INET; #endif /* LWIP_IPV4 */ } /* set up addrinfo */ if (hints != NULL) { /* copy socktype & protocol from hints if specified */ ai->ai_socktype = hints->ai_socktype; ai->ai_protocol = hints->ai_protocol; } if (nodename != NULL) { /* copy nodename to canonname if specified */ ai->ai_canonname = ((char*)ai + sizeof(struct addrinfo) + sizeof(struct sockaddr_storage)); MEMCPY(ai->ai_canonname, nodename, namelen); ai->ai_canonname[namelen] = 0; } ai->ai_addrlen = sizeof(struct sockaddr_storage); ai->ai_addr = (struct sockaddr*)sa; *res = ai; return 0; }
/** * @ingroup lwip_nosys * Process received ethernet frames. Using this function instead of directly * calling ip_input and passing ARP frames through etharp in ethernetif_input, * the ARP cache is protected from concurrent access.\n * Don't call directly, pass to netif_add() and call netif->input(). * * @param p the received packet, p->payload pointing to the ethernet header * @param netif the network interface on which the packet was received * * @see LWIP_HOOK_UNKNOWN_ETH_PROTOCOL * @see ETHARP_SUPPORT_VLAN * @see LWIP_HOOK_VLAN_CHECK */ err_t ethernet_input(struct pbuf *p, struct netif *netif) { struct eth_hdr* ethhdr; u16_t type; #if LWIP_ARP || ETHARP_SUPPORT_VLAN || LWIP_IPV6 s16_t ip_hdr_offset = SIZEOF_ETH_HDR; #endif /* LWIP_ARP || ETHARP_SUPPORT_VLAN */ if (p->len <= SIZEOF_ETH_HDR) { /* a packet with only an ethernet header (or less) is not valid for us */ ETHARP_STATS_INC(etharp.proterr); ETHARP_STATS_INC(etharp.drop); MIB2_STATS_NETIF_INC(netif, ifinerrors); goto free_and_return; } /* points to packet payload, which starts with an Ethernet header */ ethhdr = (struct eth_hdr *)p->payload; LWIP_DEBUGF(ETHARP_DEBUG | LWIP_DBG_TRACE, ("ethernet_input: dest:%"X8_F":%"X8_F":%"X8_F":%"X8_F":%"X8_F":%"X8_F", src:%"X8_F":%"X8_F":%"X8_F":%"X8_F":%"X8_F":%"X8_F", type:%"X16_F"\n", (unsigned)ethhdr->dest.addr[0], (unsigned)ethhdr->dest.addr[1], (unsigned)ethhdr->dest.addr[2], (unsigned)ethhdr->dest.addr[3], (unsigned)ethhdr->dest.addr[4], (unsigned)ethhdr->dest.addr[5], (unsigned)ethhdr->src.addr[0], (unsigned)ethhdr->src.addr[1], (unsigned)ethhdr->src.addr[2], (unsigned)ethhdr->src.addr[3], (unsigned)ethhdr->src.addr[4], (unsigned)ethhdr->src.addr[5], lwip_htons(ethhdr->type))); type = ethhdr->type; #if ETHARP_SUPPORT_VLAN if (type == PP_HTONS(ETHTYPE_VLAN)) { struct eth_vlan_hdr *vlan = (struct eth_vlan_hdr*)(((char*)ethhdr) + SIZEOF_ETH_HDR); if (p->len <= SIZEOF_ETH_HDR + SIZEOF_VLAN_HDR) { /* a packet with only an ethernet/vlan header (or less) is not valid for us */ ETHARP_STATS_INC(etharp.proterr); ETHARP_STATS_INC(etharp.drop); MIB2_STATS_NETIF_INC(netif, ifinerrors); goto free_and_return; } #if defined(LWIP_HOOK_VLAN_CHECK) || defined(ETHARP_VLAN_CHECK) || defined(ETHARP_VLAN_CHECK_FN) /* if not, allow all VLANs */ #ifdef LWIP_HOOK_VLAN_CHECK if (!LWIP_HOOK_VLAN_CHECK(netif, ethhdr, vlan)) { #elif defined(ETHARP_VLAN_CHECK_FN) if (!ETHARP_VLAN_CHECK_FN(ethhdr, vlan)) { #elif defined(ETHARP_VLAN_CHECK) if (VLAN_ID(vlan) != ETHARP_VLAN_CHECK) { #endif /* silently ignore this packet: not for our VLAN */ pbuf_free(p); return ERR_OK; } #endif /* defined(LWIP_HOOK_VLAN_CHECK) || defined(ETHARP_VLAN_CHECK) || defined(ETHARP_VLAN_CHECK_FN) */ type = vlan->tpid; ip_hdr_offset = SIZEOF_ETH_HDR + SIZEOF_VLAN_HDR; } #endif /* ETHARP_SUPPORT_VLAN */ #if LWIP_ARP_FILTER_NETIF netif = LWIP_ARP_FILTER_NETIF_FN(p, netif, lwip_htons(type)); #endif /* LWIP_ARP_FILTER_NETIF*/ if (ethhdr->dest.addr[0] & 1) { /* this might be a multicast or broadcast packet */ if (ethhdr->dest.addr[0] == LL_IP4_MULTICAST_ADDR_0) { #if LWIP_IPV4 if ((ethhdr->dest.addr[1] == LL_IP4_MULTICAST_ADDR_1) && (ethhdr->dest.addr[2] == LL_IP4_MULTICAST_ADDR_2)) { /* mark the pbuf as link-layer multicast */ p->flags |= PBUF_FLAG_LLMCAST; } #endif /* LWIP_IPV4 */ } #if LWIP_IPV6 else if ((ethhdr->dest.addr[0] == LL_IP6_MULTICAST_ADDR_0) && (ethhdr->dest.addr[1] == LL_IP6_MULTICAST_ADDR_1)) { /* mark the pbuf as link-layer multicast */ p->flags |= PBUF_FLAG_LLMCAST; } #endif /* LWIP_IPV6 */ else if (eth_addr_cmp(ðhdr->dest, ðbroadcast)) { /* mark the pbuf as link-layer broadcast */ p->flags |= PBUF_FLAG_LLBCAST; } } switch (type) { #if LWIP_IPV4 && LWIP_ARP /* IP packet? */ case PP_HTONS(ETHTYPE_IP): if (!(netif->flags & NETIF_FLAG_ETHARP)) { goto free_and_return; } /* skip Ethernet header */ if ((p->len < ip_hdr_offset) || pbuf_header(p, (s16_t)-ip_hdr_offset)) { LWIP_DEBUGF(ETHARP_DEBUG | LWIP_DBG_TRACE | LWIP_DBG_LEVEL_WARNING, ("ethernet_input: IPv4 packet dropped, too short (%"S16_F"/%"S16_F")\n", p->tot_len, ip_hdr_offset)); LWIP_DEBUGF(ETHARP_DEBUG | LWIP_DBG_TRACE, ("Can't move over header in packet")); goto free_and_return; } else { /* pass to IP layer */ ip4_input(p, netif); } break; case PP_HTONS(ETHTYPE_ARP): if (!(netif->flags & NETIF_FLAG_ETHARP)) { goto free_and_return; } /* skip Ethernet header */ if ((p->len < ip_hdr_offset) || pbuf_header(p, (s16_t)-ip_hdr_offset)) { LWIP_DEBUGF(ETHARP_DEBUG | LWIP_DBG_TRACE | LWIP_DBG_LEVEL_WARNING, ("ethernet_input: ARP response packet dropped, too short (%"S16_F"/%"S16_F")\n", p->tot_len, ip_hdr_offset)); LWIP_DEBUGF(ETHARP_DEBUG | LWIP_DBG_TRACE, ("Can't move over header in packet")); ETHARP_STATS_INC(etharp.lenerr); ETHARP_STATS_INC(etharp.drop); goto free_and_return; } else { /* pass p to ARP module */ etharp_input(p, netif); } break; #endif /* LWIP_IPV4 && LWIP_ARP */ #if PPPOE_SUPPORT case PP_HTONS(ETHTYPE_PPPOEDISC): /* PPP Over Ethernet Discovery Stage */ pppoe_disc_input(netif, p); break; case PP_HTONS(ETHTYPE_PPPOE): /* PPP Over Ethernet Session Stage */ pppoe_data_input(netif, p); break; #endif /* PPPOE_SUPPORT */ #if LWIP_IPV6 case PP_HTONS(ETHTYPE_IPV6): /* IPv6 */ /* skip Ethernet header */ if ((p->len < ip_hdr_offset) || pbuf_header(p, (s16_t)-ip_hdr_offset)) { LWIP_DEBUGF(ETHARP_DEBUG | LWIP_DBG_TRACE | LWIP_DBG_LEVEL_WARNING, ("ethernet_input: IPv6 packet dropped, too short (%"S16_F"/%"S16_F")\n", p->tot_len, ip_hdr_offset)); goto free_and_return; } else { /* pass to IPv6 layer */ ip6_input(p, netif); } break; #endif /* LWIP_IPV6 */ default: #ifdef LWIP_HOOK_UNKNOWN_ETH_PROTOCOL if(LWIP_HOOK_UNKNOWN_ETH_PROTOCOL(p, netif) == ERR_OK) { break; } #endif ETHARP_STATS_INC(etharp.proterr); ETHARP_STATS_INC(etharp.drop); MIB2_STATS_NETIF_INC(netif, ifinunknownprotos); goto free_and_return; } /* This means the pbuf is freed or consumed, so the caller doesn't have to free it again */ return ERR_OK; free_and_return: pbuf_free(p); return ERR_OK; } /** * @ingroup ethernet * Send an ethernet packet on the network using netif->linkoutput(). * The ethernet header is filled in before sending. * * @see LWIP_HOOK_VLAN_SET * * @param netif the lwIP network interface on which to send the packet * @param p the packet to send. pbuf layer must be @ref PBUF_LINK. * @param src the source MAC address to be copied into the ethernet header * @param dst the destination MAC address to be copied into the ethernet header * @param eth_type ethernet type (@ref eth_type) * @return ERR_OK if the packet was sent, any other err_t on failure */ err_t ethernet_output(struct netif* netif, struct pbuf* p, const struct eth_addr* src, const struct eth_addr* dst, u16_t eth_type) { struct eth_hdr* ethhdr; u16_t eth_type_be = lwip_htons(eth_type); #if ETHARP_SUPPORT_VLAN && defined(LWIP_HOOK_VLAN_SET) s32_t vlan_prio_vid = LWIP_HOOK_VLAN_SET(netif, p, src, dst, eth_type); if (vlan_prio_vid >= 0) { struct eth_vlan_hdr* vlanhdr; LWIP_ASSERT("prio_vid must be <= 0xFFFF", vlan_prio_vid <= 0xFFFF); if (pbuf_header(p, SIZEOF_ETH_HDR + SIZEOF_VLAN_HDR) != 0) { goto pbuf_header_failed; } vlanhdr = (struct eth_vlan_hdr*)(((u8_t*)p->payload) + SIZEOF_ETH_HDR); vlanhdr->tpid = eth_type_be; vlanhdr->prio_vid = lwip_htons((u16_t)vlan_prio_vid); eth_type_be = PP_HTONS(ETHTYPE_VLAN); } else #endif /* ETHARP_SUPPORT_VLAN && defined(LWIP_HOOK_VLAN_SET) */ { if (pbuf_header(p, SIZEOF_ETH_HDR) != 0) { goto pbuf_header_failed; } } ethhdr = (struct eth_hdr*)p->payload; ethhdr->type = eth_type_be; ETHADDR32_COPY(ðhdr->dest, dst); ETHADDR16_COPY(ðhdr->src, src); LWIP_ASSERT("netif->hwaddr_len must be 6 for ethernet_output!", (netif->hwaddr_len == ETH_HWADDR_LEN)); LWIP_DEBUGF(ETHARP_DEBUG | LWIP_DBG_TRACE, ("ethernet_output: sending packet %p\n", (void *)p)); /* send the packet */ return netif->linkoutput(netif, p); pbuf_header_failed: LWIP_DEBUGF(ETHARP_DEBUG | LWIP_DBG_TRACE | LWIP_DBG_LEVEL_SERIOUS, ("ethernet_output: could not allocate room for header.\n")); LINK_STATS_INC(link.lenerr); return ERR_BUF; }
/** * Fragment an IPv6 datagram if too large for the netif or path MTU. * * Chop the datagram in MTU sized chunks and send them in order * by pointing PBUF_REFs into p * * @param p ipv6 packet to send * @param netif the netif on which to send * @param dest destination ipv6 address to which to send * * @return ERR_OK if sent successfully, err_t otherwise */ err_t ip6_frag(struct pbuf *p, struct netif *netif, const ip6_addr_t *dest) { struct ip6_hdr *original_ip6hdr; struct ip6_hdr *ip6hdr; struct ip6_frag_hdr *frag_hdr; struct pbuf *rambuf; #if !LWIP_NETIF_TX_SINGLE_PBUF struct pbuf *newpbuf; u16_t newpbuflen = 0; u16_t left_to_copy; #endif static u32_t identification; u16_t nfb; u16_t left, cop; u16_t mtu; u16_t fragment_offset = 0; u16_t last; u16_t poff = IP6_HLEN; identification++; original_ip6hdr = (struct ip6_hdr *)p->payload; mtu = nd6_get_destination_mtu(dest, netif); /* @todo we assume there are no options in the unfragmentable part (IPv6 header). */ left = p->tot_len - IP6_HLEN; nfb = (mtu - (IP6_HLEN + IP6_FRAG_HLEN)) & IP6_FRAG_OFFSET_MASK; while (left) { last = (left <= nfb); /* Fill this fragment */ cop = last ? left : nfb; #if LWIP_NETIF_TX_SINGLE_PBUF rambuf = pbuf_alloc(PBUF_IP, cop + IP6_FRAG_HLEN, PBUF_RAM); if (rambuf == NULL) { IP6_FRAG_STATS_INC(ip6_frag.memerr); return ERR_MEM; } LWIP_ASSERT("this needs a pbuf in one piece!", (rambuf->len == rambuf->tot_len) && (rambuf->next == NULL)); poff += pbuf_copy_partial(p, (u8_t*)rambuf->payload + IP6_FRAG_HLEN, cop, poff); /* make room for the IP header */ if (pbuf_header(rambuf, IP6_HLEN)) { pbuf_free(rambuf); IP6_FRAG_STATS_INC(ip6_frag.memerr); return ERR_MEM; } /* fill in the IP header */ SMEMCPY(rambuf->payload, original_ip6hdr, IP6_HLEN); ip6hdr = (struct ip6_hdr *)rambuf->payload; frag_hdr = (struct ip6_frag_hdr *)((u8_t*)rambuf->payload + IP6_HLEN); #else /* When not using a static buffer, create a chain of pbufs. * The first will be a PBUF_RAM holding the link, IPv6, and Fragment header. * The rest will be PBUF_REFs mirroring the pbuf chain to be fragged, * but limited to the size of an mtu. */ rambuf = pbuf_alloc(PBUF_LINK, IP6_HLEN + IP6_FRAG_HLEN, PBUF_RAM); if (rambuf == NULL) { IP6_FRAG_STATS_INC(ip6_frag.memerr); return ERR_MEM; } LWIP_ASSERT("this needs a pbuf in one piece!", (p->len >= (IP6_HLEN))); SMEMCPY(rambuf->payload, original_ip6hdr, IP6_HLEN); ip6hdr = (struct ip6_hdr *)rambuf->payload; frag_hdr = (struct ip6_frag_hdr *)((u8_t*)rambuf->payload + IP6_HLEN); /* Can just adjust p directly for needed offset. */ p->payload = (u8_t *)p->payload + poff; p->len -= poff; p->tot_len -= poff; left_to_copy = cop; while (left_to_copy) { struct pbuf_custom_ref *pcr; newpbuflen = (left_to_copy < p->len) ? left_to_copy : p->len; /* Is this pbuf already empty? */ if (!newpbuflen) { p = p->next; continue; } pcr = ip6_frag_alloc_pbuf_custom_ref(); if (pcr == NULL) { pbuf_free(rambuf); IP6_FRAG_STATS_INC(ip6_frag.memerr); return ERR_MEM; } /* Mirror this pbuf, although we might not need all of it. */ newpbuf = pbuf_alloced_custom(PBUF_RAW, newpbuflen, PBUF_REF, &pcr->pc, p->payload, newpbuflen); if (newpbuf == NULL) { ip6_frag_free_pbuf_custom_ref(pcr); pbuf_free(rambuf); IP6_FRAG_STATS_INC(ip6_frag.memerr); return ERR_MEM; } pbuf_ref(p); pcr->original = p; pcr->pc.custom_free_function = ip6_frag_free_pbuf_custom; /* Add it to end of rambuf's chain, but using pbuf_cat, not pbuf_chain * so that it is removed when pbuf_dechain is later called on rambuf. */ pbuf_cat(rambuf, newpbuf); left_to_copy -= newpbuflen; if (left_to_copy) { p = p->next; } } poff = newpbuflen; #endif /* LWIP_NETIF_TX_SINGLE_PBUF */ /* Set headers */ frag_hdr->_nexth = original_ip6hdr->_nexth; frag_hdr->reserved = 0; frag_hdr->_fragment_offset = lwip_htons((fragment_offset & IP6_FRAG_OFFSET_MASK) | (last ? 0 : IP6_FRAG_MORE_FLAG)); frag_hdr->_identification = lwip_htonl(identification); IP6H_NEXTH_SET(ip6hdr, IP6_NEXTH_FRAGMENT); IP6H_PLEN_SET(ip6hdr, cop + IP6_FRAG_HLEN); /* No need for separate header pbuf - we allowed room for it in rambuf * when allocated. */ IP6_FRAG_STATS_INC(ip6_frag.xmit); netif->output_ip6(netif, rambuf, dest); /* Unfortunately we can't reuse rambuf - the hardware may still be * using the buffer. Instead we free it (and the ensuing chain) and * recreate it next time round the loop. If we're lucky the hardware * will have already sent the packet, the free will really free, and * there will be zero memory penalty. */ pbuf_free(rambuf); left -= cop; fragment_offset += cop; } return ERR_OK; }
/** * 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; }