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

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