git.sesse.net Git - greproxy/blob - greprotocol.cpp

   1 #include <string.h>
   2 #include <sys/types.h>
   3 #include <sys/socket.h>
   4
   5 #include "greprotocol.h"
   6 #include "reorderer.h"
   7
   8 using namespace std;
   9
  10 struct gre_header {
  11         uint8_t reserved0_hi : 4;
  12         uint8_t has_seq : 1;
  13         uint8_t has_key : 1;
  14         uint8_t unused : 1;
  15         uint8_t has_checksum : 1;
  16
  17         uint8_t version : 3;
  18         uint8_t reserved0_lo: 5;
  19
  20         uint16_t protocol_type;
  21 };
  22
  23
  24 GREProtocol::GREProtocol(const in6_addr &src, const in6_addr &dst)
  25         : seq(0)
  26 {
  27         memset(&dstaddr, 0, sizeof(dstaddr));
  28         dstaddr.sin6_family = AF_INET6;
  29         dstaddr.sin6_addr = dst;
  30
  31         sock = socket(AF_INET6, SOCK_RAW, IPPROTO_GRE);
  32         if (sock == -1) {
  33                 perror("socket");
  34                 exit(1);
  35         }
  36
  37         sockaddr_in6 my_addr;
  38         memset(&my_addr, 0, sizeof(my_addr));
  39         my_addr.sin6_family = AF_INET6;
  40         my_addr.sin6_addr = src;
  41         if (::bind(sock, (sockaddr *)&my_addr, sizeof(my_addr)) == -1) {
  42                 perror("bind");
  43                 exit(1);
  44         }
  45 }
  46
  47 void GREProtocol::send_packet(uint16_t proto, const string &data, int incoming_seq)
  48 {
  49         char buf[4096];
  50         gre_header *gre = (gre_header *)buf;
  51
  52         memset(gre, 0, sizeof(*gre));
  53         gre->has_seq = 1;
  54         gre->version = 0;
  55         gre->protocol_type = htons(proto);
  56
  57         char *ptr = buf + sizeof(*gre);
  58         int seq_be = htonl(seq++);  // Ignore incoming_seq.
  59         memcpy(ptr, &seq_be, sizeof(seq_be));
  60         ptr += sizeof(seq_be);
  61
  62         memcpy(ptr, data.data(), data.size());
  63
  64         if (sendto(sock, buf, data.size() + sizeof(seq_be) + sizeof(*gre), 0, (sockaddr *)&dstaddr, sizeof(dstaddr)) == -1) {
  65                 perror("sendto");
  66                 return;
  67         }
  68 }
  69
  70 int GREProtocol::fd() const
  71 {
  72         return sock;
  73 }
  74
  75 void GREProtocol::read_packet(Protocol *sender)
  76 {
  77         struct sockaddr_storage addr;
  78         socklen_t addrlen = sizeof(addr);
  79         char buf[4096];
  80         int ret = recvfrom(sock, buf, sizeof(buf), 0, (struct sockaddr *)&addr, &addrlen);
  81         if (ret == -1) {
  82                 perror("recvfrom");
  83                 exit(1);
  84         }
  85         if (addr.ss_family != AF_INET6) {
  86                 return;
  87         }
  88         struct in6_addr *addr6 = &((struct sockaddr_in6 *)&addr)->sin6_addr;
  89         if (memcmp(addr6, &dstaddr.sin6_addr, sizeof(*addr6)) != 0) {
  90                 // ignore
  91                 return;
  92         }
  93         gre_header* gre = (gre_header *)buf;
  94
  95         char* ptr = buf + sizeof(gre_header);
  96         if (gre->has_checksum) {
  97                 ptr += 4;
  98         }
  99         if (gre->has_key) {
 100                 ptr += 4;
 101         }
 102         uint32_t seq;
 103         if (gre->has_seq) {
 104                 seq = ntohl(*(uint32_t *)ptr);
 105                 ptr += 4;
 106         }
 107
 108         //printf("gre packet: proto=%x\n", ntohs(gre->protocol_type));
 109
 110         sender->send_packet(ntohs(gre->protocol_type), string(ptr, buf + ret), seq);
 111 }
 112