1 // SPDX-License-Identifier: GPL-2.0-or-later
2 /* NHRP packet handling functions
3 * Copyright (c) 2014-2015 Timo Teräs
10 #include <netinet/if_ether.h>
16 #include "nhrp_protocol.h"
19 struct nhrp_reqid_pool nhrp_packet_reqid
;
21 static uint16_t family2proto(int family
)
32 static int proto2family(uint16_t proto
)
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
)
48 struct nhrp_packet_header
*hdr
;
50 hdr
= zbuf_push(zb
, struct nhrp_packet_header
);
54 *hdr
= (struct nhrp_packet_header
){
55 .afnum
= htons(family2afi(sockunion_family(src_nbma
))),
57 htons(family2proto(sockunion_family(src_proto
))),
58 .version
= NHRP_VERSION_RFC2332
,
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
),
66 zbuf_put(zb
, sockunion_get_addr(src_nbma
), hdr
->src_nbma_address_len
);
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
);
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
)
80 struct nhrp_packet_header
*hdr
;
82 hdr
= zbuf_pull(zb
, struct nhrp_packet_header
);
86 sockunion_set(src_nbma
, afi2family(htons(hdr
->afnum
)),
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
);
101 uint16_t nhrp_packet_calculate_checksum(const uint8_t *pdu
, uint16_t len
)
103 const uint16_t *pdu16
= (const uint16_t *)pdu
;
107 for (i
= 0; i
< len
/ 2; i
++)
110 csum
+= htons(pdu
[len
- 1]);
112 while (csum
& 0xffff0000)
113 csum
= (csum
& 0xffff) + (csum
>> 16);
115 return (~csum
) & 0xffff;
118 void nhrp_packet_complete(struct zbuf
*zb
, struct nhrp_packet_header
*hdr
)
122 if (hdr
->extension_offset
)
123 nhrp_ext_push(zb
, hdr
,
125 | NHRP_EXTENSION_FLAG_COMPULSORY
);
127 size
= zb
->tail
- (uint8_t *)hdr
;
128 hdr
->packet_size
= htons(size
);
130 hdr
->checksum
= nhrp_packet_calculate_checksum((uint8_t *)hdr
, size
);
133 struct nhrp_cie_header
*nhrp_cie_push(struct zbuf
*zb
, uint8_t code
,
134 const union sockunion
*nbma
,
135 const union sockunion
*proto
)
137 struct nhrp_cie_header
*cie
;
139 cie
= zbuf_push(zb
, struct nhrp_cie_header
);
140 *cie
= (struct nhrp_cie_header
){
144 cie
->nbma_address_len
= sockunion_get_addrlen(nbma
);
145 zbuf_put(zb
, sockunion_get_addr(nbma
), cie
->nbma_address_len
);
148 cie
->protocol_address_len
= sockunion_get_addrlen(proto
);
149 zbuf_put(zb
, sockunion_get_addr(proto
),
150 cie
->protocol_address_len
);
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
)
161 struct nhrp_cie_header
*cie
;
163 cie
= zbuf_pull(zb
, struct nhrp_cie_header
);
167 if (cie
->nbma_address_len
+ cie
->nbma_subaddress_len
> 0) {
168 sockunion_set(nbma
, afi2family(htons(hdr
->afnum
)),
170 cie
->nbma_address_len
171 + cie
->nbma_subaddress_len
),
172 cie
->nbma_address_len
+ cie
->nbma_subaddress_len
);
174 sockunion_family(nbma
) = AF_UNSPEC
;
177 if (cie
->protocol_address_len
) {
178 sockunion_set(proto
, proto2family(htons(hdr
->protocol_type
)),
179 zbuf_pulln(zb
, cie
->protocol_address_len
),
180 cie
->protocol_address_len
);
182 sockunion_family(proto
) = AF_UNSPEC
;
188 struct nhrp_extension_header
*
189 nhrp_ext_push(struct zbuf
*zb
, struct nhrp_packet_header
*hdr
, uint16_t type
)
191 struct nhrp_extension_header
*ext
;
192 ext
= zbuf_push(zb
, struct nhrp_extension_header
);
196 if (!hdr
->extension_offset
)
197 hdr
->extension_offset
=
198 htons(zb
->tail
- (uint8_t *)hdr
199 - sizeof(struct nhrp_extension_header
));
201 *ext
= (struct nhrp_extension_header
){
202 .type
= htons(type
), .length
= 0,
207 void nhrp_ext_complete(struct zbuf
*zb
, struct nhrp_extension_header
*ext
)
209 ext
->length
= htons(zb
->tail
- (uint8_t *)ext
210 - sizeof(struct nhrp_extension_header
));
213 struct nhrp_extension_header
*nhrp_ext_pull(struct zbuf
*zb
,
214 struct zbuf
*payload
)
216 struct nhrp_extension_header
*ext
;
219 ext
= zbuf_pull(zb
, struct nhrp_extension_header
);
223 plen
= htons(ext
->length
);
224 zbuf_init(payload
, zbuf_pulln(zb
, plen
), plen
, plen
);
228 void nhrp_ext_request(struct zbuf
*zb
, struct nhrp_packet_header
*hdr
,
229 struct interface
*ifp
)
231 /* Place holders for standard extensions */
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
);
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
)
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
;
253 type
= htons(ext
->type
) & ~NHRP_EXTENSION_FLAG_COMPULSORY
;
254 if (type
== NHRP_EXTENSION_END
)
257 dst
= nhrp_ext_push(zb
, hdr
, htons(ext
->type
));
262 case NHRP_EXTENSION_RESPONDER_ADDRESS
:
263 cie
= nhrp_cie_push(zb
, NHRP_CODE_SUCCESS
, &nifp
->nbma
,
267 cie
->mtu
= htons(ad
->mtu
);
268 cie
->holding_time
= htons(ad
->holdtime
);
271 if (type
& NHRP_EXTENSION_FLAG_COMPULSORY
)
274 case NHRP_EXTENSION_FORWARD_TRANSIT_NHS
:
275 case NHRP_EXTENSION_REVERSE_TRANSIT_NHS
:
276 /* Supported compulsory extensions, and any
277 * non-compulsory that is not explicitly handled,
278 * should be just copied. */
279 zbuf_copy(zb
, extpayload
, zbuf_used(extpayload
));
282 nhrp_ext_complete(zb
, dst
);
289 static void nhrp_packet_recvraw(struct thread
*t
)
291 int fd
= THREAD_FD(t
), ifindex
;
293 struct interface
*ifp
;
295 union sockunion remote_nbma
;
299 thread_add_read(master
, nhrp_packet_recvraw
, 0, fd
, NULL
);
301 zb
= zbuf_alloc(1500);
306 addrlen
= sizeof(addr
);
307 if (os_recvmsg(zb
->buf
, &len
, &ifindex
, addr
, &addrlen
) < 0)
311 zb
->tail
= zb
->buf
+ len
;
315 sockunion_set(&remote_nbma
, AF_INET
, addr
, addrlen
);
321 ifp
= if_lookup_by_index(ifindex
, VRF_DEFAULT
);
325 p
= nhrp_peer_get(ifp
, &remote_nbma
);
329 nhrp_peer_recv(p
, zb
);
337 int nhrp_packet_init(void)
339 thread_add_read(master
, nhrp_packet_recvraw
, 0, os_socket(), NULL
);