--- /dev/null
+#include <stdio.h>
+#include <string.h>
+#include <stdlib.h>
+#include <sys/socket.h>
+#include <arpa/inet.h>
+#include <netinet/in.h>
+
+#include <map>
+#include <string>
+#include <queue>
+
+using namespace std;
+
+struct gre_header {
+ uint8_t reserved0_hi : 4;
+ uint8_t has_seq : 1;
+ uint8_t has_key : 1;
+ uint8_t unused : 1;
+ uint8_t has_checksum : 1;
+
+ uint8_t version : 3;
+ uint8_t reserved0_lo: 5;
+
+ uint16_t protocol_type;
+};
+
+in6_addr get_addr(const char *str) {
+ in6_addr ret;
+ if (inet_pton(AF_INET6, str, &ret) != 1) {
+ fprintf(stderr, "Could not parse %s\n", str);
+ exit(1);
+ }
+ return ret;
+}
+
+struct GREPacket {
+ int seq;
+ string data;
+
+ bool operator> (const GREPacket &other) const {
+ return seq > other.seq;
+ }
+};
+
+class Reorderer {
+public:
+ Reorderer(int sock, const in6_addr &dst);
+ void handle_packet(const char* buf, size_t size, int seq);
+
+private:
+ void send_packet(const string &data);
+
+ int sock;
+ sockaddr_in6 dstaddr;
+ int last_seq;
+
+ priority_queue<GREPacket, vector<GREPacket>, greater<GREPacket>> packet_buffer;
+ map<int, int> ccs;
+};
+
+Reorderer::Reorderer(int sock, const in6_addr &dst)
+ : sock(sock), last_seq(-1)
+{
+ memset(&dstaddr, 0, sizeof(dstaddr));
+ dstaddr.sin6_family = AF_INET6;
+ dstaddr.sin6_addr = dst;
+}
+
+#define PACKET_BUFFER_SIZE 100
+
+void Reorderer::handle_packet(const char* buf, size_t size, int seq)
+{
+ if (packet_buffer.size() >= PACKET_BUFFER_SIZE) {
+ printf("Gave up waiting for packets [%d,%d>\n",
+ last_seq + 1, packet_buffer.top().seq);
+ last_seq = packet_buffer.top().seq - 1;
+ }
+
+ GREPacket packet;
+ packet.seq = seq;
+ packet.data = string(buf, buf + size);
+ packet_buffer.push(packet);
+
+ bool silence = false;
+ while (!packet_buffer.empty() &&
+ (last_seq == -1 || packet_buffer.top().seq <= last_seq + 1)) {
+ int front_seq = packet_buffer.top().seq;
+ if (front_seq < last_seq + 1) {
+ printf("Duplicate packet or way out-of-order: seq=%d front_seq=%d\n",
+ seq, front_seq);
+ packet_buffer.pop();
+ continue;
+ }
+ //if (packet_buffer.size() > 1) {
+ // printf("seq=%d (REORDER %d)\n", front_seq, int(packet_buffer.size()));
+ //} else {
+ // printf("seq=%d\n", front_seq);
+ //}
+ const string &data = packet_buffer.top().data;
+ send_packet(data);
+ packet_buffer.pop();
+ last_seq = front_seq;
+ if (!silence && !packet_buffer.empty()) {
+ printf("Reordering with packet buffer size %d: seq=%d new_front_seq=%d\n", int(packet_buffer.size()), front_seq, packet_buffer.top().seq);
+ silence = true;
+ }
+ }
+}
+
+void Reorderer::send_packet(const string &data)
+{
+ if (data.size() == 1352) {
+ for (int i = 0; i < 7; ++i) {
+ const char *pkt = &data[i * 188 + 36];
+ int pid = (ntohl(*(uint32_t *)(pkt)) & 0x1fff00) >> 8;
+ int has_payload = pkt[3] & 0x10;
+ int cc = pkt[3] & 0xf;
+ if (has_payload) {
+ int last_cc = ccs[pid];
+ if (cc != ((last_cc + 1) & 0xf)) {
+ printf("Pid %d discontinuity (expected %d, got %d)\n", pid, (last_cc + 1) & 0xf, cc);
+ }
+ ccs[pid] = cc;
+ }
+ }
+ }
+ if (sendto(sock, data.data(), data.size(), 0, (sockaddr *)&dstaddr, sizeof(dstaddr)) == -1) {
+ perror("sendto");
+ return;
+ }
+}
+
+int main(int argc, char **argv)
+{
+ int sock = socket(AF_INET6, SOCK_RAW, IPPROTO_GRE);
+ if (sock == -1) {
+ perror("socket");
+ exit(1);
+ }
+
+ sockaddr_in6 my_addr;
+ memset(&my_addr, 0, sizeof(my_addr));
+ my_addr.sin6_family = AF_INET6;
+ my_addr.sin6_addr = get_addr(argv[3]);
+ if (bind(sock, (sockaddr *)&my_addr, sizeof(my_addr)) == -1) {
+ perror("bind");
+ exit(1);
+ }
+
+ in6_addr addr_a = get_addr(argv[1]);
+ in6_addr addr_b = get_addr(argv[2]);
+ Reorderer dst_a(sock, addr_a);
+ Reorderer dst_b(sock, addr_b);
+
+ for ( ;; ) {
+ char addrstr[256];
+ struct sockaddr_storage addr;
+ socklen_t addrlen = sizeof(addr);
+ char buf[4096];
+ int ret = recvfrom(sock, buf, sizeof(buf), 0, (struct sockaddr *)&addr, &addrlen);
+ if (ret == -1) {
+ perror("recvfrom");
+ exit(1);
+ }
+ if (addr.ss_family != AF_INET6) {
+ // ignore
+ //inet_ntop(AF_INET, &((struct sockaddr_in *)&addr)->sin_addr, addrstr, sizeof(addrstr));
+ continue;
+ }
+ struct in6_addr *addr6 = &((struct sockaddr_in6 *)&addr)->sin6_addr;
+ if (memcmp(addr6, &addr_a, sizeof(*addr6)) != 0 &&
+ memcmp(addr6, &addr_b, sizeof(*addr6)) != 0) {
+ // ignore
+ continue;
+ }
+ inet_ntop(AF_INET6, addr6, addrstr, sizeof(addrstr));
+ gre_header* gre = (gre_header *)buf;
+ //printf("GREv%d of %d bytes from %s: %02x %02x %02x %02x\n", gre->version, ret, addrstr,
+ // buf[0], buf[1], buf[2], buf[3]);
+ //printf(" has_checksum=%d, has_key=%d, has_seq=%d\n", gre->has_checksum, gre->has_key, gre->has_seq);
+
+ char* ptr = buf + sizeof(gre_header);
+ if (gre->has_checksum) {
+ ptr += 4;
+ }
+ if (gre->has_key) {
+ ptr += 4;
+ }
+ uint32_t seq;
+ if (gre->has_seq) {
+ seq = ntohl(*(uint32_t *)ptr);
+ // printf(" seq=%d\n", seq);
+ }
+ if (memcmp(addr6, &addr_a, sizeof(*addr6)) == 0) {
+ // comes from A, send to B
+ dst_b.handle_packet(buf, ret, seq);
+ } else {
+ // comes from B, send to A
+ dst_a.handle_packet(buf, ret, seq);
+ }
+ }
+}
--- /dev/null
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <sys/ioctl.h>
+#include <netinet/in.h>
+#include <arpa/inet.h>
+#include <linux/if.h>
+#include <linux/if_tun.h>
+
+#include <map>
+#include <string>
+#include <queue>
+
+using namespace std;
+
+struct gre_header {
+ uint8_t reserved0_hi : 4;
+ uint8_t has_seq : 1;
+ uint8_t has_key : 1;
+ uint8_t unused : 1;
+ uint8_t has_checksum : 1;
+
+ uint8_t version : 3;
+ uint8_t reserved0_lo: 5;
+
+ uint16_t protocol_type;
+};
+
+int tun_open(const char *name) {
+ struct ifreq ifr;
+
+ int fd = open("/dev/net/tun", O_RDWR);
+ if (fd == -1) {
+ perror("/dev/net/tun");
+ exit(1);
+ }
+
+ memset(&ifr, 0, sizeof(ifr));
+ ifr.ifr_flags = IFF_TUN;
+ strncpy(ifr.ifr_name, name, IFNAMSIZ);
+
+ int err = ioctl(fd, TUNSETIFF, &ifr);
+ if (err == -1) {
+ perror("ioctl(TUNSETIFF)");
+ exit(-1);
+ }
+
+ return fd;
+}
+
+in6_addr get_addr(const char *str) {
+ in6_addr ret;
+ if (inet_pton(AF_INET6, str, &ret) != 1) {
+ fprintf(stderr, "Could not parse %s\n", str);
+ exit(1);
+ }
+ return ret;
+}
+
+struct GREPacket {
+ int seq;
+ uint16_t proto;
+ string data;
+
+ bool operator> (const GREPacket &other) const {
+ return seq > other.seq;
+ }
+};
+
+class Sender {
+public:
+ virtual void send_packet(uint16_t proto, const string &data) = 0;
+};
+
+class GRESender : public Sender {
+public:
+ GRESender(int sock, const in6_addr &dst);
+ virtual void send_packet(uint16_t proto, const string &data);
+
+private:
+ int seq;
+ int sock;
+ sockaddr_in6 dstaddr;
+};
+
+class TUNSender : public Sender {
+public:
+ TUNSender(int tunfd);
+ virtual void send_packet(uint16_t proto, const string &data);
+
+private:
+ int tunfd;
+};
+
+class Reorderer {
+public:
+ Reorderer(Sender* sender);
+ void handle_packet(uint16_t proto, const string& data, int seq);
+
+private:
+ void send_packet(uint16_t proto, const string &data, bool silence);
+
+ Sender* sender;
+ int last_seq;
+
+ priority_queue<GREPacket, vector<GREPacket>, greater<GREPacket>> packet_buffer;
+ map<int, int> ccs;
+};
+
+GRESender::GRESender(int sock, const in6_addr &dst)
+ : sock(sock), seq(0)
+{
+ memset(&dstaddr, 0, sizeof(dstaddr));
+ dstaddr.sin6_family = AF_INET6;
+ dstaddr.sin6_addr = dst;
+}
+
+void GRESender::send_packet(uint16_t proto, const string &data)
+{
+ char buf[4096];
+ gre_header *gre = (gre_header *)buf;
+
+ memset(gre, 0, sizeof(*gre));
+ gre->has_seq = 1;
+ gre->version = 0;
+ gre->protocol_type = htons(proto);
+
+ char *ptr = buf + sizeof(*gre);
+ int seq_be = htonl(seq++);
+ memcpy(ptr, &seq_be, sizeof(seq_be));
+ ptr += sizeof(seq_be);
+
+ memcpy(ptr, data.data(), data.size());
+
+ if (sendto(sock, buf, data.size() + sizeof(seq_be) + sizeof(*gre), 0, (sockaddr *)&dstaddr, sizeof(dstaddr)) == -1) {
+ perror("sendto");
+ return;
+ }
+}
+
+TUNSender::TUNSender(int tunfd)
+ : tunfd(tunfd) {}
+
+void TUNSender::send_packet(uint16_t proto, const string &data)
+{
+ char buf[4096];
+
+ char *ptr = buf;
+ uint16_t flags = 0;
+ memcpy(ptr, &flags, sizeof(flags));
+ ptr += sizeof(flags);
+
+ proto = htons(proto);
+ memcpy(ptr, &proto, sizeof(proto));
+ ptr += sizeof(proto);
+
+ memcpy(ptr, data.data(), data.size());
+
+ int len = sizeof(flags) + sizeof(proto) + data.size();
+ if (write(tunfd, buf, len) != len) {
+ perror("write");
+ return;
+ }
+}
+
+Reorderer::Reorderer(Sender* sender)
+ : sender(sender), last_seq(-1)
+{
+}
+
+#define PACKET_BUFFER_SIZE 100
+
+void Reorderer::handle_packet(uint16_t proto, const string& data, int seq)
+{
+ bool silence = false;
+ if (packet_buffer.size() >= PACKET_BUFFER_SIZE) {
+ printf("Gave up waiting for packets [%d,%d>\n",
+ last_seq + 1, packet_buffer.top().seq);
+ silence = true;
+ last_seq = packet_buffer.top().seq - 1;
+ }
+
+ GREPacket packet;
+ packet.seq = seq;
+ packet.proto = proto;
+ packet.data = data;
+ packet_buffer.push(packet);
+
+ while (!packet_buffer.empty() &&
+ (last_seq == -1 || packet_buffer.top().seq <= last_seq + 1)) {
+ int front_seq = packet_buffer.top().seq;
+ if (front_seq < last_seq + 1) {
+ printf("Duplicate packet or way out-of-order: seq=%d front_seq=%d\n",
+ front_seq, last_seq + 1);
+ packet_buffer.pop();
+ continue;
+ }
+ //if (packet_buffer.size() > 1) {
+ // printf("seq=%d (REORDER %d)\n", front_seq, int(packet_buffer.size()));
+ //} else {
+ // printf("seq=%d\n", front_seq);
+ //}
+ const string &data = packet_buffer.top().data;
+ send_packet(packet_buffer.top().proto, data, silence);
+ packet_buffer.pop();
+ last_seq = front_seq;
+ if (!silence && !packet_buffer.empty()) {
+ printf("Reordering with packet buffer size %d: seq=%d new_front_seq=%d\n", int(packet_buffer.size()), front_seq, packet_buffer.top().seq);
+ silence = true;
+ }
+ }
+}
+
+void Reorderer::send_packet(uint16_t proto, const string &data, bool silence)
+{
+ if (data.size() == 1344) {
+ for (int i = 0; i < 7; ++i) {
+ const char *pkt = &data[i * 188 + 28];
+ int pid = (ntohl(*(uint32_t *)(pkt)) & 0x1fff00) >> 8;
+ if (pid == 8191) {
+ // stuffing, ignore
+ continue;
+ }
+ int has_payload = pkt[3] & 0x10;
+ int cc = pkt[3] & 0xf;
+ if (has_payload) {
+ int last_cc = ccs[pid];
+ if (!silence && cc != ((last_cc + 1) & 0xf)) {
+ printf("Pid %d discontinuity (expected %d, got %d)\n", pid, (last_cc + 1) & 0xf, cc);
+ }
+ ccs[pid] = cc;
+ }
+ }
+ }
+ sender->send_packet(proto, data);
+}
+
+void read_gre_packet(int gresock, const in6_addr &remoteaddr, Reorderer *sender)
+{
+ struct sockaddr_storage addr;
+ socklen_t addrlen = sizeof(addr);
+ char buf[4096];
+ int ret = recvfrom(gresock, buf, sizeof(buf), 0, (struct sockaddr *)&addr, &addrlen);
+ if (ret == -1) {
+ perror("recvfrom");
+ exit(1);
+ }
+ if (addr.ss_family != AF_INET6) {
+ return;
+ }
+ struct in6_addr *addr6 = &((struct sockaddr_in6 *)&addr)->sin6_addr;
+ if (memcmp(addr6, &remoteaddr, sizeof(*addr6)) != 0) {
+ // ignore
+ return;
+ }
+ gre_header* gre = (gre_header *)buf;
+
+ char* ptr = buf + sizeof(gre_header);
+ if (gre->has_checksum) {
+ ptr += 4;
+ }
+ if (gre->has_key) {
+ ptr += 4;
+ }
+ uint32_t seq;
+ if (gre->has_seq) {
+ seq = ntohl(*(uint32_t *)ptr);
+ ptr += 4;
+ }
+
+ //printf("gre packet: proto=%x\n", ntohs(gre->protocol_type));
+
+ sender->handle_packet(ntohs(gre->protocol_type), string(ptr, buf + ret), seq);
+}
+
+void read_tun_packet(int tunfd, Sender *sender)
+{
+ char buf[4096];
+ int ret = read(tunfd, buf, sizeof(buf));
+ if (ret == -1) {
+ perror("read");
+ exit(1);
+ }
+ if (ret == 0) {
+ fprintf(stderr, "tunfd EOF\n");
+ exit(1);
+ }
+
+ char *ptr = buf;
+ uint16_t flags = *(uint16_t *)ptr;
+ ptr += 2;
+ uint16_t proto = ntohs(*(uint16_t *)ptr);
+ ptr += 2;
+ //fprintf(stderr, "tun packet: flags=%x proto=%x len=%d\n",
+ // flags, proto, ret - 4);
+ sender->send_packet(proto, string(ptr, buf + ret));
+}
+
+int main(int argc, char **argv)
+{
+ int tunfd = tun_open("tungre");
+ int gresock = socket(AF_INET6, SOCK_RAW, IPPROTO_GRE);
+ if (gresock == -1) {
+ perror("socket");
+ exit(1);
+ }
+
+ sockaddr_in6 my_addr;
+ memset(&my_addr, 0, sizeof(my_addr));
+ my_addr.sin6_family = AF_INET6;
+ my_addr.sin6_addr = get_addr(argv[1]);
+ if (bind(gresock, (sockaddr *)&my_addr, sizeof(my_addr)) == -1) {
+ perror("bind");
+ exit(1);
+ }
+
+ in6_addr remoteaddr = get_addr(argv[2]);
+ GRESender gre_sender(gresock, remoteaddr);
+ TUNSender tun_sender(tunfd);
+
+ Reorderer tun_reorderer(&tun_sender);
+
+ fd_set fds;
+ FD_ZERO(&fds);
+ for ( ;; ) {
+ FD_SET(gresock, &fds);
+ FD_SET(tunfd, &fds);
+ int ret = select(1024, &fds, NULL, NULL, NULL);
+ if (ret == -1) {
+ perror("select");
+ continue;
+ }
+
+ if (FD_ISSET(gresock, &fds)) {
+ read_gre_packet(gresock, remoteaddr, &tun_reorderer);
+ }
+ if (FD_ISSET(tunfd, &fds)) {
+ read_tun_packet(tunfd, &gre_sender);
+ }
+ }
+}
projects / greproxy / commitdiff