stun.c

   1 // SPDX-License-Identifier: GPL-2.0-or-later
   2 /*
   3  * Copyright (C) 2022 Felix Fietkau <nbd@nbd.name>
   4  */
   5 #include <sys/types.h>
   6 #include <arpa/inet.h>
   7 #include <string.h>
   8 #include <stdio.h>
   9 #include "stun.h"
  10
  11 static uint8_t tx_buf[256];
  12
  13 bool stun_msg_is_valid(const void *data, size_t len)
  14 {
  15         const struct stun_msg_hdr *hdr = data;
  16
  17         if (len <= sizeof(*hdr))
  18                 return false;
  19
  20         return hdr->magic == htonl(STUN_MAGIC);
  21 }
  22
  23 static void *stun_msg_init(uint16_t type)
  24 {
  25         struct stun_msg_hdr *hdr = (struct stun_msg_hdr *)tx_buf;
  26
  27         memset(hdr, 0, sizeof(*hdr));
  28         hdr->msg_type = htons(type);
  29         hdr->magic = htonl(STUN_MAGIC);
  30
  31         return hdr;
  32 }
  33
  34 static void *stun_msg_add_tlv(uint16_t type, uint16_t len)
  35 {
  36         struct stun_msg_hdr *hdr = (struct stun_msg_hdr *)tx_buf;
  37         uint16_t data_len = ntohs(hdr->msg_len);
  38         struct stun_msg_tlv *tlv;
  39         void *data = hdr + 1;
  40
  41         data += data_len;
  42
  43         tlv = data;
  44         tlv->type = htons(type);
  45         tlv->len = htons(len);
  46
  47         if (len & 3)
  48                 len = (len + 3) & ~3;
  49
  50         data_len += sizeof(*tlv) + len;
  51         hdr->msg_len = htons(data_len);
  52
  53         return tlv + 1;
  54 }
  55
  56 static void
  57 stun_msg_parse_attr(const struct stun_tlv_policy *policy,
  58                     const struct stun_msg_tlv **tb, int len,
  59                     const struct stun_msg_tlv *tlv)
  60 {
  61         uint16_t type;
  62         int i;
  63
  64         type = ntohs(tlv->type);
  65
  66         for (i = 0; i < len; i++) {
  67                 if (policy[i].type != type)
  68                         continue;
  69
  70                 if (ntohs(tlv->len) < policy[i].min_len)
  71                         return;
  72
  73                 tb[i] = tlv;
  74                 return;
  75         }
  76 }
  77
  78 static void
  79 stun_msg_parse(const struct stun_tlv_policy *policy,
  80                const struct stun_msg_tlv **tb, int len,
  81                const void *data, size_t data_len)
  82 {
  83         const struct stun_msg_hdr *hdr = data;
  84         const struct stun_msg_tlv *tlv;
  85         const void *end = data + data_len;
  86         uint16_t cur_len;
  87
  88         data += sizeof(*hdr);
  89         while (1) {
  90                 tlv = data;
  91                 data = tlv + 1;
  92                 if (data > end)
  93                         break;
  94
  95                 cur_len = ntohs(tlv->len);
  96                 if (data + cur_len > end)
  97                         break;
  98
  99                 stun_msg_parse_attr(policy, tb, len, tlv);
 100                 data += (cur_len + 3) & ~3;
 101         }
 102 }
 103
 104 const void *stun_msg_request_prepare(struct stun_request *req, size_t *len,
 105                                      uint16_t response_port)
 106 {
 107         struct stun_msg_hdr *hdr;
 108         FILE *f;
 109
 110         hdr = stun_msg_init(STUN_MSGTYPE_BINDING_REQUEST);
 111         if (response_port) {
 112                 uint16_t *tlv_port = stun_msg_add_tlv(STUN_TLV_RESPONSE_PORT, 2);
 113                 *tlv_port = htons(response_port);
 114         }
 115
 116         f = fopen("/dev/urandom", "r");
 117         if (!f)
 118                 return NULL;
 119
 120         if (fread(hdr->transaction, 12, 1, f) != 1)
 121                 return NULL;
 122
 123         fclose(f);
 124         memcpy(req->transaction, hdr->transaction, sizeof(req->transaction));
 125         req->pending = true;
 126         req->port = 0;
 127         *len = htons(hdr->msg_len) + sizeof(*hdr);
 128
 129         return hdr;
 130 }
 131
 132 bool stun_msg_request_complete(struct stun_request *req, const void *data,
 133                                size_t len)
 134 {
 135         enum {
 136                 PARSE_ATTR_MAPPED,
 137                 PARSE_ATTR_XOR_MAPPED,
 138                 __PARSE_ATTR_MAX
 139         };
 140         const struct stun_msg_tlv *tb[__PARSE_ATTR_MAX];
 141         static const struct stun_tlv_policy policy[__PARSE_ATTR_MAX] = {
 142                 [PARSE_ATTR_MAPPED] = { STUN_TLV_MAPPED_ADDRESS, 8 },
 143                 [PARSE_ATTR_XOR_MAPPED] = { STUN_TLV_XOR_MAPPED_ADDRESS, 8 }
 144         };
 145         const struct stun_msg_hdr *hdr = data;
 146         const void *tlv_data;
 147         uint16_t port;
 148
 149         if (!req->pending)
 150                 return false;
 151
 152         if (!stun_msg_is_valid(data, len))
 153                 return false;
 154
 155         if (hdr->msg_type != htons(STUN_MSGTYPE_BINDING_RESPONSE))
 156                 return false;
 157
 158         if (memcmp(hdr->transaction, req->transaction, sizeof(hdr->transaction)) != 0)
 159                 return false;
 160
 161         stun_msg_parse(policy, tb, __PARSE_ATTR_MAX, data, len);
 162
 163         if (tb[PARSE_ATTR_XOR_MAPPED]) {
 164                 tlv_data = tb[PARSE_ATTR_XOR_MAPPED] + 1;
 165                 tlv_data += 2;
 166                 port = ntohs(*(const uint16_t *)tlv_data);
 167                 port ^= STUN_MAGIC >> 16;
 168         } else if (tb[PARSE_ATTR_MAPPED]) {
 169                 tlv_data = tb[PARSE_ATTR_MAPPED] + 1;
 170                 tlv_data += 2;
 171                 port = ntohs(*(const uint16_t *)tlv_data);
 172         } else
 173                 return false;
 174
 175         req->port = port;
 176         return true;
 177 }