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                                                 return 0;
  44                                 }
  45                                 break;
  46                 case DLT_LINUX_SLL:
  47                                 /*Linux Cooked Capture*/
  48                                 if(!linux_cooked_encap(old)){
  49                                         return 0;
  50                                 }
  51                                 break;
  52                 default:
  53                                 dbgprintf(0, "Unknown Link Layer\n");
  54                                 return 0;
  55         }
  56 return 1;
  57 }
  58
  59 /*Standard Ethernet Encapsulation*/
  60 int ethernet_encap(const struct const_packet *old)
  61 {
  62                 struct ether_header     *ethh;
  63                 struct const_packet nold;
  64
  65                 /*Safety checks*/
  66                 if(!old|| !old->data ||!old->h){
  67                         dbgprintf(0,"Error: Ethernet Encapsulation Function given bad data!\n");
  68                         return 0;
  69                 }
  70                 if(old->length < sizeof(struct ether_header)){
  71                         dbgprintf(0, "Error: Ethernet Encapsulation Function given packet of wrong size!\n");
  72                         return 0;
  73                 }
  74
  75                 /*Cast Pointer*/
  76                 ethh=(struct ether_header*)(old->data);
  77
  78                 /*Adjust pointers and lengths*/
  79                 nold.data= old->data+ sizeof(struct ether_header);
  80                 nold.length= old->length - sizeof(struct ether_header);
  81                 nold.h=old->h;
  82                 nold.private=old->private;
  83
  84                 /*Select Next Protocol*/
  85                 switch(ntohs(ethh->ether_type)){
  86                         case ETHERTYPE_IP:
  87                                         if(!ipv4_encap(&nold)){
  88                                                         return 0;
  89                                         }
  90                                         break;
  91                         case ETHERTYPE_IPV6:
  92                                         if(!ipv6_encap(&nold)){
  93                                                         return 0;
  94                                         }
  95                                         break;
  96                         case ETHERTYPE_VLAN:
  97                                         if(!ethernet_vlan_encap(&nold)){
  98                                                         return 0;
  99                                         }
 100                                         break;
 101                         default:
 102                                         dbgprintf(1, "Unknown Next Protocol at Ethernet\n");
 103                                         return 0;
 104                                         break;
 105                 }
 106 return 1;
 107 }
 108
 109 /*Ethernet 802.1Q VLAN Encapsulation*/
 110 int ethernet_vlan_encap(const struct const_packet *old)
 111 {
 112                 struct vlan_tag         *tag;
 113                 struct const_packet nold;
 114
 115                 /*Safety checks*/
 116                 if(!old || !old->data || !old->h){
 117                         dbgprintf(0,"Error: Ethernet VLAN Encapsulation Function given bad data!\n");
 118                         return 0;
 119                 }
 120                 if(old->length < sizeof(struct vlan_tag)){
 121                         dbgprintf(0, "Error: Ethernet VLAN Encapsulation Function given packet of wrong size!\n");
 122                         return 0;
 123                 }
 124
 125                 /*Cast Pointer*/
 126                 tag=(struct vlan_tag*)(old->data);
 127
 128                 /*Adjust pointers and lengths*/
 129                 nold.data= old->data+ sizeof(struct vlan_tag);
 130                 nold.length= old->length - sizeof(struct vlan_tag);
 131                 nold.h=old->h;
 132                 nold.private=old->private;
 133
 134                 /*Select Next Protocol*/
 135                 switch(ntohs(tag->vlan_tci)){
 136                         case ETHERTYPE_IP:
 137                                         if(!ipv4_encap(&nold)){
 138                                                         return 0;
 139                                         }
 140                                         break;
 141                         case ETHERTYPE_IPV6:
 142                                         if(!ipv6_encap(&nold)){
 143                                                         return 0;
 144                                         }
 145                                         break;
 146                         case ETHERTYPE_VLAN:
 147                                         if(!ethernet_vlan_encap(&nold)){
 148                                                         return 0;
 149                                         }
 150                                         break;
 151                         default:
 152                                         dbgprintf(1, "Unknown Next Protocol at Ethernet VLAN tag\n");
 153                                         return 0;
 154                                         break;
 155                 }
 156 return 1;
 157 }
 158
 159 /*IPv6 Encapsulation*/
 160 int ipv6_encap(const struct const_packet *old)
 161 {
 162                 struct ip6_hdr          *iph;
 163                 struct const_packet     nold;
 164
 165                 /*Safety checks*/
 166                 if(!old->data || !old->h){
 167                         dbgprintf(0,"Error: IPv6 Encapsulation Function given bad data!\n");
 168                         return 0;
 169                 }
 170                 if(old->length < sizeof(struct ip6_hdr)){
 171                         dbgprintf(0, "Error: IPv6 Encapsulation Function given packet of wrong size!\n");
 172                         return 0;
 173                 }
 174
 175                 /*Cast Pointer*/
 176                 iph=(struct ip6_hdr*)(old->data);
 177
 178                 /*Adjust pointers and lengths*/
 179                 nold.data= old->data + sizeof(struct ip6_hdr);
 180                 nold.length= old->length - sizeof(struct ip6_hdr);
 181                 nold.h=old->h;
 182                 nold.private=old->private;
 183
 184                 /*Confirm that this is IPv6*/
 185                 if((ntohl(iph->ip6_ctlun.ip6_un1.ip6_un1_flow) & (0xF0000000)) == (60000000)){
 186                         dbgprintf(1, "Note: Packet is not IPv6\n");
 187                         return 0;
 188                 }
 189
 190                 /*Select Next Protocol*/
 191                 switch(iph->ip6_ctlun.ip6_un1.ip6_un1_nxt){
 192                         case 41:
 193                                         /*6in4*/
 194                                         if(!decap_packet(&nold)){
 195                                                 return 0;
 196                                         }
 197                                         break;
 198                         default:
 199                                         dbgprintf(1, "Unknown Next Protocol at IPv6\n");
 200                                         return 0;
 201                                         break;
 202                 }
 203 return 1;
 204 }
 205
 206 /*IPv4 Encapsulation*/
 207 int ipv4_encap(const struct const_packet *old)
 208 {
 209                 struct iphdr            *iph;
 210                 struct const_packet     nold;
 211
 212                 /*Safety checks*/
 213                 if(!old || !old->data || !old->h){
 214                         dbgprintf(0,"Error: IPv4 Encapsulation Function given bad data!\n");
 215                         return 0;
 216                 }
 217                 if(old->length < sizeof(struct iphdr)){
 218                         dbgprintf(0, "Error: IPv4 Encapsulation Function given packet of wrong size!\n");
 219                         return 0;
 220                 }
 221
 222                 /*Cast Pointer*/
 223                 iph=(struct iphdr*)(old->data);
 224
 225                 /*Adjust pointers and lengths*/
 226                 nold.data= old->data +iph->ihl*4;
 227                 nold.length= old->length -iph->ihl*4;
 228                 nold.h=old->h;
 229                 nold.private=old->private;
 230
 231                 /*Confirm that this is IPv4*/
 232                 if(iph->version!=4){
 233                         dbgprintf(1, "Note: Packet is not IPv4\n");
 234                         return 0;
 235                 }
 236
 237                 /*Select Next Protocol*/
 238                 switch(iph->protocol){
 239                         case 41:
 240                                         /*6in4*/
 241                                         if(!decap_packet(&nold)){
 242                                                 return 0;
 243                                         }
 244                                         break;
 245                         default:
 246                                         dbgprintf(1, "Unknown Next Protocol at IPv4\n");
 247                                         return 0;
 248                                         break;
 249                 }
 250 return 1;
 251 }
 252
 253 int linux_cooked_encap(const struct const_packet *old)
 254 {
 255         struct sll_header               *slh;
 256         struct const_packet             nold;
 257
 258
 259         /*Safety checks*/
 260         if(!old ||  !old->data ||!old->h){
 261                 dbgprintf(0,"Error: SLL Encapsulation Function given bad data!\n");
 262                 return 0;
 263         }
 264         if(old->length < sizeof(struct sll_header)){
 265                 dbgprintf(0, "Error: SLL Encapsulation Function given packet of wrong size!\n");
 266                 return 0;
 267         }
 268
 269         /*Cast Pointer*/
 270         slh=(struct sll_header*)(old->data);
 271
 272         /*Adjust pointers and lengths*/
 273         nold.data= old->data + sizeof(struct sll_header);
 274         nold.length= old->length - sizeof(struct sll_header);
 275         nold.h=old->h;
 276         nold.private=old->private;
 277
 278         /*Confirm that this is SLL*/
 279         if(ntohs(slh->sll_pkttype) > 4){
 280                 dbgprintf(1, "Note: Packet is not SLL (Linux Cooked Capture)\n");
 281                 return 0;
 282         }
 283
 284         /*Select Next Protocol*/
 285         switch(ntohs(slh->sll_protocol)){
 286                 case ETHERTYPE_IP:
 287                                 if(!ipv4_encap(&nold)){
 288                                                 return 0;
 289                                 }
 290                                 break;
 291                 case ETHERTYPE_IPV6:
 292                                 if(!ipv6_encap(&nold)){
 293                                                 return 0;
 294                                 }
 295                                 break;
 296                 default:
 297                                 dbgprintf(1, "Unknown Next Protocol at SLL\n");
 298                                 return 0;
 299                                 break;
 300         }
 301 return 1;
 302 }