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