7 #include <netinet/in.h>
10 #include <linux/if_tun.h>
19 uint8_t reserved0_hi : 4;
23 uint8_t has_checksum : 1;
26 uint8_t reserved0_lo: 5;
28 uint16_t protocol_type;
31 int tun_open(const char *name) {
34 int fd = open("/dev/net/tun", O_RDWR);
36 perror("/dev/net/tun");
40 memset(&ifr, 0, sizeof(ifr));
41 ifr.ifr_flags = IFF_TUN;
42 strncpy(ifr.ifr_name, name, IFNAMSIZ);
44 int err = ioctl(fd, TUNSETIFF, &ifr);
46 perror("ioctl(TUNSETIFF)");
53 in6_addr get_addr(const char *str) {
55 if (inet_pton(AF_INET6, str, &ret) != 1) {
56 fprintf(stderr, "Could not parse %s\n", str);
67 bool operator> (const GREPacket &other) const {
68 return seq > other.seq;
74 virtual void send_packet(uint16_t proto, const string &data) = 0;
75 virtual int fd() const = 0;
81 class GREProtocol : public Protocol {
83 GREProtocol(const in6_addr &myaddr, const in6_addr &dst);
84 virtual void send_packet(uint16_t proto, const string &data);
85 virtual int fd() const;
86 void read_packet(Reorderer* sender);
94 class TUNProtocol : public Protocol {
96 TUNProtocol(const char *devname);
97 virtual void send_packet(uint16_t proto, const string &data);
98 virtual int fd() const;
99 void read_packet(Protocol* sender);
107 Reorderer(Protocol* sender);
108 void handle_packet(uint16_t proto, const string& data, int seq);
111 void send_packet(uint16_t proto, const string &data, bool silence);
116 priority_queue<GREPacket, vector<GREPacket>, greater<GREPacket>> packet_buffer;
120 GREProtocol::GREProtocol(const in6_addr &src, const in6_addr &dst)
123 memset(&dstaddr, 0, sizeof(dstaddr));
124 dstaddr.sin6_family = AF_INET6;
125 dstaddr.sin6_addr = dst;
127 sock = socket(AF_INET6, SOCK_RAW, IPPROTO_GRE);
133 sockaddr_in6 my_addr;
134 memset(&my_addr, 0, sizeof(my_addr));
135 my_addr.sin6_family = AF_INET6;
136 my_addr.sin6_addr = src;
137 if (bind(sock, (sockaddr *)&my_addr, sizeof(my_addr)) == -1) {
143 void GREProtocol::send_packet(uint16_t proto, const string &data)
146 gre_header *gre = (gre_header *)buf;
148 memset(gre, 0, sizeof(*gre));
151 gre->protocol_type = htons(proto);
153 char *ptr = buf + sizeof(*gre);
154 int seq_be = htonl(seq++);
155 memcpy(ptr, &seq_be, sizeof(seq_be));
156 ptr += sizeof(seq_be);
158 memcpy(ptr, data.data(), data.size());
160 if (sendto(sock, buf, data.size() + sizeof(seq_be) + sizeof(*gre), 0, (sockaddr *)&dstaddr, sizeof(dstaddr)) == -1) {
166 int GREProtocol::fd() const
171 TUNProtocol::TUNProtocol(const char *devname)
172 : tunfd(tun_open(devname)) {
175 void TUNProtocol::send_packet(uint16_t proto, const string &data)
181 memcpy(ptr, &flags, sizeof(flags));
182 ptr += sizeof(flags);
184 proto = htons(proto);
185 memcpy(ptr, &proto, sizeof(proto));
186 ptr += sizeof(proto);
188 memcpy(ptr, data.data(), data.size());
190 int len = sizeof(flags) + sizeof(proto) + data.size();
191 if (write(tunfd, buf, len) != len) {
197 int TUNProtocol::fd() const
202 Reorderer::Reorderer(Protocol* sender)
203 : sender(sender), last_seq(-1)
207 #define PACKET_BUFFER_SIZE 100
209 void Reorderer::handle_packet(uint16_t proto, const string& data, int seq)
211 bool silence = false;
212 if (packet_buffer.size() >= PACKET_BUFFER_SIZE) {
213 printf("Gave up waiting for packets [%d,%d>\n",
214 last_seq + 1, packet_buffer.top().seq);
216 last_seq = packet_buffer.top().seq - 1;
221 packet.proto = proto;
223 packet_buffer.push(packet);
225 while (!packet_buffer.empty() &&
226 (last_seq == -1 || packet_buffer.top().seq <= last_seq + 1)) {
227 int front_seq = packet_buffer.top().seq;
228 if (front_seq < last_seq + 1) {
229 printf("Duplicate packet or way out-of-order: seq=%d front_seq=%d\n",
230 front_seq, last_seq + 1);
234 //if (packet_buffer.size() > 1) {
235 // printf("seq=%d (REORDER %d)\n", front_seq, int(packet_buffer.size()));
237 // printf("seq=%d\n", front_seq);
239 const string &data = packet_buffer.top().data;
240 send_packet(packet_buffer.top().proto, data, silence);
242 last_seq = front_seq;
243 if (!silence && !packet_buffer.empty()) {
244 printf("Reordering with packet buffer size %d: seq=%d new_front_seq=%d\n", int(packet_buffer.size()), front_seq, packet_buffer.top().seq);
250 void Reorderer::send_packet(uint16_t proto, const string &data, bool silence)
252 if (data.size() == 1344) {
253 for (int i = 0; i < 7; ++i) {
254 const char *pkt = &data[i * 188 + 28];
255 int pid = (ntohl(*(uint32_t *)(pkt)) & 0x1fff00) >> 8;
260 int has_payload = pkt[3] & 0x10;
261 int cc = pkt[3] & 0xf;
263 int last_cc = ccs[pid];
264 if (!silence && cc != ((last_cc + 1) & 0xf)) {
265 printf("Pid %d discontinuity (expected %d, got %d)\n", pid, (last_cc + 1) & 0xf, cc);
271 sender->send_packet(proto, data);
274 void GREProtocol::read_packet(Reorderer *sender)
276 struct sockaddr_storage addr;
277 socklen_t addrlen = sizeof(addr);
279 int ret = recvfrom(sock, buf, sizeof(buf), 0, (struct sockaddr *)&addr, &addrlen);
284 if (addr.ss_family != AF_INET6) {
287 struct in6_addr *addr6 = &((struct sockaddr_in6 *)&addr)->sin6_addr;
288 if (memcmp(addr6, &dstaddr.sin6_addr, sizeof(*addr6)) != 0) {
292 gre_header* gre = (gre_header *)buf;
294 char* ptr = buf + sizeof(gre_header);
295 if (gre->has_checksum) {
303 seq = ntohl(*(uint32_t *)ptr);
307 //printf("gre packet: proto=%x\n", ntohs(gre->protocol_type));
309 sender->handle_packet(ntohs(gre->protocol_type), string(ptr, buf + ret), seq);
312 void TUNProtocol::read_packet(Protocol *sender)
315 int ret = read(tunfd, buf, sizeof(buf));
321 fprintf(stderr, "tunfd EOF\n");
326 uint16_t flags = *(uint16_t *)ptr;
328 uint16_t proto = ntohs(*(uint16_t *)ptr);
330 //fprintf(stderr, "tun packet: flags=%x proto=%x len=%d\n",
331 // flags, proto, ret - 4);
332 sender->send_packet(proto, string(ptr, buf + ret));
335 int main(int argc, char **argv)
337 in6_addr myaddr = get_addr(argv[1]);
338 in6_addr remoteaddr = get_addr(argv[2]);
339 GREProtocol gre(myaddr, remoteaddr);
340 TUNProtocol tun("tungre");
342 Reorderer tun_reorderer(&tun);
347 FD_SET(gre.fd(), &fds);
348 FD_SET(tun.fd(), &fds);
349 int ret = select(1024, &fds, NULL, NULL, NULL);
355 if (FD_ISSET(gre.fd(), &fds)) {
356 gre.read_packet(&tun_reorderer);
358 if (FD_ISSET(tun.fd(), &fds)) {
359 tun.read_packet(&gre);