RESULT ip_process( ip_header_p iph, SHORT len ) { DECLARE( SHORT, data_len, 0 ); DECLARE( BYTE, j, 0 ); DECLARE( WORD, dst, 0 ); if( len < sizeof( ip_header ) ) { return IP_HEADER_LEN_TOO_SMALL; } dst = R_STRUCT_VAR_TYPE( WORD, iph->dst ); if( dst != R_WORD( __IP, 0 ) ) { return IP_HEADER_DST_NOT_ME; } data_len = get_ip_data_len( iph ); if( data_len == 0 ) { return IP_HEADER_DATA_LEN_ZERO; } if( data_len > MAX_IP_DATA_LEN ) { return IP_HEADER_DATA_LEN_TOO_BIG; } if( data_len != len ) { return IP_HEADER_AND_ACTUAL_LEN_MISMATCH; } j = get_ip_header_len( iph ); if( j > data_len ) { return IP_HEADER_LEN_BIGGER_THAN_PKT; } for ( j = 0; j < sizeof( supported_ip_protocols ); j++ ) { if( supported_ip_protocols[ j ] == iph->protocol ) break; } if( j == sizeof( supported_ip_protocols ) ) { return IP_HEADER_PROTO_NOT_SUPPORTED; } return OK; }
uint8_t *build_ip_udp_pcket( uint8_t *orig_pkt, int orig_pkt_len, lisp_addr_t *addr_from, lisp_addr_t *addr_dest, int port_from, int port_dest, int *encap_pkt_len) { uint8_t *encap_pkt = NULL; void *iph_ptr = NULL; struct udphdr *udph_ptr = NULL; int ip_hdr_len = 0; int udp_hdr_len = 0; int udp_hdr_and_payload_len = 0; uint16_t udpsum = 0; if (addr_from->afi != addr_dest->afi) { lispd_log_msg(LISP_LOG_DEBUG_2, "add_ip_udp_header: Different AFI addresses"); return (NULL); } if ((addr_from->afi != AF_INET) && (addr_from->afi != AF_INET6)) { lispd_log_msg(LISP_LOG_DEBUG_2, "add_ip_udp_header: Unknown AFI %d", addr_from->afi); return (NULL); } /* Headers lengths */ ip_hdr_len = get_ip_header_len(addr_from->afi); udp_hdr_len = sizeof(struct udphdr); udp_hdr_and_payload_len = udp_hdr_len + orig_pkt_len; /* Assign memory for the original packet plus the new headers */ *encap_pkt_len = ip_hdr_len + udp_hdr_len + orig_pkt_len; if ((encap_pkt = (uint8_t *) malloc(*encap_pkt_len)) == NULL) { lispd_log_msg(LISP_LOG_DEBUG_2, "add_ip_udp_header: Couldn't allocate memory for the packet to be generated %s", strerror(errno)); return (NULL); } /* Make sure it's clean */ memset(encap_pkt, 0, *encap_pkt_len); /* IP header */ iph_ptr = encap_pkt; if ((udph_ptr = build_ip_header(iph_ptr, addr_from, addr_dest, udp_hdr_and_payload_len)) == NULL){ lispd_log_msg(LISP_LOG_DEBUG_2, "add_ip_udp_header: Couldn't build the inner ip header"); free (encap_pkt); return (NULL); } /* UDP header */ #ifdef BSD udph_ptr->uh_sport = htons(port_from); udph_ptr->uh_dport = htons(port_dest); udph_ptr->uh_ulen = htons(udp_payload_len); udph_ptr->uh_sum = 0; #else udph_ptr->source = htons(port_from); udph_ptr->dest = htons(port_dest); udph_ptr->len = htons(udp_hdr_and_payload_len); udph_ptr->check = 0; #endif /* Copy original packet after the headers */ memcpy(CO(udph_ptr, udp_hdr_len), orig_pkt, orig_pkt_len); /* * Now compute the headers checksums */ if ((udpsum = udp_checksum(udph_ptr, udp_hdr_and_payload_len, iph_ptr, addr_from->afi)) == -1) { free (encap_pkt); return (NULL); } udpsum(udph_ptr) = udpsum; return (encap_pkt); }