]>
Commit | Line | Data |
---|---|---|
2fb975da TT |
1 | /* NHRP packet handling functions |
2 | * Copyright (c) 2014-2015 Timo Teräs | |
3 | * | |
4 | * This file is free software: you may copy, redistribute and/or modify | |
5 | * it under the terms of the GNU General Public License as published by | |
6 | * the Free Software Foundation, either version 2 of the License, or | |
7 | * (at your option) any later version. | |
8 | */ | |
9 | ||
b45ac5f5 DL |
10 | #ifdef HAVE_CONFIG_H |
11 | #include "config.h" | |
12 | #endif | |
13 | ||
2fb975da TT |
14 | #include <netinet/if_ether.h> |
15 | #include "nhrpd.h" | |
16 | #include "zbuf.h" | |
17 | #include "thread.h" | |
18 | #include "hash.h" | |
19 | ||
20 | #include "nhrp_protocol.h" | |
21 | #include "os.h" | |
22 | ||
23 | struct nhrp_reqid_pool nhrp_packet_reqid; | |
24 | ||
25 | static uint16_t family2proto(int family) | |
26 | { | |
27 | switch (family) { | |
996c9314 LB |
28 | case AF_INET: |
29 | return ETH_P_IP; | |
30 | case AF_INET6: | |
31 | return ETH_P_IPV6; | |
2fb975da TT |
32 | } |
33 | return 0; | |
34 | } | |
35 | ||
36 | static int proto2family(uint16_t proto) | |
37 | { | |
38 | switch (proto) { | |
996c9314 LB |
39 | case ETH_P_IP: |
40 | return AF_INET; | |
41 | case ETH_P_IPV6: | |
42 | return AF_INET6; | |
2fb975da TT |
43 | } |
44 | return AF_UNSPEC; | |
45 | } | |
46 | ||
996c9314 LB |
47 | struct nhrp_packet_header *nhrp_packet_push(struct zbuf *zb, uint8_t type, |
48 | const union sockunion *src_nbma, | |
49 | const union sockunion *src_proto, | |
50 | const union sockunion *dst_proto) | |
2fb975da TT |
51 | { |
52 | struct nhrp_packet_header *hdr; | |
53 | ||
54 | hdr = zbuf_push(zb, struct nhrp_packet_header); | |
996c9314 LB |
55 | if (!hdr) |
56 | return NULL; | |
2fb975da | 57 | |
996c9314 | 58 | *hdr = (struct nhrp_packet_header){ |
2fb975da | 59 | .afnum = htons(family2afi(sockunion_family(src_nbma))), |
996c9314 LB |
60 | .protocol_type = |
61 | htons(family2proto(sockunion_family(src_proto))), | |
2fb975da TT |
62 | .version = NHRP_VERSION_RFC2332, |
63 | .type = type, | |
64 | .hop_count = 64, | |
65 | .src_nbma_address_len = sockunion_get_addrlen(src_nbma), | |
66 | .src_protocol_address_len = sockunion_get_addrlen(src_proto), | |
67 | .dst_protocol_address_len = sockunion_get_addrlen(dst_proto), | |
68 | }; | |
69 | ||
70 | zbuf_put(zb, sockunion_get_addr(src_nbma), hdr->src_nbma_address_len); | |
996c9314 LB |
71 | zbuf_put(zb, sockunion_get_addr(src_proto), |
72 | hdr->src_protocol_address_len); | |
73 | zbuf_put(zb, sockunion_get_addr(dst_proto), | |
74 | hdr->dst_protocol_address_len); | |
2fb975da TT |
75 | |
76 | return hdr; | |
77 | } | |
78 | ||
996c9314 LB |
79 | struct nhrp_packet_header *nhrp_packet_pull(struct zbuf *zb, |
80 | union sockunion *src_nbma, | |
81 | union sockunion *src_proto, | |
82 | union sockunion *dst_proto) | |
2fb975da TT |
83 | { |
84 | struct nhrp_packet_header *hdr; | |
85 | ||
86 | hdr = zbuf_pull(zb, struct nhrp_packet_header); | |
996c9314 LB |
87 | if (!hdr) |
88 | return NULL; | |
89 | ||
90 | sockunion_set(src_nbma, afi2family(htons(hdr->afnum)), | |
91 | zbuf_pulln(zb, | |
92 | hdr->src_nbma_address_len | |
93 | + hdr->src_nbma_subaddress_len), | |
94 | hdr->src_nbma_address_len + hdr->src_nbma_subaddress_len); | |
95 | sockunion_set(src_proto, proto2family(htons(hdr->protocol_type)), | |
96 | zbuf_pulln(zb, hdr->src_protocol_address_len), | |
97 | hdr->src_protocol_address_len); | |
98 | sockunion_set(dst_proto, proto2family(htons(hdr->protocol_type)), | |
99 | zbuf_pulln(zb, hdr->dst_protocol_address_len), | |
100 | hdr->dst_protocol_address_len); | |
2fb975da TT |
101 | |
102 | return hdr; | |
103 | } | |
104 | ||
105 | uint16_t nhrp_packet_calculate_checksum(const uint8_t *pdu, uint16_t len) | |
106 | { | |
996c9314 | 107 | const uint16_t *pdu16 = (const uint16_t *)pdu; |
2fb975da TT |
108 | uint32_t csum = 0; |
109 | int i; | |
110 | ||
111 | for (i = 0; i < len / 2; i++) | |
112 | csum += pdu16[i]; | |
113 | if (len & 1) | |
114 | csum += htons(pdu[len - 1]); | |
115 | ||
116 | while (csum & 0xffff0000) | |
117 | csum = (csum & 0xffff) + (csum >> 16); | |
118 | ||
119 | return (~csum) & 0xffff; | |
120 | } | |
121 | ||
122 | void nhrp_packet_complete(struct zbuf *zb, struct nhrp_packet_header *hdr) | |
123 | { | |
124 | unsigned short size; | |
125 | ||
126 | if (hdr->extension_offset) | |
996c9314 LB |
127 | nhrp_ext_push(zb, hdr, |
128 | NHRP_EXTENSION_END | |
129 | | NHRP_EXTENSION_FLAG_COMPULSORY); | |
2fb975da TT |
130 | |
131 | size = zb->tail - (uint8_t *)hdr; | |
132 | hdr->packet_size = htons(size); | |
133 | hdr->checksum = 0; | |
996c9314 | 134 | hdr->checksum = nhrp_packet_calculate_checksum((uint8_t *)hdr, size); |
2fb975da TT |
135 | } |
136 | ||
996c9314 LB |
137 | struct nhrp_cie_header *nhrp_cie_push(struct zbuf *zb, uint8_t code, |
138 | const union sockunion *nbma, | |
139 | const union sockunion *proto) | |
2fb975da TT |
140 | { |
141 | struct nhrp_cie_header *cie; | |
142 | ||
143 | cie = zbuf_push(zb, struct nhrp_cie_header); | |
996c9314 | 144 | *cie = (struct nhrp_cie_header){ |
2fb975da TT |
145 | .code = code, |
146 | }; | |
147 | if (nbma) { | |
148 | cie->nbma_address_len = sockunion_get_addrlen(nbma); | |
149 | zbuf_put(zb, sockunion_get_addr(nbma), cie->nbma_address_len); | |
150 | } | |
151 | if (proto) { | |
152 | cie->protocol_address_len = sockunion_get_addrlen(proto); | |
996c9314 LB |
153 | zbuf_put(zb, sockunion_get_addr(proto), |
154 | cie->protocol_address_len); | |
2fb975da TT |
155 | } |
156 | ||
157 | return cie; | |
158 | } | |
159 | ||
996c9314 LB |
160 | struct nhrp_cie_header *nhrp_cie_pull(struct zbuf *zb, |
161 | struct nhrp_packet_header *hdr, | |
162 | union sockunion *nbma, | |
163 | union sockunion *proto) | |
2fb975da TT |
164 | { |
165 | struct nhrp_cie_header *cie; | |
166 | ||
167 | cie = zbuf_pull(zb, struct nhrp_cie_header); | |
996c9314 LB |
168 | if (!cie) |
169 | return NULL; | |
2fb975da | 170 | |
c4822682 | 171 | if (cie->nbma_address_len + cie->nbma_subaddress_len > 0) { |
996c9314 LB |
172 | sockunion_set(nbma, afi2family(htons(hdr->afnum)), |
173 | zbuf_pulln(zb, | |
174 | cie->nbma_address_len | |
175 | + cie->nbma_subaddress_len), | |
176 | cie->nbma_address_len + cie->nbma_subaddress_len); | |
2fb975da TT |
177 | } else { |
178 | sockunion_family(nbma) = AF_UNSPEC; | |
179 | } | |
180 | ||
181 | if (cie->protocol_address_len) { | |
996c9314 LB |
182 | sockunion_set(proto, proto2family(htons(hdr->protocol_type)), |
183 | zbuf_pulln(zb, cie->protocol_address_len), | |
184 | cie->protocol_address_len); | |
2fb975da TT |
185 | } else { |
186 | sockunion_family(proto) = AF_UNSPEC; | |
187 | } | |
188 | ||
189 | return cie; | |
190 | } | |
191 | ||
996c9314 LB |
192 | struct nhrp_extension_header * |
193 | nhrp_ext_push(struct zbuf *zb, struct nhrp_packet_header *hdr, uint16_t type) | |
2fb975da TT |
194 | { |
195 | struct nhrp_extension_header *ext; | |
196 | ext = zbuf_push(zb, struct nhrp_extension_header); | |
996c9314 LB |
197 | if (!ext) |
198 | return NULL; | |
2fb975da TT |
199 | |
200 | if (!hdr->extension_offset) | |
996c9314 LB |
201 | hdr->extension_offset = |
202 | htons(zb->tail - (uint8_t *)hdr | |
203 | - sizeof(struct nhrp_extension_header)); | |
2fb975da | 204 | |
996c9314 LB |
205 | *ext = (struct nhrp_extension_header){ |
206 | .type = htons(type), .length = 0, | |
2fb975da TT |
207 | }; |
208 | return ext; | |
209 | } | |
210 | ||
211 | void nhrp_ext_complete(struct zbuf *zb, struct nhrp_extension_header *ext) | |
212 | { | |
996c9314 LB |
213 | ext->length = htons(zb->tail - (uint8_t *)ext |
214 | - sizeof(struct nhrp_extension_header)); | |
2fb975da TT |
215 | } |
216 | ||
996c9314 LB |
217 | struct nhrp_extension_header *nhrp_ext_pull(struct zbuf *zb, |
218 | struct zbuf *payload) | |
2fb975da TT |
219 | { |
220 | struct nhrp_extension_header *ext; | |
221 | uint16_t plen; | |
222 | ||
223 | ext = zbuf_pull(zb, struct nhrp_extension_header); | |
996c9314 LB |
224 | if (!ext) |
225 | return NULL; | |
2fb975da TT |
226 | |
227 | plen = htons(ext->length); | |
228 | zbuf_init(payload, zbuf_pulln(zb, plen), plen, plen); | |
229 | return ext; | |
230 | } | |
231 | ||
996c9314 LB |
232 | void nhrp_ext_request(struct zbuf *zb, struct nhrp_packet_header *hdr, |
233 | struct interface *ifp) | |
2fb975da TT |
234 | { |
235 | /* Place holders for standard extensions */ | |
996c9314 LB |
236 | nhrp_ext_push(zb, hdr, |
237 | NHRP_EXTENSION_FORWARD_TRANSIT_NHS | |
238 | | NHRP_EXTENSION_FLAG_COMPULSORY); | |
239 | nhrp_ext_push(zb, hdr, | |
240 | NHRP_EXTENSION_REVERSE_TRANSIT_NHS | |
241 | | NHRP_EXTENSION_FLAG_COMPULSORY); | |
242 | nhrp_ext_push(zb, hdr, | |
243 | NHRP_EXTENSION_RESPONDER_ADDRESS | |
244 | | NHRP_EXTENSION_FLAG_COMPULSORY); | |
2fb975da TT |
245 | } |
246 | ||
996c9314 LB |
247 | int nhrp_ext_reply(struct zbuf *zb, struct nhrp_packet_header *hdr, |
248 | struct interface *ifp, struct nhrp_extension_header *ext, | |
249 | struct zbuf *extpayload) | |
2fb975da TT |
250 | { |
251 | struct nhrp_interface *nifp = ifp->info; | |
252 | struct nhrp_afi_data *ad = &nifp->afi[htons(hdr->afnum)]; | |
253 | struct nhrp_extension_header *dst; | |
254 | struct nhrp_cie_header *cie; | |
255 | uint16_t type; | |
256 | ||
257 | type = htons(ext->type) & ~NHRP_EXTENSION_FLAG_COMPULSORY; | |
258 | if (type == NHRP_EXTENSION_END) | |
259 | return 0; | |
260 | ||
261 | dst = nhrp_ext_push(zb, hdr, htons(ext->type)); | |
996c9314 LB |
262 | if (!dst) |
263 | goto err; | |
2fb975da TT |
264 | |
265 | switch (type) { | |
266 | case NHRP_EXTENSION_RESPONDER_ADDRESS: | |
996c9314 LB |
267 | cie = nhrp_cie_push(zb, NHRP_CODE_SUCCESS, &nifp->nbma, |
268 | &ad->addr); | |
269 | if (!cie) | |
270 | goto err; | |
1e52c954 | 271 | cie->mtu = htons(ad->mtu); |
2fb975da TT |
272 | cie->holding_time = htons(ad->holdtime); |
273 | break; | |
274 | default: | |
275 | if (type & NHRP_EXTENSION_FLAG_COMPULSORY) | |
276 | goto err; | |
996c9314 | 277 | /* fallthru */ |
2fb975da TT |
278 | case NHRP_EXTENSION_FORWARD_TRANSIT_NHS: |
279 | case NHRP_EXTENSION_REVERSE_TRANSIT_NHS: | |
280 | /* Supported compulsory extensions, and any | |
281 | * non-compulsory that is not explicitly handled, | |
282 | * should be just copied. */ | |
283 | zbuf_copy(zb, extpayload, zbuf_used(extpayload)); | |
284 | break; | |
285 | } | |
286 | nhrp_ext_complete(zb, dst); | |
287 | return 0; | |
288 | err: | |
289 | zbuf_set_werror(zb); | |
290 | return -1; | |
291 | } | |
292 | ||
293 | static int nhrp_packet_recvraw(struct thread *t) | |
294 | { | |
295 | int fd = THREAD_FD(t), ifindex; | |
296 | struct zbuf *zb; | |
297 | struct interface *ifp; | |
298 | struct nhrp_peer *p; | |
299 | union sockunion remote_nbma; | |
300 | uint8_t addr[64]; | |
301 | size_t len, addrlen; | |
302 | ||
ffa2c898 | 303 | thread_add_read(master, nhrp_packet_recvraw, 0, fd, NULL); |
2fb975da TT |
304 | |
305 | zb = zbuf_alloc(1500); | |
996c9314 LB |
306 | if (!zb) |
307 | return 0; | |
2fb975da TT |
308 | |
309 | len = zbuf_size(zb); | |
310 | addrlen = sizeof(addr); | |
311 | if (os_recvmsg(zb->buf, &len, &ifindex, addr, &addrlen) < 0) | |
312 | goto err; | |
313 | ||
314 | zb->head = zb->buf; | |
315 | zb->tail = zb->buf + len; | |
316 | ||
317 | switch (addrlen) { | |
318 | case 4: | |
319 | sockunion_set(&remote_nbma, AF_INET, addr, addrlen); | |
320 | break; | |
321 | default: | |
322 | goto err; | |
323 | } | |
324 | ||
7e2b7603 | 325 | ifp = if_lookup_by_index(ifindex, VRF_DEFAULT); |
996c9314 LB |
326 | if (!ifp) |
327 | goto err; | |
2fb975da TT |
328 | |
329 | p = nhrp_peer_get(ifp, &remote_nbma); | |
996c9314 LB |
330 | if (!p) |
331 | goto err; | |
2fb975da TT |
332 | |
333 | nhrp_peer_recv(p, zb); | |
334 | nhrp_peer_unref(p); | |
335 | return 0; | |
336 | ||
337 | err: | |
338 | zbuf_free(zb); | |
339 | return 0; | |
340 | } | |
341 | ||
342 | int nhrp_packet_init(void) | |
343 | { | |
ffa2c898 | 344 | thread_add_read(master, nhrp_packet_recvraw, 0, os_socket(), NULL); |
2fb975da TT |
345 | return 0; |
346 | } |