package/tapi_sip/src/stun.c

   1 #include <stdio.h>
   2 #include <stdlib.h>
   3 #include <unistd.h>
   4 #include <errno.h>
   5 #include <string.h>
   6 #include <sys/types.h>
   7 #include <sys/socket.h>
   8 #include <netinet/in.h>
   9 #include <arpa/inet.h>
  10 #include <netdb.h>
  11
  12 #include <poll.h>
  13
  14
  15 struct stun_client {
  16         struct addrinfo *serverinfo;
  17 };
  18
  19 struct stun_response {
  20         struct sockaddr addr;
  21 };
  22
  23 struct stun_header {
  24         uint16_t type;
  25         uint16_t length;
  26         uint32_t cookie;
  27         uint32_t id[3];
  28 } __attribute((packed));
  29
  30 struct stun_packet {
  31         struct stun_header header;
  32         uint8_t data[0];
  33 } __attribute((packed));
  34
  35 #define STUN_CLASS(c0, c1) (((c0) << 4) | ((c1) << 8))
  36
  37 #define STUN_CLASS_REQUEST      STUN_CLASS(0, 0)
  38 #define STUN_CLASS_INDICATION   STUN_CLASS(0, 1)
  39 #define STUN_CLASS_SUCCESS      STUN_CLASS(1, 0)
  40 #define STUN_CLASS_ERROR        STUN_CLASS(1, 1)
  41
  42 #define STUN_CLASS_MASK STUN_CLASS(1, 1)
  43
  44 #define STUN_MESSAGE(msg) (((msg & 0xf10) << 2) | ((msg & 0x70) << 1) | (msg & 0xf))
  45 #define STUN_MESSAGE_BIND STUN_MESSAGE(1)
  46
  47 #define STUN_COOKIE 0x2112a442
  48
  49 enum {
  50         STUN_ATTR_TYPE_MAPPED_ADDRESS = 0x1,
  51         STUN_ATTR_TYPE_XOR_MAPPED_ADDRESS = 0x20,
  52         STUN_ATTR_TYPE_XOR_MAPPED_ADDRESS2 = 0x8020,
  53 };
  54
  55 static inline uint16_t get_unaligned_be16(const uint8_t *buf)
  56 {
  57         return (buf[0] << 8) | buf[1];
  58 }
  59
  60 static inline uint16_t get_unaligned_be32(const uint8_t *buf)
  61 {
  62         return (buf[0] << 24) | (buf[1] << 16) | (buf[2] << 8) | buf[3];
  63 }
  64
  65 static int stun_parse_xor_mapped_address(struct stun_response *response,
  66         const uint8_t *buf, int length)
  67 {
  68         uint8_t fam = buf[1];
  69         uint16_t port = get_unaligned_be16(&buf[2]);
  70         struct sockaddr_in *sin = (struct sockaddr_in *)&response->addr;
  71         struct sockaddr_in6 *sin6 = (struct sockaddr_in6 *)&response->addr;
  72
  73
  74         switch (fam) {
  75         case 0x1:
  76                 sin->sin_family = AF_INET;
  77                 sin->sin_port = htons((port ^ (uint16_t)((STUN_COOKIE & 0xffff0000) >> 16)));
  78                 memcpy(&sin->sin_addr.s_addr, buf + 4, 4);
  79                 sin->sin_addr.s_addr ^= htonl(STUN_COOKIE);
  80                 printf("xor port: %d\n", sin->sin_port);
  81                 break;
  82         case 0x2:
  83                 sin6->sin6_family = AF_INET6;
  84                 sin->sin_port = htons((port ^ (uint16_t)((STUN_COOKIE & 0xffff0000) >> 16)));
  85                 memcpy(sin6->sin6_addr.s6_addr, buf + 4, 16);
  86                 break;
  87         }
  88
  89         return 0;
  90 }
  91
  92 static int stun_parse_mapped_address(struct stun_response *response,
  93         const uint8_t *buf, int length)
  94 {
  95         uint8_t fam = buf[1];
  96         uint16_t port = get_unaligned_be16(&buf[2]);
  97         struct sockaddr_in *sin = (struct sockaddr_in *)&response->addr;
  98         struct sockaddr_in6 *sin6 = (struct sockaddr_in6 *)&response->addr;
  99
 100         printf("port: %d\n", port);
 101
 102         switch (fam) {
 103         case 0x1:
 104                 sin->sin_family = AF_INET;
 105                 sin->sin_port = htons(port);
 106                 memcpy(&sin->sin_addr.s_addr, buf + 4, 4);
 107                 break;
 108         case 0x2:
 109                 sin6->sin6_family = AF_INET6;
 110                 sin6->sin6_port = htons(port);
 111                 memcpy(sin6->sin6_addr.s6_addr, buf + 4, 16);
 112                 break;
 113         }
 114
 115         return 0;
 116 }
 117
 118 static int stun_parse_response(struct stun_response *response,
 119         const struct stun_packet *packet)
 120 {
 121         uint16_t attr_type, attr_length;
 122         const uint8_t *buf;
 123         int length = ntohs(packet->header.length);
 124         int ret;
 125         int i = 0;
 126
 127         if (packet->header.cookie != htonl(STUN_COOKIE))
 128                 return -1;
 129
 130         if (packet->header.length < 4)
 131                 return 0;
 132
 133         buf = packet->data;
 134
 135         do {
 136                 attr_type = get_unaligned_be16(&buf[i]);
 137                 attr_length = get_unaligned_be16(&buf[i + 2]);
 138                 i += 4;
 139
 140                 if (i + attr_length > length)
 141                         break;
 142
 143                 switch (attr_type) {
 144                 case STUN_ATTR_TYPE_MAPPED_ADDRESS:
 145                         ret = stun_parse_mapped_address(response, &buf[i], attr_length);
 146                         break;
 147                 case STUN_ATTR_TYPE_XOR_MAPPED_ADDRESS:
 148                 case STUN_ATTR_TYPE_XOR_MAPPED_ADDRESS2:
 149                         ret = stun_parse_xor_mapped_address(response, &buf[i], attr_length);
 150                         break;
 151                 }
 152
 153                 i += attr_length;
 154
 155         } while (i < length && ret == 0);
 156
 157         return 0;
 158 }
 159
 160 static struct stun_packet *stun_packet_alloc(size_t data_size)
 161 {
 162         return malloc(sizeof(struct stun_packet) + data_size);
 163 }
 164
 165 int stun_client_resolve(struct stun_client *stun, int sockfd, struct sockaddr *addr)
 166 {
 167         struct stun_packet *packet = stun_packet_alloc(200);
 168         struct stun_response response;
 169         int ret;
 170         int retries = 4;
 171         int timeout = 500;
 172         struct pollfd pollfd;
 173
 174         pollfd.events = POLLIN;
 175         pollfd.fd = sockfd;
 176
 177         packet->header.type = htons(STUN_CLASS_REQUEST | STUN_MESSAGE_BIND);
 178         packet->header.cookie = htonl(STUN_COOKIE);
 179         packet->header.id[0] = 0x12345678;
 180         packet->header.id[1] = 0x12345678;
 181         packet->header.id[2] = 0x12345678;
 182         packet->header.length = 0;
 183
 184         while (retries--) {
 185                 ret = sendto(sockfd, packet, sizeof(struct stun_header) + packet->header.length,
 186                         0, stun->serverinfo->ai_addr, stun->serverinfo->ai_addrlen);
 187
 188                 ret = poll(&pollfd, 1, timeout);
 189                 switch (ret) {
 190                 case 0:
 191                         timeout <<= 1;
 192                 case -EINTR:
 193                         printf("retry\n");
 194                         continue;
 195                 default:
 196                         retries = 0;
 197                 }
 198                 ret = recvfrom(sockfd, packet, 200, 0, NULL, NULL);
 199         }
 200
 201         if (ret <= 0)
 202                 return ret ? ret : -ETIMEDOUT;
 203
 204         memset(&response, 0, sizeof(response));
 205         ret = stun_parse_response(&response, packet);
 206
 207         *addr = response.addr;
 208
 209         return ret;
 210 }
 211
 212 struct stun_client *stun_client_alloc(const char *hostname, uint16_t port)
 213 {
 214     struct addrinfo hints;
 215         struct stun_client *stun;
 216         int ret;
 217         char p[6];
 218
 219
 220         stun = malloc(sizeof(*stun));
 221         if (!stun)
 222                 return NULL;
 223
 224     memset(&hints, 0, sizeof(hints));
 225     hints.ai_family = AF_UNSPEC;
 226     hints.ai_socktype = SOCK_DGRAM;
 227         hints.ai_flags = AI_NUMERICSERV;
 228
 229         snprintf(p, sizeof(p), "%d", port);
 230     if ((ret = getaddrinfo(hostname, p, &hints, &stun->serverinfo)) != 0) {
 231         fprintf(stderr, "getaddrinfo: %s\n", gai_strerror(ret));
 232         return NULL;
 233     }
 234
 235
 236         return stun;
 237 }
 238
 239 void stun_client_free(struct stun_client *stun)
 240 {
 241         freeaddrinfo(stun->serverinfo);
 242         free(stun);
 243 }