int pkt_push_udp_and_ip(lbuf_t *b, uint16_t sp, uint16_t dp, ip_addr_t *sip, ip_addr_t *dip) { uint16_t udpsum; struct udphdr *uh; if (pkt_push_udp(b, sp, dp) == NULL) { OOR_LOG(LDBG_1, "Failed to push UDP header! Discarding"); return(BAD); } lbuf_reset_udp(b); if (pkt_push_ip(b, sip, dip, IPPROTO_UDP) == NULL) { OOR_LOG(LDBG_1, "Failed to push IP header! Discarding"); return(BAD); } lbuf_reset_ip(b); uh = lbuf_udp(b); udpsum = udp_checksum(uh, ntohs(uh->len), lbuf_ip(b), ip_addr_afi(sip)); if (udpsum == -1) { OOR_LOG(LDBG_1, "Failed UDP checksum! Discarding"); return (BAD); } udpsum(uh) = udpsum; return(GOOD); }
void rewriteIpv4( char* argv, int len, char* ipv4 ) { PETH_HDR ether; PIP_HDR ip; PTCP_HDR tcp; PUDP_HDR udp; int ulen; ether = (PETH_HDR)argv; // IPv4 if ( ntohs( ether->ether_type ) == ETHER_TYPE_IP ) { #ifdef DEBUG printf("rewrite.c - IPv4 rewrite\n"); #endif if ( checkPacketNotLen( len, ( ETHER_HDRLEN + IP_HDRLEN ) ) ) return; ip = (PIP_HDR)(argv + ETHER_HDRLEN); inet_pton( AF_INET, ipv4, &(ip->ip_saddr) ); ip->ip_checksum = 0; ip->ip_checksum = ip_checksum( ip, ( ip->ip_ihl * 4 ) ); #ifdef DEBUG printf("rewrite.c - IPv4 checksum recalculated = %x\n", ntohs( ip->ip_checksum ) ); #endif // TCP checksum if ( ip->ip_protocol == IPPROTO_TCP ) { ulen = len - ETHER_HDRLEN - ( ip->ip_ihl * 4 ); if ( checkPacketNotLen( len, ( ETHER_HDRLEN + ( ip->ip_ihl * 4 ) + TCP_HDRLEN ) ) ) return; tcp = (PTCP_HDR)(argv + ETHER_HDRLEN + ( ip->ip_ihl * 4 ) ); tcp->tcp_checksum = 0; tcp->tcp_checksum = tcp_checksum( tcp, ulen, ip->ip_saddr, ip->ip_daddr ); #ifdef DEBUG printf("rewrite.c - TCPv4 checksum recalculated = %x\n", ntohs( tcp->tcp_checksum ) ); #endif } // UDP checksum if ( ip->ip_protocol == IPPROTO_UDP ) { if ( checkPacketNotLen( len, ( ETHER_HDRLEN + ( ip->ip_ihl * 4 ) + UDP_HDRLEN ) ) ) return; ulen = len - ETHER_HDRLEN - ( ip->ip_ihl * 4 ); udp = (PUDP_HDR)(argv + ETHER_HDRLEN + ( ip->ip_ihl * 4 ) ); udp->udp_checksum = 0; udp->udp_checksum = udp_checksum( udp, ulen, ip->ip_saddr, ip->ip_daddr ); #ifdef DEBUG printf("rewrite.c - UDPv4 checksum recalculated = %x\n", ntohs( udp->udp_checksum ) ); #endif } } }
static inline void udps_write(TCPIPS* tcpips, HANDLE handle, IO* io) { IO* cur; unsigned int offset, size; unsigned short remote_port; IP dst; UDP_STACK* udp_stack; UDP_HEADER* udp; UDP_HANDLE* uh = so_get(&tcpips->udps.handles, handle); if (uh == NULL) return; #if (ICMP) if (uh->err != ERROR_OK) { error(uh->err); return; } #endif //ICMP //listening socket if (uh->remote_port == 0) { if (io->stack_size < sizeof(UDP_STACK)) { error(ERROR_INVALID_PARAMS); return; } udp_stack = io_stack(io); remote_port = udp_stack->remote_port; dst.u32.ip = udp_stack->remote_addr.u32.ip; io_pop(io, sizeof(UDP_STACK)); } else { remote_port = uh->remote_port; dst.u32.ip = uh->remote_addr.u32.ip; } for (offset = 0; offset < io->data_size; offset += size) { size = UDP_FRAME_MAX_DATA_SIZE; if (size > io->data_size - offset) size = io->data_size - offset; cur = ips_allocate_io(tcpips, size + sizeof(UDP_HEADER), PROTO_UDP); if (cur == NULL) return; //copy data memcpy((uint8_t*)io_data(cur) + sizeof(UDP_HEADER), (uint8_t*)io_data(io) + offset, size); udp = io_data(cur); // correct size cur->data_size = size + sizeof(UDP_HEADER); //format header short2be(udp->src_port_be, uh->local_port); short2be(udp->dst_port_be, remote_port); short2be(udp->len_be, size + sizeof(UDP_HEADER)); short2be(udp->checksum_be, 0); short2be(udp->checksum_be, udp_checksum(io_data(cur), cur->data_size, &tcpips->ips.ip, &dst)); ips_tx(tcpips, cur, &dst); } }
/* Process encapsulated map request header: lisp header and the interal IP and * UDP header */ int lisp_msg_ecm_decap(lbuf_t *pkt, uint16_t *src_port) { uint16_t ipsum = 0; uint16_t udpsum = 0; int udp_len = 0; struct udphdr *udph; struct ip *iph; /* this is the new start of the packet */ lisp_msg_pull_ecm_hdr(pkt); /* Set and extract inner layer 3 packet */ lbuf_reset_l3(pkt); iph = pkt_pull_ip(pkt); /* Set and extract inner layer 4 packet */ lbuf_reset_l4(pkt); udph = pkt_pull_udp(pkt); /* Set the beginning of the LISP msg*/ lbuf_reset_lisp(pkt); /* This should overwrite the external port (dst_port in map-reply = * inner src_port in encap map-request) */ *src_port = ntohs(udph->source); #ifdef BSD udp_len = ntohs(udph->uh_ulen); #else udp_len = ntohs(udph->len); #endif /* Verify the checksums. */ if (iph->ip_v == IPVERSION) { ipsum = ip_checksum((uint16_t *) iph, sizeof(struct ip)); if (ipsum != 0) { OOR_LOG(LDBG_2, "IP checksum failed."); } } /* Verify UDP checksum only if different from 0. * This means we ACCEPT UDP checksum 0! */ if (udph->check != 0) { udpsum = udp_checksum(udph, udp_len, iph, ip_version_to_sock_afi(iph->ip_v)); if (udpsum != 0) { OOR_LOG(LDBG_2, "UDP checksum failed."); return (BAD); } } OOR_LOG(LDBG_2, "%s, inner IP: %s -> %s, inner UDP: %d -> %d", lisp_msg_hdr_to_char(pkt), ip_to_char(&iph->ip_src, ip_version_to_sock_afi(iph->ip_v)), ip_to_char(&iph->ip_dst, ip_version_to_sock_afi(iph->ip_v)), ntohs(udph->source), ntohs(udph->dest)); return (GOOD); }
void forge_udphdr(t_env *env, t_udp_packet *packet, uint16_t port, int pton_addr) { ft_memset(&packet->udp_header, 0, sizeof(packet->udp_header)); packet->udp_header.source = htons(env->port); packet->udp_header.dest = htons(port); packet->udp_header.len = htons(sizeof(t_udp_packet) - sizeof(packet->ip_header)); packet->udp_header.check = udp_checksum(packet, pton_addr); }
void print_udp_header(UDP *ud,u_char_t *src,u_char_t *dst) { kprintf("\n"); kprintf("UDP Header\n"); kprintf(" |-Source Port : %u\n",ntohs_tos(ud->src_port)); kprintf(" |-Destination Port : %u\n",ntohs_tos(ud->dst_port)); kprintf(" |-Length : %u\n",ntohs_tos(ud->len)); kprintf(" |-UDP checksum : %04x\n",ntohs_tos(ud->checksum)); kprintf(" |-Computed UDP checksum : %04x\n",ntohs_tos(udp_checksum(ud,src,dst))); }
int send_pktbuff(pktbuff *pktbuff_out, struct pcb *pcb, struct thread_context *context, struct worker_data *data) { int rv; uint16_t len; // TODO: build layers based on pcb or pktbuff layer data // complete the lower layers and send pktbuff len = ethernet_build_header(&context->shared->if_info->mac, &pcb->remote_mac, IP4_ETHERTYPE, 0xFFFF, pktbuff_out->data); len += ip4_build_header(context->shared->inet_info->addr.s_addr, FLOWID_REMOTE_IP(pcb->flowid), pktbuff_out->len, pcb->ipproto, NULL, FCL_PTR_PAST(pktbuff_out->data, len)); pktbuff_out->len += len; // fixup layer 3 checksums struct ip4_pkt *send_ip = FCL_PTR_PAST(pktbuff_out->data, pcb_layer_offset(pcb, PCB_LAYER_IP4)); struct tcp_pkt *tcp; struct udp_pkt *udp; switch(pcb->ipproto) { case IPPROTO_TCP: tcp = ip4_get_next(send_ip); tcp->tcp_h.th_sum = tcp_checksum(send_ip); break; case IPPROTO_UDP: udp = ip4_get_next(send_ip); udp->udp_h.uh_sum = udp_checksum(send_ip); break; } // and finally queue the pktbuff for sending rv = tqueue_insert(context->pkt_xmit_q, &data->xmit_transaction, pktbuff_out); // xmit packet without waiting to fill a transaction if (rv == TQUEUE_SUCCESS) { tqueue_publish_transaction(context->pkt_xmit_q, &data->xmit_transaction); return 1; } return -1; }
static void udps_replay(TCPIPS* tcpips, IO* io, const IP* src) { UDP_HEADER* udp; IP dst; uint16_t src_port; dst.u32.ip = src->u32.ip; io_unhide(io, sizeof(UDP_HEADER)); udp = io_data(io); //format header src_port = be2short(udp->dst_port_be); udp->dst_port_be[0] = udp->src_port_be[0]; udp->dst_port_be[1] = udp->src_port_be[1]; short2be(udp->src_port_be, src_port); short2be(udp->len_be, io->data_size); short2be(udp->checksum_be, 0); short2be(udp->checksum_be, udp_checksum(io_data(io), io->data_size, &tcpips->ips.ip, &dst)); ips_tx(tcpips, io, &dst); }
/* * send_map_reply() * * Send a map-reply to a querier. */ void send_map_reply(lispd_pkt_map_request_t *pkt, struct sockaddr_in *source) { lispd_pkt_map_request_eid_prefix_record_t *rec; lispd_pkt_map_request_itr_rloc_t *itr_rloc; lispd_locator_chain_t *loc_chain; lispd_pkt_map_reply_t *reply_pkt; struct ip *iphdr; struct udphdr *udphdr; lisp_addr_t eid_prefix; struct sockaddr_in dst; lispd_if_t *oif = get_primary_interface(); char addr_buf[128]; char *ptr; int offset, i, s, len, nbytes, udp_len; if (!oif) { log_msg(INFO, "No interfaces are available to source reply"); return; } /* * Figure out what they are asking for, need to advance past * all the ITR-RLOCs in the packet. Sigh. */ offset = sizeof(lispd_pkt_map_request_t); if (pkt->source_eid_afi != 0) { if (lisp2inetafi(ntohs(pkt->source_eid_afi)) == AF_INET) { offset += sizeof(struct in_addr); } else { offset += sizeof(struct in6_addr); } } itr_rloc = (lispd_pkt_map_request_itr_rloc_t *)CO(pkt, offset); for (i = 0; i < pkt->additional_itr_rloc_count + 1; i++) { offset += sizeof(lispd_pkt_map_request_itr_rloc_t); if (lisp2inetafi(ntohs(itr_rloc->afi)) == AF_INET) { offset += sizeof(struct in_addr); } else { offset += sizeof(struct in6_addr); } itr_rloc = (lispd_pkt_map_request_itr_rloc_t *)CO(pkt, offset); } rec = (lispd_pkt_map_request_eid_prefix_record_t *)CO(pkt, offset); ptr = CO(rec, sizeof(lispd_pkt_map_request_eid_prefix_record_t)); memcpy(&eid_prefix.address.ip, ptr, sizeof(struct in_addr)); if (lisp2inetafi(ntohs(rec->eid_prefix_afi)) != AF_INET) { log_msg(INFO, " AF: %d unsupported currently.", lisp2inetafi(ntohs(rec->eid_prefix_afi))); return; } log_msg(INFO, " Request for EID: %s/%d", inet_ntop(AF_INET, &eid_prefix.address.ip.s_addr, addr_buf, 128), rec->eid_prefix_mask_length); /* * Lookup in our local database */ if (!lookup_eid_in_db(lisp2inetafi(ntohs(rec->eid_prefix_afi)), eid_prefix.address.ip.s_addr, &loc_chain)) { log_msg(INFO, " Unable to find entry in local database."); return; } else { log_msg(INFO, " Found entry, building reply."); } reply_pkt = build_map_reply(source->sin_addr.s_addr, pkt->rloc_probe, loc_chain, (char *)&pkt->nonce, &len); if (!reply_pkt) { log_msg(INFO, "Failed to build map reply"); return; } /* * Build the outer IP header ourselves, this doesn't * go using LISP, so we don't want the EID used as the source. */ iphdr = (struct ip *)malloc(len + sizeof(struct ip) + sizeof(struct udphdr)); udphdr = (struct udphdr *)CO(iphdr, sizeof(struct ip)); ptr = CO(udphdr, sizeof(struct udphdr)); memcpy(ptr, (char *)reply_pkt, len); free(reply_pkt); udp_len = len + sizeof(struct udphdr); /* * AF_INET for now XXX */ iphdr->ip_hl = 5; iphdr->ip_v = IPVERSION; iphdr->ip_tos = 0; iphdr->ip_len = htons(udp_len + sizeof(struct ip)); iphdr->ip_id = htons(54321); iphdr->ip_off = 0; iphdr->ip_ttl = 255; iphdr->ip_p = IPPROTO_UDP; iphdr->ip_sum = 0; // Raw socket handler does this for us. iphdr->ip_src.s_addr = oif->address.address.ip.s_addr; /// XXX Huh? iphdr->ip_dst.s_addr = source->sin_addr.s_addr; udphdr->source = htons(LISP_CONTROL_PORT); udphdr->dest = source->sin_port; udphdr->len = htons(udp_len); udphdr->check = 0; udphdr->check = udp_checksum(udphdr, udp_len, iphdr, AF_INET); /* XXX: assumes v4 transport */ if ((s = socket(AF_INET, SOCK_RAW, IPPROTO_RAW)) < 0) { log_msg(INFO, "socket (send_map_request): %s", strerror(errno)); return; } memset((char *) &dst, 0, sizeof(dst)); dst.sin_family = AF_INET; /* XXX: assume v4 transport */ dst.sin_addr.s_addr = source->sin_addr.s_addr; if ((nbytes = sendto(s, (const void *)iphdr, len + sizeof(struct ip) + sizeof(struct udphdr), 0, (struct sockaddr *)&dst, sizeof(struct sockaddr))) < 0) { log_msg(INFO, "sendto (send_map_request): %s", strerror(errno)); return; } if (nbytes != (int)(len + sizeof(struct udphdr) + sizeof(struct ip))) { log_msg(INFO, "send_map_request: nbytes (%d) != packet_len (%d)\n", nbytes, len); return; } close(s); free(iphdr); return; }
/* * func: restruct a new udp package. * param: * return: 0 success; -1 fail */ int restruct_pkt(char *buf, struct eth_hdr *pethh, struct ip_hdr *piph, int iph_len, struct udp_hdr *pudph, uint32_t multi_ip, uint16_t multi_port) { char errbuf[PCAP_ERRBUF_SIZE] = {0}; // char * device = "eth0"; char device [DEV_BUF_SIZE]; pcap_t *adhandle = NULL; int ret; char *str_dev = get_dev_name(piph->ip_destaddr, device); if(str_dev == NULL) { fprintf(gfp_log, "[%s:%d]get_dev_name is null!\n", __FILE__, __LINE__); fflush(gfp_log); return -1; } if((adhandle = pcap_open_live(device, 0x10000, PCAP_OPENFLAG_PROMISCUOUS, 1000, errbuf)) == NULL) { fprintf(gfp_log, "[%s:%d][pcap_open_live error] : %s\n", __FILE__, __LINE__, errbuf); fflush(gfp_log); return -1; } pethh->ether_dhost[0] = 0x01; pethh->ether_dhost[1] = 0x00; pethh->ether_dhost[2] = 0x5e; pethh->ether_dhost[3] = (unsigned char)((multi_ip >> 8) & 0x7F);; pethh->ether_dhost[4] = (unsigned char)((multi_ip >> 16) & 0xFF);; pethh->ether_dhost[5] = (unsigned char)((multi_ip >> 24) & 0xFF);; //AC:85:3D:AF:C7:08 pethh->ether_shost[0] = 0x00; pethh->ether_shost[1] = 0x16; pethh->ether_shost[2] = 0x3e; pethh->ether_shost[3] = 0x00; pethh->ether_shost[4] = 0x04; pethh->ether_shost[5] = 0x0e; pethh->ether_type = htons(ETHERTYPE_IP); if((sizeof(struct ip_hdr) % 4) != 0) { fprintf(gfp_log, "[%s:%d][IP Header error]\n", __FILE__, __LINE__); fflush(gfp_log); pcap_close(adhandle); return -1; } uint16_t ip_len = ntohs(piph->ip_totallength); uint16_t udp_len = htons(pudph->udp_length); piph->ip_tos = 0; piph->ip_id = htons(0x0000); piph->ip_offset = htons(0x4000); piph->ip_ttl = 0x20; piph->ip_checksum = 0; piph->ip_destaddr = multi_ip; piph->ip_totallength = htons(ip_len); piph->ip_checksum = checksum((uint16_t *)piph, iph_len); pudph->dest_portno = htons(multi_port); pudph->src_portno = htons(52540); pudph->udp_checksum = 0; pudph->udp_length = htons(udp_len); pudph->udp_checksum = udp_checksum(piph, iph_len, udp_len); //pudph->udp_checksum = htons(0x16ed); //////just for test printf //printf("rebuild multicast package: \n"); //printf(" src : %s" , inet_ntoa(*((struct in_addr *)(&piph->ip_srcaddr)))); //printf(" dst : %s\n" , inet_ntoa(*((struct in_addr *)(&piph->ip_destaddr)))); /////////////////////////////////////////////////// int pkt_len = ip_len + ETHER_HEADER_SIZE; /* just print the pkt info */ /* int i=0; for(i = 0 ; i != sizeof(struct ip_hdr); i++) { printf("%02x ",(*((char *)piph+i))&0xff); } */ //printf("pkt len %u\n", pkt_len); ret = write(tun, piph, ip_len); if(pcap_sendpacket(adhandle, (const u_char*)buf, pkt_len) == -1) { fprintf(gfp_log, "[%s:%d][pcap_sendpacket error]\n", __FILE__, __LINE__); fflush(gfp_log); pcap_close(adhandle); return -1; } else { // printf("=====send a pkt!\n"); } pcap_close(adhandle); return 0; }
int process_encapsulated_map_request_headers( uint8_t *packet, int *len, uint16_t *dst_port){ struct ip *iph = NULL; struct ip6_hdr *ip6h = NULL; struct udphdr *udph = NULL; int ip_header_len = 0; int encap_afi = 0; uint16_t udpsum = 0; uint16_t ipsum = 0; int udp_len = 0; /* * Read IP header.source_mapping */ iph = (struct ip *) CO(packet, sizeof(lisp_encap_control_hdr_t)); switch (iph->ip_v) { case IPVERSION: ip_header_len = sizeof(struct ip); udph = (struct udphdr *) CO(iph, ip_header_len); encap_afi = AF_INET; break; case IP6VERSION: ip6h = (struct ip6_hdr *) CO(packet, sizeof(lisp_encap_control_hdr_t)); ip_header_len = sizeof(struct ip6_hdr); udph = (struct udphdr *) CO(ip6h, ip_header_len); encap_afi = AF_INET6; break; default: lispd_log_msg(LISP_LOG_DEBUG_2, "process_map_request_msg: couldn't read incoming Encapsulated Map-Request: IP header corrupted."); return(BAD); } /* This should overwrite the external port (dst_port in map-reply = inner src_port in encap map-request) */ *dst_port = ntohs(udph->source); #ifdef BSD udp_len = ntohs(udph->uh_ulen); // sport = ntohs(udph->uh_sport); #else udp_len = ntohs(udph->len); // sport = ntohs(udph->source); #endif /* * Verify the checksums. */ if (iph->ip_v == IPVERSION) { ipsum = ip_checksum((uint16_t *)iph, ip_header_len); if (ipsum != 0) { lispd_log_msg(LISP_LOG_DEBUG_2, "process_map_request_msg: Map-Request: IP checksum failed."); } /* We accept checksum 0 in the inner header*/ if (udph->check != 0){ if ((udpsum = udp_checksum(udph, udp_len, iph, encap_afi)) == -1) { return(BAD); } if (udpsum != 0) { lispd_log_msg(LISP_LOG_DEBUG_2, "process_map_request_msg: Map-Request: UDP checksum failed."); return(BAD); } } } //Pranathi: Added this if (iph->ip_v == IP6VERSION) { /* We accept checksum 0 in the inner header*/ if (udph->check != 0){ if ((udpsum = udp_checksum(udph, udp_len, iph, encap_afi)) == -1) { return(BAD); } if (udpsum != 0) { lispd_log_msg(LISP_LOG_DEBUG_2, "process_map_request_msg: Map-Request:v6 UDP checksum failed."); return(BAD); } } } *len = sizeof(lisp_encap_control_hdr_t)+ip_header_len + sizeof(struct udphdr); return (GOOD); }
int encapsulate_packet( char *original_packet, int original_packet_length, lisp_addr_t *src_addr, lisp_addr_t *dst_addr, int src_port, int dst_port, int iid, char **encap_packet, int *encap_packet_size) { int extra_headers_size = 0; char *new_packet = NULL; struct udphdr *udh = NULL; int encap_afi = 0; int iphdr_len = 0; int udphdr_len = 0; int lisphdr_len = 0; encap_afi = src_addr->afi; switch (encap_afi){ case AF_INET: iphdr_len = sizeof(struct iphdr); break; case AF_INET6: iphdr_len = sizeof(struct ip6_hdr); break; } udphdr_len = sizeof(struct udphdr); lisphdr_len = sizeof(struct lisphdr); extra_headers_size = iphdr_len + udphdr_len + lisphdr_len; new_packet = (char *) malloc (original_packet_length + extra_headers_size); if (new_packet == NULL){ lispd_log_msg(LISP_LOG_WARNING, "encapsulate_packet: Unable to allocate memory for encapsulated packet: %s", strerror(errno)); return (BAD); } memset(new_packet,0,original_packet_length+extra_headers_size); memcpy (new_packet + extra_headers_size, original_packet, original_packet_length); add_lisp_header((char *)(new_packet + iphdr_len + udphdr_len), iid); add_udp_header((char *)(new_packet + iphdr_len),original_packet_length+lisphdr_len,src_port,dst_port); // switch (encap_afi){ // case AF_INET: // add_ip_header(new_packet, // original_packet, // original_packet_length+lisphdr_len+udphdr_len, // src_addr, // dst_addr); // break; // case AF_INET6: // //arnatal TODO: write IPv6 support // break; // } add_ip_header(new_packet, original_packet, original_packet_length+lisphdr_len+udphdr_len, src_addr, dst_addr); /* UDP checksum mandatory for IPv6. Could be skipped if check disabled on receiver */ udh = (struct udphdr *)(new_packet + iphdr_len); udh->check = udp_checksum(udh,ntohs(udh->len),new_packet,encap_afi); *encap_packet = new_packet; *encap_packet_size = extra_headers_size + original_packet_length; lispd_log_msg(LISP_LOG_DEBUG_3,"OUTPUT: Encap src: %s | Encap dst: %s\n", get_char_from_lisp_addr_t(*src_addr),get_char_from_lisp_addr_t(*dst_addr)); return (GOOD); }
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; void *iph_ptr; struct udphdr *udph_ptr; int ip_hdr_len; int udp_hdr_len; int udp_hdr_and_payload_len; uint16_t udpsum; if (lisp_addr_ip_afi(addr_from) != lisp_addr_ip_afi(addr_dest)) { OOR_LOG(LDBG_2, "add_ip_udp_header: Different AFI addresses %d (%s) and %d (%s)", lisp_addr_ip_afi(addr_from), lisp_addr_to_char(addr_from), lisp_addr_ip_afi(addr_dest), lisp_addr_to_char(addr_dest)); return (NULL); } if ((lisp_addr_ip_afi(addr_from) != AF_INET) && (lisp_addr_ip_afi(addr_from) != AF_INET6)) { OOR_LOG(LDBG_2, "add_ip_udp_header: Unknown AFI %d", lisp_addr_ip_afi(addr_from)); return (NULL); } /* Headers lengths */ ip_hdr_len = ip_sock_afi_to_hdr_len(lisp_addr_ip_afi(addr_from)); 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) { OOR_LOG(LDBG_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) { OOR_LOG(LDBG_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, lisp_addr_ip_afi(addr_from))) == -1) { free(encap_pkt); return (NULL); } udpsum(udph_ptr) = udpsum; return (encap_pkt); }
static void send_fragment(int fd_raw, struct sockaddr *addr, socklen_t alen, int offset, bool ipv6) { int frag_len; int res; int payload_offset = offset > 0 ? offset - UDP_HLEN : 0; uint8_t *frag_start = ipv6 ? ip_frame + IP6_HLEN + FRAG_HLEN : ip_frame + IP4_HLEN; if (offset == 0) { struct udphdr udphdr; udphdr.source = htons(cfg_port + 1); udphdr.dest = htons(cfg_port); udphdr.len = htons(UDP_HLEN + payload_len); udphdr.check = 0; if (ipv6) udphdr.check = udp6_checksum((struct ip6_hdr *)ip_frame, &udphdr); else udphdr.check = udp_checksum((struct ip *)ip_frame, &udphdr); memcpy(frag_start, &udphdr, UDP_HLEN); } if (ipv6) { struct ip6_hdr *ip6hdr = (struct ip6_hdr *)ip_frame; struct ip6_frag *fraghdr = (struct ip6_frag *)(ip_frame + IP6_HLEN); if (payload_len - payload_offset <= max_frag_len && offset > 0) { /* This is the last fragment. */ frag_len = FRAG_HLEN + payload_len - payload_offset; fraghdr->ip6f_offlg = htons(offset); } else { frag_len = FRAG_HLEN + max_frag_len; fraghdr->ip6f_offlg = htons(offset | IP6_MF); } ip6hdr->ip6_plen = htons(frag_len); if (offset == 0) memcpy(frag_start + UDP_HLEN, udp_payload, frag_len - FRAG_HLEN - UDP_HLEN); else memcpy(frag_start, udp_payload + payload_offset, frag_len - FRAG_HLEN); frag_len += IP6_HLEN; } else { struct ip *iphdr = (struct ip *)ip_frame; if (payload_len - payload_offset <= max_frag_len && offset > 0) { /* This is the last fragment. */ frag_len = IP4_HLEN + payload_len - payload_offset; iphdr->ip_off = htons(offset / 8); } else { frag_len = IP4_HLEN + max_frag_len; iphdr->ip_off = htons(offset / 8 | IP4_MF); } iphdr->ip_len = htons(frag_len); if (offset == 0) memcpy(frag_start + UDP_HLEN, udp_payload, frag_len - IP4_HLEN - UDP_HLEN); else memcpy(frag_start, udp_payload + payload_offset, frag_len - IP4_HLEN); } res = sendto(fd_raw, ip_frame, frag_len, 0, addr, alen); if (res < 0) error(1, errno, "send_fragment"); if (res != frag_len) error(1, 0, "send_fragment: %d vs %d", res, frag_len); frag_counter++; }
// socksify and desocksify packets // ---------------------------------------- unsigned char *process_packet(unsigned char *pkt, int *len, u_int8_t hook) { struct conn_t *conn = NULL; struct ip *iph; struct udphdr *udph; unsigned char *data, *new_data; unsigned char *new_pkt = NULL; u_int32_t *dst_ip; u_int16_t *dstport; u_int8_t in_out; struct udps_config_t *config; /* It's a trick to avoid problems with routings * We only change source ip to nat-ip */ if (hook == NF_IP_POST_ROUTING) { new_pkt = malloc(*len); memcpy(new_pkt, pkt, *len); iph = (struct ip *)new_pkt; udph = (struct udphdr *)(new_pkt + sizeof(struct ip)); config = find_config_by_ip(iph->ip_src.s_addr); iph->ip_src.s_addr = config->nat_ip; udp_checksum(iph); ip_checksum(iph); return new_pkt; } iph = (struct ip *)pkt; udph = (struct udphdr *)(pkt + sizeof(struct ip)); in_out = 0; config = default_config; while (!in_out && config) { if (iph->ip_dst.s_addr == config->nat_ip) in_out = 1; config = config->next; } if (in_out) { /* incoming packet - to client * Desocksify packet: * * Change src ip to real from socks5 * and unpack udp */ if (*len > sizeof(struct ip) + sizeof(struct udphdr) + SOCKS_HEADER_UDP_SIZE_V4) { /* Is this correct udp-packet? */ data = (unsigned char *)(pkt + sizeof(struct ip) + sizeof(struct udphdr)); dst_ip = (u_int32_t *)(data + sizeof(u_int32_t)); dstport = (u_int16_t *)(data + sizeof(u_int32_t) * 2); conn = connections_revise(iph->ip_dst.s_addr, udph->uh_dport, *dst_ip, *dstport, iph->ip_src.s_addr, udph->uh_sport, 1); if (conn == NULL) return NULL; *len = *len - SOCKS_HEADER_UDP_SIZE_V4; new_pkt = malloc(*len); memcpy(new_pkt, pkt, sizeof(struct ip) + sizeof(struct udphdr)); new_data = (unsigned char *)(new_pkt + sizeof(struct ip) + sizeof(struct udphdr)); memcpy(new_data, &data[SOCKS_HEADER_UDP_SIZE_V4], *len - sizeof(struct ip) - sizeof(struct udphdr)); iph = (struct ip *)new_pkt; udph = (struct udphdr *)(new_pkt + sizeof(struct ip)); iph->ip_src.s_addr = conn->dst_ip; iph->ip_dst.s_addr = conn->src_ip; // little bit an ip-spoofing for udp iph->ip_len = htons(ntohs(iph->ip_len) - SOCKS_HEADER_UDP_SIZE_V4); udph->uh_sport = conn->dstport; udph->uh_ulen = htons(ntohs(udph->uh_ulen) - SOCKS_HEADER_UDP_SIZE_V4); /* trancate udp data */ /* Calculate checksums... */ udp_checksum(iph); ip_checksum(iph); } } else { /* outcoming packet - to socks5 server * Socksify packet * * Change dst ip from real to socks5 * and pack udp */ if (*len > sizeof(struct ip) + sizeof(struct udphdr)) { /* Is this correct udp-packet? */ conn = connections_revise(iph->ip_src.s_addr, udph->uh_sport, iph->ip_dst.s_addr, udph->uh_dport, 0, 0, 0); if (conn == NULL) /* Something wrong... */ return NULL; *len = *len + SOCKS_HEADER_UDP_SIZE_V4; new_pkt = malloc(*len); memcpy(new_pkt, pkt, sizeof(struct ip) + sizeof(struct udphdr)); data = (unsigned char *)(pkt + sizeof(struct ip) + sizeof(struct udphdr)); new_data = (unsigned char *)(new_pkt + sizeof(struct ip) + sizeof(struct udphdr)); new_data[0] = 0x0; new_data[1] = 0x0; new_data[2] = 0x0; new_data[3] = 0x1; memcpy(&new_data[4], &conn->dst_ip, sizeof(uint32_t)); memcpy(&new_data[8], &conn->dstport, sizeof(uint16_t)); memcpy(&new_data[10], data, *len - sizeof(struct ip) - sizeof(struct udphdr) - SOCKS_HEADER_UDP_SIZE_V4); iph = (struct ip *)new_pkt; udph = (struct udphdr *)((long *)iph + iph->ip_hl); iph->ip_dst.s_addr = conn->usock->sk_ip; iph->ip_len = htons(ntohs(iph->ip_len) + SOCKS_HEADER_UDP_SIZE_V4); udph->uh_dport = conn->usock->sk_port; udph->uh_ulen = htons(ntohs(udph->uh_ulen) + SOCKS_HEADER_UDP_SIZE_V4); /* trancate udp data */ /* Calculate checksums... */ udp_checksum(iph); ip_checksum(iph); } /*else printf("len is too small\n");*/ } return new_pkt; }
void packet_send(struct in_addr ip, uint16_t port, uint8_t type) { uint8_t packet[IP_MAXPACKET]; struct ifreq ifr; struct sockaddr_ll sll; int sockfd, done = 0; int ethdrlen = sizeof(struct ether_header); int iphdrlen = sizeof(struct ip); int tcphdrlen = sizeof(struct tcphdr); int udphdrlen = sizeof(struct udphdr); int packetlen = ethdrlen + iphdrlen; int payloadlen = 0; struct ether_header *eth = (struct ether_header *)packet; struct ip *iphdr = (struct ip *)(packet + ethdrlen); struct tcphdr *tcphdr = (struct tcphdr *)((uint8_t *)iphdr + iphdrlen); struct udphdr *udphdr = (struct udphdr *)((uint8_t *)iphdr + iphdrlen); char *payload; memset(packet, 0, IP_MAXPACKET); //printf("\tsend() - IP: %s, PORT: %d, TYPE:%x\n", inet_ntoa(ip), port, type); /* Set Static Options */ eth->ether_type = htons(ETHERTYPE_IP); iphdr->ip_hl = iphdrlen / sizeof(uint32_t); // 5 iphdr->ip_v = 4; iphdr->ip_ttl = 128; memcpy(&iphdr->ip_dst, &ip, sizeof(struct in_addr)); if( type == UDP ) { payload = (char *)packet + packetlen + udphdrlen; payloadlen = UDP_PAYLOAD; iphdr->ip_p = IPPROTO_UDP; udphdr->len = htons(udphdrlen + payloadlen); udphdr->dest = htons(port); packetlen += udphdrlen + payloadlen; }else { payload = (char *)packet + packetlen + tcphdrlen; iphdr->ip_p = IPPROTO_TCP; tcphdr->dest = htons(port); tcphdr->doff = tcphdrlen / 4; // 5 tcphdr->window = htons(8192); if( type == SYN ) { memcpy(payload, "\x05\xb4\x01\x01\x04\x02", 6); payloadlen = 6; iphdr->ip_len = htons(iphdrlen + tcphdrlen + 6); tcphdr->syn = 1; }else { iphdr->ip_len = htons(iphdrlen + tcphdrlen); tcphdr->fin = 1; } packetlen += tcphdrlen + payloadlen; } /* SEND PACKET */ srand(time(NULL) + pthread_self()); /* Set Packet Options */ if( (sockfd = socket(PF_PACKET, SOCK_RAW, IPPROTO_RAW)) < 0 ) { perror("socket(): "); return; } memset(&ifr, 0, sizeof(struct ifreq)); strncpy(ifr.ifr_name, "eth0", IFNAMSIZ-1); if( ioctl(sockfd, SIOCGIFINDEX, &ifr) < 0 ) { perror("ioctl(): "); close(sockfd); return; } sll.sll_family = AF_PACKET; sll.sll_ifindex = ifr.ifr_ifindex; if( bind(sockfd, (struct sockaddr *)&sll, sizeof(sll)) < 0 ) { perror("bind(): "); close(sockfd); return; } while( 1 ) { /* IP HEADER */ iphdr->ip_id = rand(); iphdr->ip_src.s_addr = rand(); iphdr->ip_sum = 0; iphdr->ip_sum = checksum((uint16_t *)iphdr, iphdrlen); if( type == UDP ) { /* UDP HEADER */ udphdr->source = rand(); udphdr->check = 0; udphdr->check = udp_checksum(iphdr, udphdr, payload, payloadlen); }else { /* TCP HEADER */ tcphdr->source = rand(); tcphdr->seq = rand(); tcphdr->ack_seq = rand(); tcphdr->check = 0; tcphdr->check = tcp_checksum(iphdr, tcphdr, payload, payloadlen); } //rawprint(packet, packetlen); /* SEND PACKET */ // if( (done = write(sockfd, packet, packetlen)) != packetlen ) if( (done = write(sockfd, packet, packetlen)) < 0 ) { perror("write()"); }else if( done != packetlen ) { printf("%d / %d bytes sended\n", done, packetlen); } sleep(1); } close(sockfd); }
/* This entire section is essentially a duplicate of the net_transmit code, except its formatted differently. I wish I could combine them somehow. */ static bool biudp_write_segment(const uint8 *in_data, uint32 in_data_length) { ether_ii_header_t *frame_out; ip_header_t *ip; udp_header_t *udp; uint16 total_length, ip_length, ip_header_length; bool retval; /* STAGE: Use Ethernet II. Everyone MUST support it. */ ip_length = sizeof(ip_header_t) + sizeof(udp_header_t) + in_data_length; total_length = sizeof(ether_ii_header_t) + ip_length; /* STAGE: malloc() the proper size output buffer. */ frame_out = malloc(total_length); if (!frame_out) return FALSE; ip = (ip_header_t *) ((uint8 *) frame_out + sizeof(ether_ii_header_t)); /* STAGE: Setup the frame layer. */ memcpy(frame_out->source, rtl_info.mac, ETHER_MAC_SIZE); memcpy(frame_out->dest, control.dest_mac, ETHER_MAC_SIZE); frame_out->ethertype = htons(0x0800); /* STAGE: Setup the IP layer. */ ip->version_ihl = 0x45; /* 4 is the IP version, 5 is the size of the header in 32-bit segments. No options and no padding. */ ip->tos = 0x14; /* Normal precedence, low delay, high reliability - this is all we want but we'll never get. */ ip->length = htons(ip_length); ip->packet_id = 0; /* TODO (!!!): Add some sort of ID system later. */ ip->flags_frag_offset = htons(0x4000); /* One order of IP packet, hold the fragmentation. */ ip->ttl = 0x20; /* 32 seems like plenty - it gets me to Kirk. */ ip->protocol = IP_PROTO_UDP; ip->checksum = 0; SAFE_UINT32_COPY(ip->source, control.source_ip); SAFE_UINT32_COPY(ip->dest, control.dest_ip); /* STAGE: Calculate the IP checksum last. */ ip_header_length = IP_HEADER_SIZE(ip); ip->checksum = ip_checksum(ip, ip_header_length); /* STAGE: Identify the UDP layer. */ udp = (udp_header_t *) ((uint8 *) ip + ip_header_length); /* STAGE: Setup the UDP layer. */ udp->src = htons(VOOT_UDP_PORT); udp->dest = control.port; udp->length = htons(sizeof(udp_header_t) + in_data_length); /* STAGE: So, how about that data? */ memcpy((uint8 *) udp + sizeof(udp_header_t), in_data, in_data_length); /* STAGE: Calculate the UDP checksum last. */ udp->checksum = udp_checksum(ip, ip_header_length); /* STAGE: ... and transmit it, god willing. */ retval = rtl_tx((uint8 *) frame_out, sizeof(ether_ii_header_t) + ip_length); /* STAGE: Free the output buffer. */ free(frame_out); return retval; }
void udps_rx(TCPIPS* tcpips, IO* io, IP* src) { HANDLE handle; UDP_HEADER* hdr; UDP_HANDLE* uh; uint16_t src_port, dst_port; #if(UDP_BROADCAST) const IP* dst; dst = (const IP*)io_data(io) - 1; if (io->data_size < sizeof(UDP_HEADER) || udp_checksum(io_data(io), io->data_size, src, dst)) #else if (io->data_size < sizeof(UDP_HEADER) || udp_checksum(io_data(io), io->data_size, src, &tcpips->ips.ip)) #endif { ips_release_io(tcpips, io); return; } hdr = io_data(io); src_port = be2short(hdr->src_port_be); dst_port = be2short(hdr->dst_port_be); #if (UDP_DEBUG_FLOW) printf("UDP: "); ip_print(src); printf(":%d -> ", src_port); ip_print(&tcpips->ips.ip); printf(":%d, %d byte(s)\n", dst_port, io->data_size - sizeof(UDP_HEADER)); #endif //UDP_DEBUG_FLOW #if(DHCPS) if((dst_port == DHCP_SERVER_PORT)||(src_port == DHCP_CLIENT_PORT)) { io_hide(io, sizeof(UDP_HEADER)); if(dhcps_rx(tcpips,io,src)) udps_replay(tcpips,io,&__BROADCAST); else ips_release_io(tcpips, io); return; } #endif #if(DNSS) if((dst_port == DNS_PORT)&&(dst->u32.ip == tcpips->ips.ip.u32.ip)) { io_hide(io, sizeof(UDP_HEADER)); if(dnss_rx(tcpips,io,src)) udps_replay(tcpips,io,src); else ips_release_io(tcpips, io); return; } #endif //search in listeners handle = udps_find(tcpips, dst_port); if (handle != INVALID_HANDLE) { uh = so_get(&tcpips->udps.handles, handle); //listener or connected if (uh->remote_port == 0 || (uh->remote_port == src_port && uh->remote_addr.u32.ip == src->u32.ip)) udps_send_user(tcpips, src, io, handle); else handle = INVALID_HANDLE; } #if(UDP_BROADCAST) if ((handle == INVALID_HANDLE) && (dst->u32.ip != BROADCAST)) #else if (handle == INVALID_HANDLE) #endif { #if (UDP_DEBUG) printf("UDP: no connection, datagramm dropped\n"); #endif //UDP_DEBUG #if (ICMP) icmps_tx_error(tcpips, io, ICMP_ERROR_PORT, 0); #endif //ICMP } ips_release_io(tcpips, io); }