+#include <string.h>
+#include <sys/types.h>
+#include <sys/socket.h>
+
+#include "greprotocol.h"
+#include "reorderer.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;
+};
+
+
+GREProtocol::GREProtocol(const in6_addr &src, const in6_addr &dst)
+ : seq(0)
+{
+ memset(&dstaddr, 0, sizeof(dstaddr));
+ dstaddr.sin6_family = AF_INET6;
+ dstaddr.sin6_addr = dst;
+
+ 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 = src;
+ if (::bind(sock, (sockaddr *)&my_addr, sizeof(my_addr)) == -1) {
+ perror("bind");
+ exit(1);
+ }
+}
+
+void GREProtocol::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;
+ }
+}
+
+int GREProtocol::fd() const
+{
+ return sock;
+}
+
+void GREProtocol::read_packet(Reorderer *sender)
+{
+ 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) {
+ return;
+ }
+ struct in6_addr *addr6 = &((struct sockaddr_in6 *)&addr)->sin6_addr;
+ if (memcmp(addr6, &dstaddr.sin6_addr, 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);
+}
+