X-Git-Url: http://sjero.net/git/?a=blobdiff_plain;f=encap.c;h=d2c06b1b44b98ce80131ef7fb2f87b7545141ac4;hb=5e3bc5722d048a5b9581cbe27c64de5048d444b0;hp=4cda535226eff16ba7f84b9924f2f05fc82d869d;hpb=d4e8f9301a46e196dd8090595b8aae1a04d2a42a;p=dccp2tcp diff --git a/encap.c b/encap.c index 4cda535..d2c06b1 100644 --- a/encap.c +++ b/encap.c @@ -24,7 +24,9 @@ Date: 11/2012 #include "encap.h" #include "checksums.h" #include +#include #include +#include /*Encapsulation start point and link layer selector*/ int do_encap(int link, struct packet *new, const struct const_packet *old) @@ -92,6 +94,14 @@ int ethernet_encap(struct packet *new, const struct const_packet *old) nnew.length= new->length - sizeof(struct ether_header); nnew.h=new->h; nold.h=old->h; + nnew.print_id=NULL; + nnew.dest_id=NULL; + nnew.src_id=NULL; + nnew.id_len=0; + nold.print_id=NULL; + nold.dest_id=NULL; + nold.src_id=NULL; + nold.id_len=0; /*Select Next Protocol*/ switch(ntohs(ethh->ether_type)){ @@ -105,6 +115,11 @@ int ethernet_encap(struct packet *new, const struct const_packet *old) return 0; } break; + case ETHERTYPE_VLAN: + if(!ethernet_vlan_encap(&nnew, &nold)){ + return 0; + } + break; default: dbgprintf(1, "Unknown Next Protocol at Ethernet\n"); return 0; @@ -116,6 +131,73 @@ int ethernet_encap(struct packet *new, const struct const_packet *old) return 1; } +/*Ethernet 802.1Q VLAN Encapsulation*/ +int ethernet_vlan_encap(struct packet *new, const struct const_packet *old) +{ + struct vlan_tag *tag; + struct const_packet nold; + struct packet nnew; + + /*Safety checks*/ + if(!new || !old || !new->data || !old->data || !new->h || !old->h){ + dbgprintf(0,"Error: Ethernet VLAN Encapsulation Function given bad data!\n"); + return 0; + } + if(old->length < sizeof(struct vlan_tag) || new->length < sizeof(struct vlan_tag)){ + dbgprintf(0, "Error: Ethernet VLAN Encapsulation Function given packet of wrong size!\n"); + return 0; + } + + /*Copy VLAN tag over*/ + memcpy(new->data, old->data, sizeof(struct vlan_tag)); + + /*Cast Pointer*/ + tag=(struct vlan_tag*)(new->data); + + /*Adjust pointers and lengths*/ + nold.data= old->data+ sizeof(struct vlan_tag); + nnew.data= new->data + sizeof(struct vlan_tag); + nold.length= old->length - sizeof(struct vlan_tag); + nnew.length= new->length - sizeof(struct vlan_tag); + nnew.h=new->h; + nold.h=old->h; + nnew.print_id=NULL; + nnew.dest_id=NULL; + nnew.src_id=NULL; + nnew.id_len=0; + nold.print_id=NULL; + nold.dest_id=NULL; + nold.src_id=NULL; + nold.id_len=0; + + /*Select Next Protocol*/ + switch(ntohs(tag->vlan_tci)){ + case ETHERTYPE_IP: + if(!ipv4_encap(&nnew, &nold)){ + return 0; + } + break; + case ETHERTYPE_IPV6: + if(!ipv6_encap(&nnew, &nold)){ + return 0; + } + break; + case ETHERTYPE_VLAN: + if(!ethernet_vlan_encap(&nnew, &nold)){ + return 0; + } + break; + default: + dbgprintf(1, "Unknown Next Protocol at Ethernet VLAN tag\n"); + return 0; + break; + } + + /*Adjust length*/ + new->length=nnew.length + sizeof(struct vlan_tag); +return 1; +} + /*IPv6 Encapsulation*/ int ipv6_encap(struct packet *new, const struct const_packet *old) { @@ -146,6 +228,10 @@ int ipv6_encap(struct packet *new, const struct const_packet *old) nnew.length= new->length - sizeof(struct ip6_hdr); nnew.h=new->h; nold.h=old->h; + nnew.print_id=print_ipv6; + nold.print_id=print_ipv6; + nnew.id_len=16; + nold.id_len=16; /*Confirm that this is IPv6*/ if((ntohl(iph->ip6_ctlun.ip6_un1.ip6_un1_flow) & (0xF0000000)) == (60000000)){ @@ -157,15 +243,19 @@ int ipv6_encap(struct packet *new, const struct const_packet *old) switch(iph->ip6_ctlun.ip6_un1.ip6_un1_nxt){ case 33: /*DCCP*/ - nnew.id_len=16; nnew.src_id=malloc(nnew.id_len); nnew.dest_id=malloc(nnew.id_len); - if(nnew.src_id==NULL||nnew.dest_id==NULL){ + nold.src_id=malloc(nold.id_len); + nold.dest_id=malloc(nold.id_len); + if(nnew.src_id==NULL||nnew.dest_id==NULL || + nold.src_id==NULL||nold.dest_id==NULL){ dbgprintf(0,"Error: Couldn't allocate Memory\n"); exit(1); } memcpy(nnew.src_id,&iph->ip6_src,nnew.id_len); memcpy(nnew.dest_id,&iph->ip6_dst,nnew.id_len); + memcpy(nold.src_id,&iph->ip6_src,nold.id_len); + memcpy(nold.dest_id,&iph->ip6_dst,nold.id_len); if(!convert_packet(&nnew, &nold)){ return 0; } @@ -194,6 +284,8 @@ int ipv6_encap(struct packet *new, const struct const_packet *old) /*Cleanup*/ free(nnew.dest_id); free(nnew.src_id); + free(nold.dest_id); + free(nold.src_id); return 1; } @@ -227,6 +319,10 @@ int ipv4_encap(struct packet *new, const struct const_packet *old) nnew.length= new->length -iph->ihl*4; nnew.h=new->h; nold.h=old->h; + nnew.print_id=print_ipv4; + nold.print_id=print_ipv4; + nnew.id_len=4; + nold.id_len=4; /*Confirm that this is IPv4*/ if(iph->version!=4){ @@ -238,15 +334,19 @@ int ipv4_encap(struct packet *new, const struct const_packet *old) switch(iph->protocol){ case 33: /*DCCP*/ - nnew.id_len=4; nnew.src_id=malloc(nnew.id_len); nnew.dest_id=malloc(nnew.id_len); - if(nnew.src_id==NULL||nnew.dest_id==NULL){ + nold.src_id=malloc(nold.id_len); + nold.dest_id=malloc(nold.id_len); + if(nnew.src_id==NULL||nnew.dest_id==NULL|| + nold.src_id==NULL||nold.dest_id==NULL){ dbgprintf(0,"Error: Couldn't allocate Memory\n"); exit(1); } memcpy(nnew.src_id,&iph->saddr,nnew.id_len); memcpy(nnew.dest_id,&iph->daddr,nnew.id_len); + memcpy(nold.src_id,&iph->saddr,nold.id_len); + memcpy(nold.dest_id,&iph->daddr,nold.id_len); if(!convert_packet(&nnew, &nold)){ return 0; } @@ -280,6 +380,8 @@ int ipv4_encap(struct packet *new, const struct const_packet *old) /*Cleanup*/ free(nnew.src_id); free(nnew.dest_id); + free(nold.src_id); + free(nold.dest_id); return 1; } @@ -313,6 +415,14 @@ int linux_cooked_encap(struct packet *new, const struct const_packet *old) nnew.length= new->length- sizeof(struct sll_header); nnew.h=new->h; nold.h=old->h; + nnew.print_id=NULL; + nnew.dest_id=NULL; + nnew.src_id=NULL; + nnew.id_len=0; + nold.print_id=NULL; + nold.dest_id=NULL; + nold.src_id=NULL; + nold.id_len=0; /*Confirm that this is SLL*/ if(ntohs(slh->sll_pkttype) > 4){ @@ -342,3 +452,38 @@ int linux_cooked_encap(struct packet *new, const struct const_packet *old) new->length=nnew.length + sizeof(struct sll_header); return 1; } + + +char *print_ipv6(char* buf, int len, u_char* id, int id_len) +{ + struct sockaddr_in6 sa; + + if(buf==NULL){ + return NULL; + } + + memcpy(&sa.sin6_addr, id, id_len); + sa.sin6_family=AF_INET6; + if(getnameinfo((struct sockaddr*)&sa, sizeof(struct sockaddr_in6), + buf, len, NULL,0,NI_NUMERICHOST)<0){ + return NULL; + } + return buf; +} + +char *print_ipv4(char* buf, int len, u_char* id, int id_len) +{ + struct sockaddr_in sa; + + if(buf==NULL){ + return NULL; + } + + memcpy(&sa.sin_addr, id, id_len); + sa.sin_family=AF_INET; + if(getnameinfo((struct sockaddr*)&sa, sizeof(struct sockaddr_in), + buf, len, NULL,0,NI_NUMERICHOST)<0){ + return NULL; + } + return buf; +}