From 01d0292ac62e286e4bd7db94342438e7270c9b01 Mon Sep 17 00:00:00 2001 From: "Steinar H. Gunderson" Date: Fri, 6 Feb 2015 23:51:06 +0100 Subject: [PATCH] Initial commit. --- greproxy.cpp | 202 ++++++++++++++++++++++++++++++ tungre.cpp | 343 +++++++++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 545 insertions(+) create mode 100644 greproxy.cpp create mode 100644 tungre.cpp diff --git a/greproxy.cpp b/greproxy.cpp new file mode 100644 index 0000000..af0ba49 --- /dev/null +++ b/greproxy.cpp @@ -0,0 +1,202 @@ +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +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, greater> packet_buffer; + map 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); + } + } +} diff --git a/tungre.cpp b/tungre.cpp new file mode 100644 index 0000000..82bfa34 --- /dev/null +++ b/tungre.cpp @@ -0,0 +1,343 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +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, greater> packet_buffer; + map 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); + } + } +} -- 2.39.2