1 // SPDX-License-Identifier: GPL-2.0-or-later
3 * Copyright (C) 2022 Felix Fietkau <nbd@nbd.name>
11 static uint8_t tx_buf
[256];
13 bool stun_msg_is_valid(const void *data
, size_t len
)
15 const struct stun_msg_hdr
*hdr
= data
;
17 if (len
<= sizeof(*hdr
))
20 return hdr
->magic
== htonl(STUN_MAGIC
);
23 static void *stun_msg_init(uint16_t type
)
25 struct stun_msg_hdr
*hdr
= (struct stun_msg_hdr
*)tx_buf
;
27 memset(hdr
, 0, sizeof(*hdr
));
28 hdr
->msg_type
= htons(type
);
29 hdr
->magic
= htonl(STUN_MAGIC
);
34 static void *stun_msg_add_tlv(uint16_t type
, uint16_t len
)
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
;
44 tlv
->type
= htons(type
);
45 tlv
->len
= htons(len
);
50 data_len
+= sizeof(*tlv
) + len
;
51 hdr
->msg_len
= htons(data_len
);
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
)
64 type
= ntohs(tlv
->type
);
66 for (i
= 0; i
< len
; i
++) {
67 if (policy
[i
].type
!= type
)
70 if (ntohs(tlv
->len
) < policy
[i
].min_len
)
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
)
83 const struct stun_msg_hdr
*hdr
= data
;
84 const struct stun_msg_tlv
*tlv
;
85 const void *end
= data
+ data_len
;
95 cur_len
= ntohs(tlv
->len
);
96 if (data
+ cur_len
> end
)
99 stun_msg_parse_attr(policy
, tb
, len
, tlv
);
100 data
+= (cur_len
+ 3) & ~3;
104 const void *stun_msg_request_prepare(struct stun_request
*req
, size_t *len
,
105 uint16_t response_port
)
107 struct stun_msg_hdr
*hdr
;
110 hdr
= stun_msg_init(STUN_MSGTYPE_BINDING_REQUEST
);
112 uint16_t *tlv_port
= stun_msg_add_tlv(STUN_TLV_RESPONSE_PORT
, 2);
113 *tlv_port
= htons(response_port
);
116 f
= fopen("/dev/urandom", "r");
120 if (fread(hdr
->transaction
, 12, 1, f
) != 1)
124 memcpy(req
->transaction
, hdr
->transaction
, sizeof(req
->transaction
));
127 *len
= htons(hdr
->msg_len
) + sizeof(*hdr
);
132 bool stun_msg_request_complete(struct stun_request
*req
, const void *data
,
137 PARSE_ATTR_XOR_MAPPED
,
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 }
145 const struct stun_msg_hdr
*hdr
= data
;
146 const void *tlv_data
;
152 if (!stun_msg_is_valid(data
, len
))
155 if (hdr
->msg_type
!= htons(STUN_MSGTYPE_BINDING_RESPONSE
))
158 if (memcmp(hdr
->transaction
, req
->transaction
, sizeof(hdr
->transaction
)) != 0)
161 stun_msg_parse(policy
, tb
, __PARSE_ATTR_MAX
, data
, len
);
163 if (tb
[PARSE_ATTR_XOR_MAPPED
]) {
164 tlv_data
= tb
[PARSE_ATTR_XOR_MAPPED
] + 1;
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;
171 port
= ntohs(*(const uint16_t *)tlv_data
);