git.sesse.net Git - greproxy/blob - greproxy.cpp

   1 #include <stdio.h>
   2 #include <string.h>
   3 #include <stdlib.h>
   4 #include <sys/socket.h>
   5 #include <arpa/inet.h>
   6 #include <netinet/in.h>
   7
   8 #include <map>
   9 #include <string>
  10 #include <queue>
  11
  12 using namespace std;
  13
  14 struct gre_header {
  15         uint8_t reserved0_hi : 4;
  16         uint8_t has_seq : 1;
  17         uint8_t has_key : 1;
  18         uint8_t unused : 1;
  19         uint8_t has_checksum : 1;
  20
  21         uint8_t version : 3;
  22         uint8_t reserved0_lo: 5;
  23
  24         uint16_t protocol_type;
  25 };
  26
  27 in6_addr get_addr(const char *str) {
  28         in6_addr ret;
  29         if (inet_pton(AF_INET6, str, &ret) != 1) {
  30                 fprintf(stderr, "Could not parse %s\n", str);
  31                 exit(1);
  32         }
  33         return ret;
  34 }
  35
  36 struct GREPacket {
  37         int seq;
  38         string data;
  39
  40         bool operator> (const GREPacket &other) const {
  41                 return seq > other.seq;
  42         }
  43 };
  44
  45 class Reorderer {
  46 public:
  47         Reorderer(int sock, const in6_addr &dst);
  48         void handle_packet(const char* buf, size_t size, int seq);
  49
  50 private:
  51         void send_packet(const string &data);
  52
  53         int sock;
  54         sockaddr_in6 dstaddr;
  55         int last_seq;
  56
  57         priority_queue<GREPacket, vector<GREPacket>, greater<GREPacket>> packet_buffer;
  58         map<int, int> ccs;
  59 };
  60
  61 Reorderer::Reorderer(int sock, const in6_addr &dst)
  62         : sock(sock), last_seq(-1)
  63 {
  64         memset(&dstaddr, 0, sizeof(dstaddr));
  65         dstaddr.sin6_family = AF_INET6;
  66         dstaddr.sin6_addr = dst;
  67 }
  68
  69 #define PACKET_BUFFER_SIZE 100
  70
  71 void Reorderer::handle_packet(const char* buf, size_t size, int seq)
  72 {
  73         if (packet_buffer.size() >= PACKET_BUFFER_SIZE) {
  74                 printf("Gave up waiting for packets [%d,%d>\n",
  75                         last_seq + 1, packet_buffer.top().seq);
  76                 last_seq = packet_buffer.top().seq - 1;
  77         }
  78
  79         GREPacket packet;
  80         packet.seq = seq;
  81         packet.data = string(buf, buf + size);
  82         packet_buffer.push(packet);
  83
  84         bool silence = false;
  85         while (!packet_buffer.empty() &&
  86                (last_seq == -1 || packet_buffer.top().seq <= last_seq + 1)) {
  87                 int front_seq = packet_buffer.top().seq;
  88                 if (front_seq < last_seq + 1) {
  89                         printf("Duplicate packet or way out-of-order: seq=%d front_seq=%d\n",
  90                                 seq, front_seq);
  91                         packet_buffer.pop();
  92                         continue;
  93                 }
  94                 //if (packet_buffer.size() > 1) {
  95                 //      printf("seq=%d (REORDER %d)\n", front_seq, int(packet_buffer.size()));
  96                 //} else {
  97                 //      printf("seq=%d\n", front_seq);
  98                 //}
  99                 const string &data = packet_buffer.top().data;
 100                 send_packet(data);
 101                 packet_buffer.pop();
 102                 last_seq = front_seq;
 103                 if (!silence && !packet_buffer.empty()) {
 104                         printf("Reordering with packet buffer size %d: seq=%d new_front_seq=%d\n", int(packet_buffer.size()), front_seq, packet_buffer.top().seq);
 105                         silence = true;
 106                 }
 107         }
 108 }
 109
 110 void Reorderer::send_packet(const string &data)
 111 {
 112         if (data.size() == 1352) {
 113                 for (int i = 0; i < 7; ++i) {
 114                         const char *pkt = &data[i * 188 + 36];
 115                         int pid = (ntohl(*(uint32_t *)(pkt)) & 0x1fff00) >> 8;
 116                         int has_payload = pkt[3] & 0x10;
 117                         int cc = pkt[3] & 0xf;
 118                         if (has_payload) {
 119                                 int last_cc = ccs[pid];
 120                                 if (cc != ((last_cc + 1) & 0xf)) {
 121                                         printf("Pid %d discontinuity (expected %d, got %d)\n", pid, (last_cc + 1) & 0xf, cc);
 122                                 }
 123                                 ccs[pid] = cc;
 124                         }
 125                 }
 126         }
 127         if (sendto(sock, data.data(), data.size(), 0, (sockaddr *)&dstaddr, sizeof(dstaddr)) == -1) {
 128                 perror("sendto");
 129                 return;
 130         }
 131 }
 132
 133 int main(int argc, char **argv)
 134 {
 135         int sock = socket(AF_INET6, SOCK_RAW, IPPROTO_GRE);
 136         if (sock == -1) {
 137                 perror("socket");
 138                 exit(1);
 139         }
 140
 141         sockaddr_in6 my_addr;
 142         memset(&my_addr, 0, sizeof(my_addr));
 143         my_addr.sin6_family = AF_INET6;
 144         my_addr.sin6_addr = get_addr(argv[3]);
 145         if (bind(sock, (sockaddr *)&my_addr, sizeof(my_addr)) == -1) {
 146                 perror("bind");
 147                 exit(1);
 148         }
 149
 150         in6_addr addr_a = get_addr(argv[1]);
 151         in6_addr addr_b = get_addr(argv[2]);
 152         Reorderer dst_a(sock, addr_a);
 153         Reorderer dst_b(sock, addr_b);
 154
 155         for ( ;; ) {
 156                 char addrstr[256];
 157                 struct sockaddr_storage addr;
 158                 socklen_t addrlen = sizeof(addr);
 159                 char buf[4096];
 160                 int ret = recvfrom(sock, buf, sizeof(buf), 0, (struct sockaddr *)&addr, &addrlen);
 161                 if (ret == -1) {
 162                         perror("recvfrom");
 163                         exit(1);
 164                 }
 165                 if (addr.ss_family != AF_INET6) {
 166                         // ignore
 167                         //inet_ntop(AF_INET, &((struct sockaddr_in *)&addr)->sin_addr, addrstr, sizeof(addrstr));
 168                         continue;
 169                 }
 170                 struct in6_addr *addr6 = &((struct sockaddr_in6 *)&addr)->sin6_addr;
 171                 if (memcmp(addr6, &addr_a, sizeof(*addr6)) != 0 &&
 172                     memcmp(addr6, &addr_b, sizeof(*addr6)) != 0) {
 173                         // ignore
 174                         continue;
 175                 }
 176                 inet_ntop(AF_INET6, addr6, addrstr, sizeof(addrstr));
 177                 gre_header* gre = (gre_header *)buf;
 178                 //printf("GREv%d of %d bytes from %s: %02x %02x %02x %02x\n", gre->version, ret, addrstr,
 179                 //      buf[0], buf[1], buf[2], buf[3]);
 180                 //printf("  has_checksum=%d, has_key=%d, has_seq=%d\n", gre->has_checksum, gre->has_key, gre->has_seq);
 181
 182                 char* ptr = buf + sizeof(gre_header);
 183                 if (gre->has_checksum) {
 184                         ptr += 4;
 185                 }
 186                 if (gre->has_key) {
 187                         ptr += 4;
 188                 }
 189                 uint32_t seq;
 190                 if (gre->has_seq) {
 191                         seq = ntohl(*(uint32_t *)ptr);
 192                 //      printf("  seq=%d\n", seq);
 193                 }
 194                 if (memcmp(addr6, &addr_a, sizeof(*addr6)) == 0) {
 195                         // comes from A, send to B
 196                         dst_b.handle_packet(buf, ret, seq);
 197                 } else {
 198                         // comes from B, send to A
 199                         dst_a.handle_packet(buf, ret, seq);
 200                 }
 201         }
 202 }