7 #include <sys/socket.h>
8 #include <netinet/in.h>
16 struct addrinfo *serverinfo;
19 struct stun_response {
28 } __attribute((packed));
31 struct stun_header header;
33 } __attribute((packed));
35 #define STUN_CLASS(c0, c1) (((c0) << 4) | ((c1) << 8))
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)
42 #define STUN_CLASS_MASK STUN_CLASS(1, 1)
44 #define STUN_MESSAGE(msg) (((msg & 0xf10) << 2) | ((msg & 0x70) << 1) | (msg & 0xf))
45 #define STUN_MESSAGE_BIND STUN_MESSAGE(1)
47 #define STUN_COOKIE 0x2112a442
50 STUN_ATTR_TYPE_MAPPED_ADDRESS = 0x1,
51 STUN_ATTR_TYPE_XOR_MAPPED_ADDRESS = 0x20,
52 STUN_ATTR_TYPE_XOR_MAPPED_ADDRESS2 = 0x8020,
55 static inline uint16_t get_unaligned_be16(const uint8_t *buf)
57 return (buf[0] << 8) | buf[1];
60 static inline uint16_t get_unaligned_be32(const uint8_t *buf)
62 return (buf[0] << 24) | (buf[1] << 16) | (buf[2] << 8) | buf[3];
65 static int stun_parse_xor_mapped_address(struct stun_response *response,
66 const uint8_t *buf, int length)
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;
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);
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);
92 static int stun_parse_mapped_address(struct stun_response *response,
93 const uint8_t *buf, int length)
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;
100 printf("port: %d\n", port);
104 sin->sin_family = AF_INET;
105 sin->sin_port = htons(port);
106 memcpy(&sin->sin_addr.s_addr, buf + 4, 4);
109 sin6->sin6_family = AF_INET6;
110 sin6->sin6_port = htons(port);
111 memcpy(sin6->sin6_addr.s6_addr, buf + 4, 16);
118 static int stun_parse_response(struct stun_response *response,
119 const struct stun_packet *packet)
121 uint16_t attr_type, attr_length;
123 int length = ntohs(packet->header.length);
127 if (packet->header.cookie != htonl(STUN_COOKIE))
130 if (packet->header.length < 4)
136 attr_type = get_unaligned_be16(&buf[i]);
137 attr_length = get_unaligned_be16(&buf[i + 2]);
140 if (i + attr_length > length)
144 case STUN_ATTR_TYPE_MAPPED_ADDRESS:
145 ret = stun_parse_mapped_address(response, &buf[i], attr_length);
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);
155 } while (i < length && ret == 0);
160 static struct stun_packet *stun_packet_alloc(size_t data_size)
162 return malloc(sizeof(struct stun_packet) + data_size);
165 int stun_client_resolve(struct stun_client *stun, int sockfd, struct sockaddr *addr)
167 struct stun_packet *packet = stun_packet_alloc(200);
168 struct stun_response response;
172 struct pollfd pollfd;
174 pollfd.events = POLLIN;
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;
185 ret = sendto(sockfd, packet, sizeof(struct stun_header) + packet->header.length,
186 0, stun->serverinfo->ai_addr, stun->serverinfo->ai_addrlen);
188 ret = poll(&pollfd, 1, timeout);
198 ret = recvfrom(sockfd, packet, 200, 0, NULL, NULL);
202 return ret ? ret : -ETIMEDOUT;
204 memset(&response, 0, sizeof(response));
205 ret = stun_parse_response(&response, packet);
207 *addr = response.addr;
212 struct stun_client *stun_client_alloc(const char *hostname, uint16_t port)
214 struct addrinfo hints;
215 struct stun_client *stun;
220 stun = malloc(sizeof(*stun));
224 memset(&hints, 0, sizeof(hints));
225 hints.ai_family = AF_UNSPEC;
226 hints.ai_socktype = SOCK_DGRAM;
227 hints.ai_flags = AI_NUMERICSERV;
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));
239 void stun_client_free(struct stun_client *stun)
241 freeaddrinfo(stun->serverinfo);