stun.c

   1 #include <stdio.h>
   2 #include <stdlib.h>
   3 #include <stdint.h>
   4 #include <string.h>
   5 #include <errno.h>
   6 #include <time.h>
   7 #include <unistd.h>
   8 #include <netdb.h>
   9 #include <netinet/in.h>
  10 #include <sys/socket.h>
  11 #include <netinet/in.h>
  12 #include <arpa/inet.h>
  13 #include <sys/select.h>
  14
  15 #include "xmalloc.h"
  16 #include "die.h"
  17 #include "sock.h"
  18 #include "stun.h"
  19
  20 #define BINDING_REQUEST               0x0001
  21 #define BINDING_RESPONSE              0x0101
  22
  23 #define MAPPED_ADDRESS                0x0001
  24
  25 #define TIMEOUT                       5000
  26 #define REQUEST_LEN                   20
  27
  28 #define ID_COOKIE_FIELD               htonl(((int) 'a' << 24) + \
  29                                             ((int) 'c' << 16) + \
  30                                             ((int) 'd' <<  8) + \
  31                                              (int) 'c')
  32
  33 struct stun_header {
  34         uint16_t type;
  35         uint16_t len;
  36         uint32_t magic_cookie;
  37         uint32_t transid[3];
  38 };
  39
  40 struct stun_attrib {
  41         uint16_t type;
  42         uint16_t len;
  43         uint8_t *value;
  44 };
  45
  46 struct stun_mapped_addr {
  47         uint8_t none;
  48         uint8_t family;
  49         uint16_t port;
  50         uint32_t ip;
  51 };
  52
  53 static int stun_test(const char *server_ip, int server_port,
  54                      int tun_port)
  55 {
  56         int ret, sock;
  57         uint8_t pkt[256];
  58         uint8_t rpkt[256];
  59         size_t len, off, max;
  60         struct in_addr in;
  61         struct timeval timeout;
  62         struct stun_header *hdr, *rhdr;
  63         struct stun_attrib *attr;
  64         struct stun_mapped_addr *addr;
  65         struct sockaddr_in saddr, daddr;
  66         fd_set fdset;
  67
  68         if (!server_ip)
  69                 return -EINVAL;
  70
  71         sock = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);
  72         if (sock < 0)
  73                 panic("Cannot obtain socket!\n");
  74
  75         set_reuseaddr(sock);
  76
  77         memset(&saddr, 0, sizeof(saddr));
  78         saddr.sin_family = PF_INET;
  79         saddr.sin_port = htons(tun_port);
  80         saddr.sin_addr.s_addr = INADDR_ANY;
  81
  82         ret = bind(sock, (struct sockaddr *) &saddr, sizeof(saddr));
  83         if (ret)
  84                 panic("Cannot bind udp socket!\n");
  85
  86         len = REQUEST_LEN;
  87         hdr = (struct stun_header *) pkt;
  88         hdr->type = htons(BINDING_REQUEST);
  89         hdr->len = 0;
  90         hdr->magic_cookie = ID_COOKIE_FIELD;
  91         hdr->transid[0] = htonl(rand());
  92         hdr->transid[1] = htonl(rand());
  93         hdr->transid[2] = htonl(rand());
  94
  95         daddr.sin_family = PF_INET;
  96         daddr.sin_port = htons(server_port);
  97         daddr.sin_addr.s_addr = inet_addr(server_ip);
  98
  99         ret = sendto(sock, pkt, len, 0, (struct sockaddr *) &daddr,
 100                      sizeof(daddr));
 101         if (ret != len) {
 102                 printf("Error sending request (%s)!\n", strerror(errno));
 103                 goto close_error;
 104         }
 105
 106         timeout.tv_sec = TIMEOUT / 1000;
 107         timeout.tv_usec = (TIMEOUT % 1000) * 1000;
 108
 109         FD_ZERO(&fdset);
 110         FD_SET(sock, &fdset);
 111
 112         ret = select(sock + 1, &fdset, NULL, NULL, &timeout);
 113         if (ret <= 0) {
 114                 printf("STUN server timeout!\n");
 115                 goto close_error;
 116         }
 117
 118         memset(rpkt, 0, sizeof(rpkt));
 119         len = read(sock, rpkt, sizeof(rpkt));
 120
 121         close(sock);
 122
 123         if (len < REQUEST_LEN) {
 124                 printf("Bad STUN response (%s)!\n", strerror(errno));
 125                 return -EIO;
 126         }
 127
 128         rhdr = (struct stun_header *) rpkt;
 129         if (ntohs(rhdr->type) != BINDING_RESPONSE) {
 130                 printf("Wrong STUN response type!\n");
 131                 return -EIO;
 132         }
 133
 134         if (rhdr->len == 0) {
 135                 printf("No attributes in STUN response!\n");
 136                 return -EIO;
 137         }
 138
 139         if (rhdr->magic_cookie != hdr->magic_cookie ||
 140             rhdr->transid[0] != hdr->transid[0] ||
 141             rhdr->transid[1] != hdr->transid[1] ||
 142             rhdr->transid[2] != hdr->transid[2]) {
 143                 printf("Got wrong STUN transaction id!\n");
 144                 return -EIO;
 145         }
 146
 147         off = REQUEST_LEN;
 148         max = ntohs(rhdr->len) + REQUEST_LEN;
 149
 150         while (off + 8 < max) {
 151                 attr = (struct stun_attrib *) (rpkt + off);
 152                 if (ntohs(attr->type) != MAPPED_ADDRESS)
 153                         goto next;
 154
 155                 addr = (struct stun_mapped_addr *) (rpkt + off + 4);
 156                 if (addr->family != 0x1)
 157                         break;
 158
 159                 in.s_addr = addr->ip;
 160                 printf("Public mapping %s:%u!\n",
 161                        inet_ntoa(in), ntohs(addr->port));
 162                 break;
 163 next:
 164                 off += 4;
 165                 off += ntohs(attr->len);
 166         }
 167
 168         return 0;
 169 close_error:
 170         close(sock);
 171         return -EIO;
 172 }
 173
 174 int print_stun_probe(char *server, int sport, int tport)
 175 {
 176         char *address;
 177         struct hostent *hp;
 178
 179         printf("STUN on %s:%u\n", server, sport);
 180
 181         srand(time(NULL));
 182         hp = gethostbyname(server);
 183         if (!hp)
 184                 return -EIO;
 185         address = inet_ntoa(*(struct in_addr *) hp->h_addr_list[0]);
 186         return stun_test(address, sport, tport);
 187 }