sjero.net Git - strip6in4/blob - encap.c

   1 /******************************************************************************
   2 Utility to create a pcap file of a 6in4 stream present in an origin pcap file
   3
   4 Copyright (C) 2013  Samuel Jero <sj323707@ohio.edu>
   5
   6 This program is free software: you can redistribute it and/or modify
   7 it under the terms of the GNU General Public License as published by
   8 the Free Software Foundation, either version 3 of the License, or
   9 (at your option) any later version.
  10
  11 This program is distributed in the hope that it will be useful,
  12 but WITHOUT ANY WARRANTY; without even the implied warranty of
  13 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  14 GNU General Public License for more details.
  15
  16 You should have received a copy of the GNU General Public License
  17 along with this program.  If not, see <http://www.gnu.org/licenses/>.
  18
  19 Author: Samuel Jero <sj323707@ohio.edu>
  20 Date: 03/2013
  21 ******************************************************************************/
  22 #include "strip6in4.h"
  23 #include "encap.h"
  24 #include <pcap/sll.h>
  25 #include <pcap/vlan.h>
  26 #include <netinet/ip6.h>
  27 #include <netinet/if_ether.h>
  28 #include <netinet/ip.h>
  29
  30 /*Encapsulation start point and link layer selector*/
  31 int do_encap(int link, const struct const_packet *old)
  32 {
  33         switch(link){
  34                 case DLT_EN10MB:
  35                                 /*Ethernet*/
  36                                 if(!ethernet_encap(old)){
  37                                                 return 0;
  38                                 }
  39                                 break;
  40                 case DLT_RAW:
  41                                 /*Raw. Just IP*/
  42                                 if(!ipv4_encap(old)){
  43                                         if(!ipv6_encap(old)){
  44                                                 return 0;
  45                                         }
  46                                 }
  47                                 break;
  48                 case DLT_LINUX_SLL:
  49                                 /*Linux Cooked Capture*/
  50                                 if(!linux_cooked_encap(old)){
  51                                         return 0;
  52                                 }
  53                                 break;
  54                 default:
  55                                 dbgprintf(0, "Unknown Link Layer\n");
  56                                 return 0;
  57         }
  58 return 1;
  59 }
  60
  61 /*Standard Ethernet Encapsulation*/
  62 int ethernet_encap(const struct const_packet *old)
  63 {
  64                 struct ether_header     *ethh;
  65                 struct const_packet nold;
  66
  67                 /*Safety checks*/
  68                 if(!old|| !old->data ||!old->h){
  69                         dbgprintf(0,"Error: Ethernet Encapsulation Function given bad data!\n");
  70                         return 0;
  71                 }
  72                 if(old->length < sizeof(struct ether_header)){
  73                         dbgprintf(0, "Error: Ethernet Encapsulation Function given packet of wrong size!\n");
  74                         return 0;
  75                 }
  76
  77                 /*Cast Pointer*/
  78                 ethh=(struct ether_header*)(old->data);
  79
  80                 /*Adjust pointers and lengths*/
  81                 nold.data= old->data+ sizeof(struct ether_header);
  82                 nold.length= old->length - sizeof(struct ether_header);
  83                 nold.h=old->h;
  84                 nold.private=old->private;
  85
  86                 /*Select Next Protocol*/
  87                 switch(ntohs(ethh->ether_type)){
  88                         case ETHERTYPE_IP:
  89                                         if(!ipv4_encap(&nold)){
  90                                                         return 0;
  91                                         }
  92                                         break;
  93                         case ETHERTYPE_IPV6:
  94                                         if(!ipv6_encap(&nold)){
  95                                                         return 0;
  96                                         }
  97                                         break;
  98                         case ETHERTYPE_VLAN:
  99                                         if(!ethernet_vlan_encap(&nold)){
 100                                                         return 0;
 101                                         }
 102                                         break;
 103                         default:
 104                                         dbgprintf(1, "Unknown Next Protocol at Ethernet\n");
 105                                         return 0;
 106                                         break;
 107                 }
 108 return 1;
 109 }
 110
 111 /*Ethernet 802.1Q VLAN Encapsulation*/
 112 int ethernet_vlan_encap(const struct const_packet *old)
 113 {
 114                 struct vlan_tag         *tag;
 115                 struct const_packet nold;
 116
 117                 /*Safety checks*/
 118                 if(!old || !old->data || !old->h){
 119                         dbgprintf(0,"Error: Ethernet VLAN Encapsulation Function given bad data!\n");
 120                         return 0;
 121                 }
 122                 if(old->length < sizeof(struct vlan_tag)){
 123                         dbgprintf(0, "Error: Ethernet VLAN Encapsulation Function given packet of wrong size!\n");
 124                         return 0;
 125                 }
 126
 127                 /*Cast Pointer*/
 128                 tag=(struct vlan_tag*)(old->data);
 129
 130                 /*Adjust pointers and lengths*/
 131                 nold.data= old->data+ sizeof(struct vlan_tag);
 132                 nold.length= old->length - sizeof(struct vlan_tag);
 133                 nold.h=old->h;
 134                 nold.private=old->private;
 135
 136                 /*Select Next Protocol*/
 137                 switch(ntohs(tag->vlan_tci)){
 138                         case ETHERTYPE_IP:
 139                                         if(!ipv4_encap(&nold)){
 140                                                         return 0;
 141                                         }
 142                                         break;
 143                         case ETHERTYPE_IPV6:
 144                                         if(!ipv6_encap(&nold)){
 145                                                         return 0;
 146                                         }
 147                                         break;
 148                         case ETHERTYPE_VLAN:
 149                                         if(!ethernet_vlan_encap(&nold)){
 150                                                         return 0;
 151                                         }
 152                                         break;
 153                         default:
 154                                         dbgprintf(1, "Unknown Next Protocol at Ethernet VLAN tag\n");
 155                                         return 0;
 156                                         break;
 157                 }
 158 return 1;
 159 }
 160
 161 /*IPv6 Encapsulation*/
 162 int ipv6_encap(const struct const_packet *old)
 163 {
 164                 struct ip6_hdr          *iph;
 165                 struct const_packet     nold;
 166
 167                 /*Safety checks*/
 168                 if(!old->data || !old->h){
 169                         dbgprintf(0,"Error: IPv6 Encapsulation Function given bad data!\n");
 170                         return 0;
 171                 }
 172                 if(old->length < sizeof(struct ip6_hdr)){
 173                         dbgprintf(0, "Error: IPv6 Encapsulation Function given packet of wrong size!\n");
 174                         return 0;
 175                 }
 176
 177                 /*Cast Pointer*/
 178                 iph=(struct ip6_hdr*)(old->data);
 179
 180                 /*Adjust pointers and lengths*/
 181                 nold.data= old->data + sizeof(struct ip6_hdr);
 182                 nold.length= old->length - sizeof(struct ip6_hdr);
 183                 nold.h=old->h;
 184                 nold.private=old->private;
 185
 186                 /*Confirm that this is IPv6*/
 187                 if((ntohl(iph->ip6_ctlun.ip6_un1.ip6_un1_flow) & (0xF0000000)) == (60000000)){
 188                         dbgprintf(1, "Note: Packet is not IPv6\n");
 189                         return 0;
 190                 }
 191
 192                 /*Select Next Protocol*/
 193                 switch(iph->ip6_ctlun.ip6_un1.ip6_un1_nxt){
 194                         case 41:
 195                                         /*6in4*/
 196                                         if(!decap_packet(&nold)){
 197                                                 return 0;
 198                                         }
 199                                         break;
 200                         default:
 201                                         dbgprintf(1, "Unknown Next Protocol at IPv6\n");
 202                                         return 0;
 203                                         break;
 204                 }
 205 return 1;
 206 }
 207
 208 /*IPv4 Encapsulation*/
 209 int ipv4_encap(const struct const_packet *old)
 210 {
 211                 struct iphdr            *iph;
 212                 struct const_packet     nold;
 213
 214                 /*Safety checks*/
 215                 if(!old || !old->data || !old->h){
 216                         dbgprintf(0,"Error: IPv4 Encapsulation Function given bad data!\n");
 217                         return 0;
 218                 }
 219                 if(old->length < sizeof(struct iphdr)){
 220                         dbgprintf(0, "Error: IPv4 Encapsulation Function given packet of wrong size!\n");
 221                         return 0;
 222                 }
 223
 224                 /*Cast Pointer*/
 225                 iph=(struct iphdr*)(old->data);
 226
 227                 /*Adjust pointers and lengths*/
 228                 nold.data= old->data +iph->ihl*4;
 229                 nold.length= old->length -iph->ihl*4;
 230                 nold.h=old->h;
 231                 nold.private=old->private;
 232
 233                 /*Confirm that this is IPv4*/
 234                 if(iph->version!=4){
 235                         dbgprintf(1, "Note: Packet is not IPv4\n");
 236                         return 0;
 237                 }
 238
 239                 /*Select Next Protocol*/
 240                 switch(iph->protocol){
 241                         case 41:
 242                                         /*6in4*/
 243                                         if(!decap_packet(&nold)){
 244                                                 return 0;
 245                                         }
 246                                         break;
 247                         default:
 248                                         dbgprintf(1, "Unknown Next Protocol at IPv4\n");
 249                                         return 0;
 250                                         break;
 251                 }
 252 return 1;
 253 }
 254
 255 int linux_cooked_encap(const struct const_packet *old)
 256 {
 257         struct sll_header               *slh;
 258         struct const_packet             nold;
 259
 260
 261         /*Safety checks*/
 262         if(!old ||  !old->data ||!old->h){
 263                 dbgprintf(0,"Error: SLL Encapsulation Function given bad data!\n");
 264                 return 0;
 265         }
 266         if(old->length < sizeof(struct sll_header)){
 267                 dbgprintf(0, "Error: SLL Encapsulation Function given packet of wrong size!\n");
 268                 return 0;
 269         }
 270
 271         /*Cast Pointer*/
 272         slh=(struct sll_header*)(old->data);
 273
 274         /*Adjust pointers and lengths*/
 275         nold.data= old->data + sizeof(struct sll_header);
 276         nold.length= old->length - sizeof(struct sll_header);
 277         nold.h=old->h;
 278         nold.private=old->private;
 279
 280         /*Confirm that this is SLL*/
 281         if(ntohs(slh->sll_pkttype) > 4){
 282                 dbgprintf(1, "Note: Packet is not SLL (Linux Cooked Capture)\n");
 283                 return 0;
 284         }
 285
 286         /*Select Next Protocol*/
 287         switch(ntohs(slh->sll_protocol)){
 288                 case ETHERTYPE_IP:
 289                                 if(!ipv4_encap(&nold)){
 290                                                 return 0;
 291                                 }
 292                                 break;
 293                 case ETHERTYPE_IPV6:
 294                                 if(!ipv6_encap(&nold)){
 295                                                 return 0;
 296                                 }
 297                                 break;
 298                 default:
 299                                 dbgprintf(1, "Unknown Next Protocol at SLL\n");
 300                                 return 0;
 301                                 break;
 302         }
 303 return 1;
 304 }