4 #include <sys/socket.h>
6 #include <netinet/in.h>
15 uint8_t reserved0_hi : 4;
19 uint8_t has_checksum : 1;
22 uint8_t reserved0_lo: 5;
24 uint16_t protocol_type;
27 in6_addr get_addr(const char *str) {
29 if (inet_pton(AF_INET6, str, &ret) != 1) {
30 fprintf(stderr, "Could not parse %s\n", str);
40 bool operator> (const GREPacket &other) const {
41 return seq > other.seq;
47 Reorderer(int sock, const in6_addr &dst);
48 void handle_packet(const char* buf, size_t size, int seq);
51 void send_packet(const string &data);
57 priority_queue<GREPacket, vector<GREPacket>, greater<GREPacket>> packet_buffer;
61 Reorderer::Reorderer(int sock, const in6_addr &dst)
62 : sock(sock), last_seq(-1)
64 memset(&dstaddr, 0, sizeof(dstaddr));
65 dstaddr.sin6_family = AF_INET6;
66 dstaddr.sin6_addr = dst;
69 #define PACKET_BUFFER_SIZE 100
71 void Reorderer::handle_packet(const char* buf, size_t size, int seq)
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;
81 packet.data = string(buf, buf + size);
82 packet_buffer.push(packet);
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",
94 //if (packet_buffer.size() > 1) {
95 // printf("seq=%d (REORDER %d)\n", front_seq, int(packet_buffer.size()));
97 // printf("seq=%d\n", front_seq);
99 const string &data = packet_buffer.top().data;
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);
110 void Reorderer::send_packet(const string &data)
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;
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);
127 if (sendto(sock, data.data(), data.size(), 0, (sockaddr *)&dstaddr, sizeof(dstaddr)) == -1) {
133 int main(int argc, char **argv)
135 int sock = socket(AF_INET6, SOCK_RAW, IPPROTO_GRE);
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) {
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);
157 struct sockaddr_storage addr;
158 socklen_t addrlen = sizeof(addr);
160 int ret = recvfrom(sock, buf, sizeof(buf), 0, (struct sockaddr *)&addr, &addrlen);
165 if (addr.ss_family != AF_INET6) {
167 //inet_ntop(AF_INET, &((struct sockaddr_in *)&addr)->sin_addr, addrstr, sizeof(addrstr));
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) {
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);
182 char* ptr = buf + sizeof(gre_header);
183 if (gre->has_checksum) {
191 seq = ntohl(*(uint32_t *)ptr);
192 // printf(" seq=%d\n", seq);
194 if (memcmp(addr6, &addr_a, sizeof(*addr6)) == 0) {
195 // comes from A, send to B
196 dst_b.handle_packet(buf, ret, seq);
198 // comes from B, send to A
199 dst_a.handle_packet(buf, ret, seq);