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);
}
