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>\n",a0);
  72         exit(254);
  73 }
  74
  75
  76 int main(int argc, char *argv[])
  77 {
  78         int     fdtap, fdraw;
  79
  80         if (argc != 4)
  81                 usage(argv[0]);
  82
  83         fd_set fds;
  84         struct  sockaddr_in sin;
  85         in_addr_t locip;
  86         in_addr_t remip;
  87
  88         union {
  89                 uint8_t buf[65536];
  90                 struct iphdr ip;
  91         } pkt;
  92
  93         fdtap = opentun(IFF_TAP|IFF_NO_PI, argv[1]);
  94         locip = inet_addr(argv[2]);
  95         remip = inet_addr(argv[3]);
  96
  97         /* eoip socket */
  98         fdraw = socket(AF_INET, SOCK_RAW, 47);
  99         sin.sin_family = AF_INET;
 100         sin.sin_addr.s_addr = locip;
 101         sin.sin_port = htons(47);
 102         assert(bind(fdraw, (struct sockaddr*)&sin, sizeof(sin))==0);
 103
 104         /* sending socket */
 105         memset(&sin, 0, sizeof(sin));
 106         sin.sin_family = AF_INET;
 107         sin.sin_addr.s_addr = remip;
 108
 109
 110         FD_ZERO(&fds);
 111         ioctl(fdtap, TUNSETNOCSUM, 1);
 112
 113         /* ad nausea */
 114         while (1) {
 115                 int len;
 116                 uint8_t *p;
 117                 struct timeval tv;
 118                 uint32_t tid;
 119
 120                 FD_SET(fdtap, &fds);
 121                 FD_SET(fdraw, &fds);
 122
 123                 tv.tv_sec = 1;
 124                 tv.tv_usec = 0;
 125
 126                 select(max(fdtap,fdraw)+1,&fds,NULL,NULL,&tv);
 127
 128                 /* got mtk GRE packet */
 129                 if (FD_ISSET(fdraw, &fds)) {
 130                         len = recv(fdraw, pkt.buf, sizeof(pkt), 0);
 131
 132                         /* but not for us */
 133                         if (pkt.ip.daddr != locip) continue;
 134                         if (pkt.ip.saddr != remip) continue;
 135
 136                         /* move past ip header */
 137                         p = pkt.buf + pkt.ip.ihl*4; len -= pkt.ip.ihl*4;
 138                         DEBUG("len is=%d\n", len);
 139
 140                         /* check its really eoip */
 141                         if (memcmp(p, GREHDR, GREHDRSZ)) continue;
 142
 143                         /* move past header */
 144                         p += GREHDRSZ + 4;
 145                         len -= GREHDRSZ + 4;
 146                         if (len < 0) continue;
 147
 148                         tid = ((uint16_t *) p)[-1]; /* tunnel id, actually 802.1q tag number */
 149
 150                         /* IP hdr/eoip length mismatch */
 151                         if (len != ntohs(((uint16_t*)p)[-2])) {
 152                                 DEBUG("%d %d\n",len,ntohs(((uint16_t*)p)[-2]));
 153                                 continue;
 154                         }
 155 #if DUMPING
 156                         int i;
 157                         DEBUG("len=%d\n",len);
 158                         for (i = 0; i < len; i++) DEBUG("%02hhx ", p[i]);
 159                         DEBUG("\n\n");
 160 #endif
 161                         /* shift src/dst mac by 4 bytes */
 162                         memmove(p-4,p,12);
 163                         p[12-4] = 0x81; p[13-4] = 0x00;
 164                         p[14-4] = /*0x40 | */(tid>>8);
 165                         p[15-4] = tid & 0xff;
 166                         write(fdtap, p-4, len+4);
 167                 }
 168
 169
 170                 /* local tap */
 171                 if (FD_ISSET(fdtap, &fds)) {
 172                         union {
 173                                 struct gre_packet gre;
 174                                 uint8_t mybuf[65536];
 175                         } sbuf;
 176
 177                         p = pkt.buf;
 178                         len = read(fdtap, p, sizeof(pkt));
 179                         /* some sanity */
 180                         if (len <= 14) continue;
 181                         /* only tagged packets please */
 182                         if (p[12] != 0x81 || p[13] != 0x00) continue;
 183                         tid = ((p[14] << 8)&0xf00) | p[15];
 184
 185                         sin.sin_port = htons(47);
 186                         memcpy(sbuf.gre.magic, GREHDR, GREHDRSZ);
 187                         sbuf.gre.len = htons(len);
 188                         sbuf.gre.tid = tid; /* little endian! */
 189                         memcpy(sbuf.gre.data, p, len);
 190                         sendto(fdraw, sbuf.mybuf, len+8, 0, (struct sockaddr*) &sin, sizeof(sin));
 191                 }
 192         }
 193 }