evlan.c

   1 /*
   2  * 802.1q vlan -> eoip tagging gateway
   3  * (c) 2008-2011 Karel Tuma <karel.tuma@pilsfree.net>
   4  *
   5  * This program is free software; you can redistribute it and/or modify
   6  * it under the terms of the GNU General Public License as published by
   7  * the Free Software Foundation; either version 2, or (at your option)
   8  * any later version.
   9  *
  10  * You should have received a copy of the GNU General Public License
  11  * (for example /usr/src/linux/COPYING); if not, write to the Free
  12  * Software Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  13  */
  14
  15 #include <stdlib.h>
  16 #include <stdio.h>
  17 #include <fcntl.h>
  18 #include <unistd.h>
  19 #include <sys/ioctl.h>
  20 #include <sys/time.h>
  21 #include <sys/types.h>
  22 #include <sys/socket.h>
  23
  24 #include <linux/if.h>
  25 #include <linux/if_tun.h>
  26 #include <string.h>
  27 #include <stdarg.h>
  28 #include <assert.h>
  29 #include <time.h>
  30
  31 #include <netinet/ip.h>
  32 #include <arpa/inet.h>
  33
  34 #define max(a,b) ((a)>(b) ? (a):(b))
  35
  36 #if 0
  37 #define DEBUG printf
  38 #else
  39 #define DEBUG(x...)
  40 #endif
  41 #define LOG(msg...) {printf(msg); printf("\n");}
  42
  43 /* gre header */
  44 #define GREHDR "\x20\x01\x64\x00"
  45 #define GREHDRSZ 4
  46
  47 struct gre_packet {
  48         uint8_t magic[4];
  49         uint16_t len;
  50         uint16_t tid;
  51         char data[0];
  52 };
  53
  54 static  int opentun(int flags, char *name)
  55 {
  56         struct ifreq ifr;
  57         int fd = open("/dev/net/tun", O_RDWR);
  58         assert(fd>=0);
  59         memset(&ifr,0,sizeof(ifr));
  60         ifr.ifr_flags = flags;
  61         strncpy(ifr.ifr_name, name, IFNAMSIZ);
  62 //      write(2, &ifr, sizeof(ifr));
  63         ioctl(fd, TUNSETIFF, (void *) &ifr);
  64 //      ioctl(fd, TUNSETPERSIST, 1);
  65         return fd;
  66 }
  67
  68 void usage(char *a0)
  69 {
  70         fprintf(stderr,
  71                 "%s <intf> <localip> <remoteip - eoip target> [ignored vlan ids]\n",a0);
  72         exit(254);
  73 }
  74
  75
  76 int main(int argc, char *argv[])
  77 {
  78         int     fdtap, fdraw, i;
  79         unsigned char ignore[4096];
  80
  81         if (argc < 4)
  82                 usage(argv[0]);
  83         memset(ignore,0,sizeof(ignore));
  84         for (i = 3; i < argc; i++)
  85                 ignore[atoi(argv[i])]=1;
  86
  87         fd_set fds;
  88         struct  sockaddr_in sin;
  89         in_addr_t locip;
  90         in_addr_t remip;
  91
  92         union {
  93                 uint8_t buf[65536];
  94                 struct iphdr ip;
  95         } pkt;
  96
  97         fdtap = opentun(IFF_TAP|IFF_NO_PI, argv[1]);
  98         locip = inet_addr(argv[2]);
  99         remip = inet_addr(argv[3]);
 100
 101         /* eoip socket */
 102         fdraw = socket(AF_INET, SOCK_RAW, 47);
 103         sin.sin_family = AF_INET;
 104         sin.sin_addr.s_addr = locip;
 105         sin.sin_port = htons(47);
 106         assert(bind(fdraw, (struct sockaddr*)&sin, sizeof(sin))==0);
 107
 108         /* sending socket */
 109         memset(&sin, 0, sizeof(sin));
 110         sin.sin_family = AF_INET;
 111         sin.sin_addr.s_addr = remip;
 112
 113
 114         FD_ZERO(&fds);
 115         ioctl(fdtap, TUNSETNOCSUM, 1);
 116
 117         /* ad nausea */
 118         while (1) {
 119                 int len;
 120                 uint8_t *p;
 121                 struct timeval tv;
 122                 uint32_t tid;
 123
 124                 FD_SET(fdtap, &fds);
 125                 FD_SET(fdraw, &fds);
 126
 127                 tv.tv_sec = 1;
 128                 tv.tv_usec = 0;
 129
 130                 select(max(fdtap,fdraw)+1,&fds,NULL,NULL,&tv);
 131
 132                 /* got mtk GRE packet */
 133                 if (FD_ISSET(fdraw, &fds)) {
 134                         len = recv(fdraw, pkt.buf, sizeof(pkt), 0);
 135
 136                         /* but not for us */
 137                         if (pkt.ip.daddr != locip) continue;
 138                         if (pkt.ip.saddr != remip) continue;
 139
 140                         /* move past ip header */
 141                         p = pkt.buf + pkt.ip.ihl*4; len -= pkt.ip.ihl*4;
 142                         DEBUG("len is=%d\n", len);
 143
 144                         /* check its really eoip */
 145                         if (memcmp(p, GREHDR, GREHDRSZ)) continue;
 146
 147                         /* move past header */
 148                         p += GREHDRSZ + 4;
 149                         len -= GREHDRSZ + 4;
 150                         if (len < 0) continue;
 151
 152                         tid = ((uint16_t *) p)[-1]; /* tunnel id, actually 802.1q tag number */
 153                         if (tid > 4095) continue;
 154                         if (ignore[tid]) continue;
 155
 156                         /* IP hdr/eoip length mismatch */
 157                         if (len != ntohs(((uint16_t*)p)[-2])) {
 158                                 DEBUG("%d %d\n",len,ntohs(((uint16_t*)p)[-2]));
 159                                 continue;
 160                         }
 161 #if DUMPING
 162                         int i;
 163                         DEBUG("len=%d\n",len);
 164                         for (i = 0; i < len; i++) DEBUG("%02hhx ", p[i]);
 165                         DEBUG("\n\n");
 166 #endif
 167                         /* shift src/dst mac by 4 bytes */
 168                         memmove(p-4,p,12);
 169                         p[12-4] = 0x81; p[13-4] = 0x00;
 170                         p[14-4] = /*0x40 | */(tid>>8);
 171                         p[15-4] = tid & 0xff;
 172                         write(fdtap, p-4, len+4);
 173                 }
 174
 175
 176                 /* local tap */
 177                 if (FD_ISSET(fdtap, &fds)) {
 178                         union {
 179                                 struct gre_packet gre;
 180                                 uint8_t mybuf[65536];
 181                         } sbuf;
 182
 183                         p = pkt.buf;
 184                         len = read(fdtap, p, sizeof(pkt));
 185                         /* some sanity */
 186                         if (len <= 16) continue;
 187                         /* only tagged packets please */
 188                         if (p[12] != 0x81 || p[13] != 0x00) continue;
 189                         tid = ((p[14] << 8)&0xf00) | p[15];
 190                         if (ignore[tid]) continue;
 191                         memcpy(sbuf.gre.data, p, 12); /* src & dst mac */
 192                         memcpy(sbuf.gre.data + 12, p + 16, len-16); /* skip the tag */
 193
 194                         sin.sin_port = htons(47);
 195                         memcpy(sbuf.gre.magic, GREHDR, GREHDRSZ);
 196                         sbuf.gre.len = htons(len - 4);
 197                         sbuf.gre.tid = tid; /* little endian! */
 198                         sendto(fdraw, sbuf.mybuf, len+8-4, 0, (struct sockaddr*) &sin, sizeof(sin));
 199                 }
 200         }
 201 }