/* $Id: rewrite_l2.c 1503 2006-07-17 02:33:45Z aturner $ */ /* * Copyright (c) 2005 Aaron Turner. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * 1. Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * 2. Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * 3. Neither the names of the copyright owners nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED ``AS IS'' AND ANY EXPRESS OR IMPLIED * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER * IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #include "config.h" #include "defines.h" #include "common.h" /* * rewrite_l2.c can be compiled for tcprewrite or tcpbridge * in both cases, it requires a number of variables to be * available in the global options structure */ #include "tcpedit.h" #include "../common.h" #include "lib/sll.h" #include "dlt.h" #include "rewrite_l2.h" extern int maxpacket; static int check_pkt_len(tcpedit_t *tcpedit, struct pcap_pkthdr *pkthdr, int oldl2len, int newl2len); /* * Do all the layer 2 rewriting. Change ethernet header or even * rewrite mac addresses * return layer 2 length on success or 0 on don't send packet * or -1 on critical failure */ int rewrite_l2(tcpedit_t *tcpedit, struct pcap_pkthdr **pkthdr, u_char **pktdata, int direction) { u_char *l2data = NULL; /* ptr to the user specified layer2 data if any */ int newl2len = 0; pcap_t *pcap; /* do we need a ptr for l2data ? */ if (tcpedit->l2.dlt == DLT_USER) { if (direction == CACHE_SECONDARY) { l2data = tcpedit->l2.data2; } else { l2data = tcpedit->l2.data1; } } pcap = direction == CACHE_PRIMARY ? tcpedit->runtime.pcap1 : tcpedit->runtime.pcap2; /* * figure out what the CURRENT packet encapsulation is and we'll call * the appropriate function to: * 1) resize the L2 header * 2) copy over existing L2 header info (protocol, MAC's) to a new * standard 802.3 ethernet header where applicable * We do NOT apply any src/dst mac rewriting, as that is common * to all conversions, so that happens at the bottom of this function */ switch (pcap_datalink(pcap)) { case DLT_EN10MB: /* Standard 802.3 Ethernet */ newl2len = rewrite_en10mb(tcpedit, pktdata, pkthdr, l2data); break; case DLT_LINUX_SLL: /* Linux Cooked sockets */ newl2len = rewrite_linux_sll(tcpedit, pktdata, pkthdr, l2data); break; case DLT_RAW: /* No ethernet header, raw IP */ newl2len = rewrite_raw(tcpedit, pktdata, pkthdr, l2data); break; case DLT_C_HDLC: /* Cisco HDLC */ newl2len = rewrite_c_hdlc(tcpedit, pktdata, pkthdr, l2data); break; default: tcpedit_seterr(tcpedit, "Unsupported DLT type: %d", pcap_datalink(pcap)); return -1; } /* switch (dlt) */ /* if newl2len == 0, then return zero so we don't send the packet */ if (! newl2len) return 0; /* * Okay... we've got our new layer 2 header * if required. The next question, is do we have to * replace the src/dst MAC?? */ if (direction == CACHE_SECONDARY) { if (tcpedit->mac_mask & TCPEDIT_MAC_MASK_SMAC2) { memcpy(*pktdata + ETHER_ADDR_LEN, tcpedit->intf2_smac, ETHER_ADDR_LEN); } if (tcpedit->mac_mask & TCPEDIT_MAC_MASK_DMAC2) { memcpy(*pktdata, tcpedit->intf2_dmac, ETHER_ADDR_LEN); } } else { if (tcpedit->mac_mask & TCPEDIT_MAC_MASK_SMAC1) { memcpy(*pktdata + ETHER_ADDR_LEN, tcpedit->intf1_smac, ETHER_ADDR_LEN); } if (tcpedit->mac_mask & TCPEDIT_MAC_MASK_DMAC1) { memcpy(*pktdata, tcpedit->intf1_dmac, ETHER_ADDR_LEN); } } /* return the updated layer 2 len */ return (newl2len); } /* * All of these functions return the NEW layer two length and update the * total packet length in (*pkthdr)->caplen */ /* * logic to rewrite packets using DLT_EN10MB * returns -1 on error * returns 0 un unable to rewrite * returns length of new layer 2 header on success */ int rewrite_en10mb(tcpedit_t *tcpedit, u_char **pktdata, struct pcap_pkthdr **pkthdr, u_char *l2data) { eth_hdr_t *eth_hdr = NULL; vlan_hdr_t *vlan_hdr = NULL; int oldl2len = 0, newl2len = 0, lendiff; u_char tmpbuff[MAXPACKET]; /* * is the header ethernet or 802.1q? */ eth_hdr = (eth_hdr_t *)(*pktdata); if (eth_hdr->ether_type == ETHERTYPE_VLAN) { newl2len = oldl2len = LIBNET_802_1Q_H; } else { newl2len = oldl2len = LIBNET_ETH_H; } switch (tcpedit->l2.dlt) { case DLT_USER: /* track the new L2 len */ newl2len = tcpedit->l2.len; if (! check_pkt_len(tcpedit, *pkthdr, oldl2len, newl2len)) return 0; /* unable to send packet */ /* * remove the old header and copy our header back */ dbg(3, "Rewriting packet via --dlink..."); /* do we need a temp buff? */ if (newl2len > oldl2len) { memcpy(tmpbuff, *pktdata, (*pkthdr)->caplen); memcpy(*pktdata, l2data, tcpedit->l2.len); memcpy(*pktdata + newl2len, (tmpbuff + oldl2len), (*pkthdr)->caplen); } else if (newl2len < oldl2len) { memcpy(*pktdata, l2data, newl2len); memmove(*pktdata + newl2len, *pktdata + oldl2len, (*pkthdr)->caplen); } else { // same size memcpy(*pktdata, l2data, newl2len); } break; case DLT_VLAN: /* are we adding/modifying a VLAN header? */ if (tcpedit->vlan == TCPEDIT_VLAN_ADD) { newl2len = LIBNET_802_1Q_H; if (! check_pkt_len(tcpedit, *pkthdr, oldl2len, newl2len)) return 0; /* unable to send packet */ vlan_hdr = (vlan_hdr_t *)*pktdata; /* do we modify the VLAN header? */ if (oldl2len == newl2len) { /* user must always specify a tag */ vlan_hdr->vlan_priority_c_vid |= htons((u_int16_t)tcpedit->l2.vlan_tag & LIBNET_802_1Q_VIDMASK); /* these are optional */ if (tcpedit->l2.vlan_pri) vlan_hdr->vlan_priority_c_vid |= htons((u_int16_t)tcpedit->l2.vlan_pri) << 13; if (tcpedit->l2.vlan_cfi) vlan_hdr->vlan_priority_c_vid |= htons((u_int16_t)tcpedit->l2.vlan_cfi) << 12; } /* else we are adding a VLAN header */ else if (oldl2len == LIBNET_ETH_H) { /* zero out our L2 header */ memset(tmpbuff, 0, newl2len); /* copy the dst/src MAC's over to our temp buffer */ memcpy(tmpbuff, *pktdata, ETHER_ADDR_LEN * 2); vlan_hdr = (vlan_hdr_t *)tmpbuff; eth_hdr = (eth_hdr_t *)*pktdata; /* these fields are always set this way */ vlan_hdr->vlan_tpi = htons(ETHERTYPE_VLAN); vlan_hdr->vlan_len = eth_hdr->ether_type; /* user must always specify a tag */ vlan_hdr->vlan_priority_c_vid |= htons((u_int16_t)tcpedit->l2.vlan_tag & LIBNET_802_1Q_VIDMASK); /* other things are optional */ if (tcpedit->l2.vlan_pri) vlan_hdr->vlan_priority_c_vid |= htons((u_int16_t)tcpedit->l2.vlan_pri) << 13; if (tcpedit->l2.vlan_cfi) vlan_hdr->vlan_priority_c_vid |= htons((u_int16_t)tcpedit->l2.vlan_cfi) << 12; /* move around our buffers */ memcpy(&tmpbuff[newl2len], (*pktdata + oldl2len), ((*pkthdr)->caplen - oldl2len)); memcpy(*pktdata, tmpbuff, ((*pkthdr)->caplen + newl2len - oldl2len)); } else { err(1, "Uh, how are we supposed to rewrite the header when the oldl2len != LIBNET_ETH_H?"); } } else { /* remove VLAN header */ newl2len = LIBNET_ETH_H; /* we still verify packet len incase MTU has shrunk */ if (! check_pkt_len(tcpedit, *pkthdr, oldl2len, newl2len)) return 0; /* unable to send packet */ memcpy(tmpbuff, *pktdata, (*pkthdr)->caplen); eth_hdr = (eth_hdr_t *)*pktdata; vlan_hdr = (vlan_hdr_t *)tmpbuff; eth_hdr->ether_type = vlan_hdr->vlan_len; memcpy(*pktdata + LIBNET_ETH_H, (tmpbuff + LIBNET_802_1Q_H), (*pkthdr)->caplen - oldl2len); } break; case DLT_EN10MB: /* nothing to do here since we're already ethernet! */ break; default: tcpedit_seterr(tcpedit, "Invalid tcpedit->l2.dlt value: 0x%04x", tcpedit->l2.dlt); return -1; break; } /* new packet len */ lendiff = newl2len - oldl2len; (*pkthdr)->caplen += lendiff; (*pkthdr)->len += lendiff; return newl2len; } /* * logic to rewrite packets using DLT_RAW * returns -1 on error * returns 0 un unable to rewrite * returns length of new layer 2 header on success */ int rewrite_raw(tcpedit_t *tcpedit, u_char **pktdata, struct pcap_pkthdr **pkthdr, u_char *l2data) { int oldl2len = 0, newl2len = 0, lendiff; u_char tmpbuff[MAXPACKET]; vlan_hdr_t *vlan_hdr = NULL; eth_hdr_t *eth_hdr = NULL; /* we have no ethernet header, but we know we're IP */ switch (tcpedit->l2.dlt) { case DLT_USER: newl2len = tcpedit->l2.len; if (! check_pkt_len(tcpedit, *pkthdr, oldl2len, newl2len)) return 0; /* unable to send packet */ /* * add our user specified header */ dbg(3, "Rewriting packet via --dlink..."); /* backup the old packet */ memcpy(tmpbuff, *pktdata, (*pkthdr)->caplen); memcpy(*pktdata, l2data, newl2len); memcpy(*pktdata + newl2len, tmpbuff, (*pkthdr)->caplen); break; case DLT_VLAN: newl2len = LIBNET_802_1Q_H; if (! check_pkt_len(tcpedit, *pkthdr, oldl2len, newl2len)) return 0; /* unable to send packet */ /* prep a 802.1q tagged frame */ /* make space for the header */ memcpy(tmpbuff, *pktdata, (*pkthdr)->caplen); memcpy(*pktdata + LIBNET_802_1Q_H, tmpbuff, (*pkthdr)->caplen); vlan_hdr = (vlan_hdr_t *)*pktdata; /* these fields are always set this way */ vlan_hdr->vlan_tpi = ETHERTYPE_VLAN; vlan_hdr->vlan_len = tcpedit->l2proto; /* user must always specify a tag */ vlan_hdr->vlan_priority_c_vid |= htons((u_int16_t)tcpedit->l2.vlan_tag & LIBNET_802_1Q_VIDMASK); /* other things are optional */ if (tcpedit->l2.vlan_pri) vlan_hdr->vlan_priority_c_vid |= htons((u_int16_t)tcpedit->l2.vlan_pri) << 13; if (tcpedit->l2.vlan_cfi) vlan_hdr->vlan_priority_c_vid |= htons((u_int16_t)tcpedit->l2.vlan_cfi) << 12; /* new packet len */ newl2len = LIBNET_802_1Q_H; break; case DLT_EN10MB: newl2len = LIBNET_ETH_H; if (! check_pkt_len(tcpedit, *pkthdr, oldl2len, newl2len)) return 0; /* unable to send packet */ /* make room for L2 header */ memmove(*pktdata + LIBNET_ETH_H, *pktdata, (*pkthdr)->caplen); /* these fields are always set this way */ eth_hdr = (eth_hdr_t *)*pktdata; eth_hdr->ether_type = tcpedit->l2proto; break; default: tcpedit_seterr(tcpedit, "Invalid tcpedit->l2.dlt value: 0x%x", tcpedit->l2.dlt); return -1; break; } /* new packet len */ lendiff = newl2len - oldl2len; (*pkthdr)->caplen += lendiff; (*pkthdr)->len += lendiff; return newl2len; } /* * logic to rewrite packets using DLT_LINUX_SLL * returns -1 on error * returns 0 un unable to rewrite * returns length of new layer 2 header on success */ int rewrite_linux_sll(tcpedit_t *tcpedit, u_char **pktdata, struct pcap_pkthdr **pkthdr, u_char *l2data) { int oldl2len = 0, newl2len = 0, lendiff; u_char tmpbuff[MAXPACKET]; vlan_hdr_t *vlan_hdr = NULL; eth_hdr_t *eth_hdr = NULL; sll_hdr_t *sll_hdr = NULL; newl2len = oldl2len = SLL_HDR_LEN; switch (tcpedit->l2.dlt) { case DLT_USER: newl2len = tcpedit->l2.len; if (! check_pkt_len(tcpedit, *pkthdr, oldl2len, newl2len)) return 0; /* unable to send packet */ /* * add our user specified header */ dbg(3, "Rewriting packet via --dlink..."); /* backup the old packet */ memcpy(tmpbuff, *pktdata, (*pkthdr)->caplen); memcpy(*pktdata, l2data, newl2len); memcpy(*pktdata + newl2len, tmpbuff, (*pkthdr)->caplen); break; case DLT_VLAN: /* prep a 802.1q tagged frame */ newl2len = LIBNET_802_1Q_H; if (! check_pkt_len(tcpedit, *pkthdr, oldl2len, newl2len)) return 0; /* unable to send packet */ /* make space for the header */ memcpy(tmpbuff, *pktdata, (*pkthdr)->caplen); memcpy(*pktdata + LIBNET_802_1Q_H, tmpbuff, (*pkthdr)->caplen - oldl2len); vlan_hdr = (vlan_hdr_t *)*pktdata; sll_hdr = (sll_hdr_t *)tmpbuff; /* these fields are always set this way */ vlan_hdr->vlan_tpi = ETHERTYPE_VLAN; vlan_hdr->vlan_len = sll_hdr->sll_protocol; /* the sll header might have a src mac */ if (sll_hdr->sll_halen == ETHER_ADDR_LEN) memcpy(vlan_hdr->vlan_shost, sll_hdr->sll_addr, ETHER_ADDR_LEN); /* user must always specify a tag */ vlan_hdr->vlan_priority_c_vid |= htons((u_int16_t)(tcpedit->l2.vlan_tag & LIBNET_802_1Q_VIDMASK)); /* other things are optional */ if (tcpedit->l2.vlan_pri) vlan_hdr->vlan_priority_c_vid |= htons((u_int16_t)tcpedit->l2.vlan_pri) << 13; if (tcpedit->l2.vlan_cfi) vlan_hdr->vlan_priority_c_vid |= htons((u_int16_t)tcpedit->l2.vlan_cfi) << 12; break; case DLT_EN10MB: newl2len = LIBNET_ETH_H; if (! check_pkt_len(tcpedit, *pkthdr, oldl2len, newl2len)) return 0; /* unable to send packet */ /* make room for L2 header */ memcpy(tmpbuff, *pktdata, (*pkthdr)->caplen); memcpy(*pktdata + LIBNET_ETH_H, (tmpbuff + oldl2len), (*pkthdr)->caplen - oldl2len); /* these fields are always set this way */ sll_hdr = (sll_hdr_t *)tmpbuff; eth_hdr = (eth_hdr_t *)*pktdata; eth_hdr->ether_type = sll_hdr->sll_protocol; /* the sll header might have a src mac */ if (sll_hdr->sll_halen == ETHER_ADDR_LEN) memcpy(eth_hdr->ether_shost, sll_hdr->sll_addr, ETHER_ADDR_LEN); break; default: tcpedit_seterr(tcpedit, "Invalid tcpedit->l2.dlt value: 0x%x", tcpedit->l2.dlt); return -1; break; } /* new packet len */ lendiff = newl2len - oldl2len; (*pkthdr)->caplen += lendiff; (*pkthdr)->len += lendiff; return newl2len; } /* * logic to rewrite packets using DLT_C_HDLC * returns -1 on error or 0 on unable to send packet. Returns length of new * layer 2 header */ int rewrite_c_hdlc(tcpedit_t *tcpedit, u_char **pktdata, struct pcap_pkthdr **pkthdr, u_char *l2data) { int oldl2len = 0, newl2len = 0, lendiff; u_char tmpbuff[MAXPACKET]; hdlc_hdr_t *hdlc_hdr = NULL; eth_hdr_t *eth_hdr = NULL; vlan_hdr_t *vlan_hdr = NULL; newl2len = oldl2len = dlt2layer2len(tcpedit, DLT_C_HDLC); switch (tcpedit->l2.dlt) { case DLT_USER: /* track the new L2 len */ newl2len = dlt2layer2len(tcpedit, DLT_USER); if (! check_pkt_len(tcpedit, *pkthdr, oldl2len, newl2len)) return 0; /* unable to send packet */ /* * add our user specified header */ dbg(3, "Rewriting packet via --dlink..."); /* backup the old packet */ memcpy(tmpbuff, *pktdata, (*pkthdr)->caplen); memcpy(*pktdata, l2data, tcpedit->l2.len); memcpy(*pktdata + tcpedit->l2.len, (tmpbuff + oldl2len), (*pkthdr)->caplen - oldl2len); break; case DLT_VLAN: /* new l2 len */ newl2len = dlt2layer2len(tcpedit, DLT_VLAN); if (! check_pkt_len(tcpedit, *pkthdr, oldl2len, newl2len)) return 0; /* unable to send packet */ memcpy(tmpbuff, *pktdata, (*pkthdr)->caplen); hdlc_hdr = (hdlc_hdr_t *)tmpbuff; vlan_hdr = (vlan_hdr_t *)*pktdata; memcpy(*pktdata + LIBNET_802_1Q_H, tmpbuff + oldl2len, (*pkthdr)->caplen - oldl2len); vlan_hdr->vlan_tpi = ETHERTYPE_VLAN; vlan_hdr->vlan_len = hdlc_hdr->protocol; /* user must always specify a tag */ vlan_hdr->vlan_priority_c_vid |= htons((u_int16_t)tcpedit->l2.vlan_tag & LIBNET_802_1Q_VIDMASK); /* other things are optional */ if (tcpedit->l2.vlan_pri) vlan_hdr->vlan_priority_c_vid |= htons((u_int16_t)tcpedit->l2.vlan_pri) << 13; if (tcpedit->l2.vlan_cfi) vlan_hdr->vlan_priority_c_vid |= htons((u_int16_t)tcpedit->l2.vlan_cfi) << 12; break; case DLT_EN10MB: newl2len = dlt2layer2len(tcpedit, DLT_EN10MB); if (! check_pkt_len(tcpedit, *pkthdr, oldl2len, newl2len)) return 0; /* unable to send packet */ memcpy(tmpbuff, *pktdata, (*pkthdr)->caplen); hdlc_hdr = (hdlc_hdr_t *)tmpbuff; eth_hdr = (eth_hdr_t *)*pktdata; memcpy(*pktdata + LIBNET_ETH_H, tmpbuff + oldl2len, (*pkthdr)->caplen - oldl2len); eth_hdr->ether_type = hdlc_hdr->protocol; break; default: tcpedit_seterr(tcpedit, "Invalid tcpedit->l2.dlt value: 0x%x", tcpedit->l2.dlt); return -1; break; } /* new packet len */ lendiff = newl2len - oldl2len; (*pkthdr)->caplen += lendiff; (*pkthdr)->len += lendiff; return newl2len; } /* * will the new packet be too big? * If so, we have to change the (*pkthdr)->caplen to be artifically lower * so we don't go beyond tcpedit->maxpacket */ static int check_pkt_len(tcpedit_t *tcpedit, struct pcap_pkthdr *pkthdr, int oldl2len, int newl2len) { /* * is new packet too big? */ if ((pkthdr->caplen - oldl2len + newl2len) > (u_int)tcpedit->maxpacket) { if (tcpedit->fixlen) { warnx("Packet length (%u) is greater then MTU (%u); " "truncating packet.", (pkthdr->caplen - oldl2len + newl2len), tcpedit->maxpacket); /* set our packet length to the max packet size */ pkthdr->caplen = tcpedit->maxpacket; } else { warnx("Packet length (%u) is greater then MTU (%u); " "skipping packet.", (pkthdr->caplen - oldl2len + newl2len), tcpedit->maxpacket); return (0); } } /* all is fine */ return 1; } /* Local Variables: mode:c indent-tabs-mode:nil c-basic-offset:4 End: */