+#include <arpa/inet.h>
+#include <netinet/in.h>
#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 <sys/select.h>
+#include <sys/socket.h>
-#include <map>
-#include <string>
-#include <queue>
+#include "greprotocol.h"
+#include "reorderer.h"
+#include "tunprotocol.h"
+#include "rsdecoder.h"
+#include "pacer.h"
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) {
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 myaddr = get_addr(argv[1]);
in6_addr remoteaddr = get_addr(argv[2]);
- GRESender gre_sender(gresock, remoteaddr);
- TUNSender tun_sender(tunfd);
+ GREProtocol gre(myaddr, remoteaddr);
+ Pacer gre_pacer(&gre, 40000, 6);
+ TUNProtocol tun("tungre");
- Reorderer tun_reorderer(&tun_sender);
+ Pacer tun_pacer(&tun, 40000, 6);
+ Reorderer tun_reorderer(&tun_pacer);
+ RSDecoder tun_decoder(&tun_reorderer);
fd_set fds;
FD_ZERO(&fds);
for ( ;; ) {
- FD_SET(gresock, &fds);
- FD_SET(tunfd, &fds);
- int ret = select(1024, &fds, NULL, NULL, NULL);
+ timeval tv = { 1, 0 };
+ FD_SET(gre.fd(), &fds);
+ FD_SET(tun.fd(), &fds);
+ tun_reorderer.possibly_adjust_tv(&tv);
+ tun_pacer.possibly_adjust_tv(&tv);
+ int ret = select(1024, &fds, NULL, NULL, &tv);
if (ret == -1) {
perror("select");
continue;
}
- if (FD_ISSET(gresock, &fds)) {
- read_gre_packet(gresock, remoteaddr, &tun_reorderer);
+ if (FD_ISSET(gre.fd(), &fds)) {
+ gre.read_packet(&tun_decoder);
}
- if (FD_ISSET(tunfd, &fds)) {
- read_tun_packet(tunfd, &gre_sender);
+ if (FD_ISSET(tun.fd(), &fds)) {
+ tun.read_packet(&gre_pacer);
}
+ tun_pacer.possibly_flush_packets();
}
}