1 /* NHRP packet handling functions
2 * Copyright (c) 2014-2015 Timo Teräs
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.
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
)
24 case AF_INET
: return ETH_P_IP
;
25 case AF_INET6
: return ETH_P_IPV6
;
30 static int proto2family(uint16_t proto
)
33 case ETH_P_IP
: return AF_INET
;
34 case ETH_P_IPV6
: return AF_INET6
;
39 struct nhrp_packet_header
*nhrp_packet_push(
40 struct zbuf
*zb
, uint8_t type
,
41 const union sockunion
*src_nbma
,
42 const union sockunion
*src_proto
,
43 const union sockunion
*dst_proto
)
45 struct nhrp_packet_header
*hdr
;
47 hdr
= zbuf_push(zb
, struct nhrp_packet_header
);
48 if (!hdr
) return NULL
;
50 *hdr
= (struct nhrp_packet_header
) {
51 .afnum
= htons(family2afi(sockunion_family(src_nbma
))),
52 .protocol_type
= htons(family2proto(sockunion_family(src_proto
))),
53 .version
= NHRP_VERSION_RFC2332
,
56 .src_nbma_address_len
= sockunion_get_addrlen(src_nbma
),
57 .src_protocol_address_len
= sockunion_get_addrlen(src_proto
),
58 .dst_protocol_address_len
= sockunion_get_addrlen(dst_proto
),
61 zbuf_put(zb
, sockunion_get_addr(src_nbma
), hdr
->src_nbma_address_len
);
62 zbuf_put(zb
, sockunion_get_addr(src_proto
), hdr
->src_protocol_address_len
);
63 zbuf_put(zb
, sockunion_get_addr(dst_proto
), hdr
->dst_protocol_address_len
);
68 struct nhrp_packet_header
*nhrp_packet_pull(
70 union sockunion
*src_nbma
,
71 union sockunion
*src_proto
,
72 union sockunion
*dst_proto
)
74 struct nhrp_packet_header
*hdr
;
76 hdr
= zbuf_pull(zb
, struct nhrp_packet_header
);
77 if (!hdr
) return NULL
;
80 src_nbma
, afi2family(htons(hdr
->afnum
)),
81 zbuf_pulln(zb
, hdr
->src_nbma_address_len
+ hdr
->src_nbma_subaddress_len
),
82 hdr
->src_nbma_address_len
+ hdr
->src_nbma_subaddress_len
);
84 src_proto
, proto2family(htons(hdr
->protocol_type
)),
85 zbuf_pulln(zb
, hdr
->src_protocol_address_len
),
86 hdr
->src_protocol_address_len
);
88 dst_proto
, proto2family(htons(hdr
->protocol_type
)),
89 zbuf_pulln(zb
, hdr
->dst_protocol_address_len
),
90 hdr
->dst_protocol_address_len
);
95 uint16_t nhrp_packet_calculate_checksum(const uint8_t *pdu
, uint16_t len
)
97 const uint16_t *pdu16
= (const uint16_t *) pdu
;
101 for (i
= 0; i
< len
/ 2; i
++)
104 csum
+= htons(pdu
[len
- 1]);
106 while (csum
& 0xffff0000)
107 csum
= (csum
& 0xffff) + (csum
>> 16);
109 return (~csum
) & 0xffff;
112 void nhrp_packet_complete(struct zbuf
*zb
, struct nhrp_packet_header
*hdr
)
116 if (hdr
->extension_offset
)
117 nhrp_ext_push(zb
, hdr
, NHRP_EXTENSION_END
| NHRP_EXTENSION_FLAG_COMPULSORY
);
119 size
= zb
->tail
- (uint8_t *)hdr
;
120 hdr
->packet_size
= htons(size
);
122 hdr
->checksum
= nhrp_packet_calculate_checksum((uint8_t *) hdr
, size
);
125 struct nhrp_cie_header
*nhrp_cie_push(
128 const union sockunion
*nbma
,
129 const union sockunion
*proto
)
131 struct nhrp_cie_header
*cie
;
133 cie
= zbuf_push(zb
, struct nhrp_cie_header
);
134 *cie
= (struct nhrp_cie_header
) {
138 cie
->nbma_address_len
= sockunion_get_addrlen(nbma
);
139 zbuf_put(zb
, sockunion_get_addr(nbma
), cie
->nbma_address_len
);
142 cie
->protocol_address_len
= sockunion_get_addrlen(proto
);
143 zbuf_put(zb
, sockunion_get_addr(proto
), cie
->protocol_address_len
);
149 struct nhrp_cie_header
*nhrp_cie_pull(
151 struct nhrp_packet_header
*hdr
,
152 union sockunion
*nbma
,
153 union sockunion
*proto
)
155 struct nhrp_cie_header
*cie
;
157 cie
= zbuf_pull(zb
, struct nhrp_cie_header
);
158 if (!cie
) return NULL
;
160 if (cie
->nbma_address_len
+ cie
->nbma_subaddress_len
) {
162 nbma
, afi2family(htons(hdr
->afnum
)),
163 zbuf_pulln(zb
, cie
->nbma_address_len
+ cie
->nbma_subaddress_len
),
164 cie
->nbma_address_len
+ cie
->nbma_subaddress_len
);
166 sockunion_family(nbma
) = AF_UNSPEC
;
169 if (cie
->protocol_address_len
) {
171 proto
, proto2family(htons(hdr
->protocol_type
)),
172 zbuf_pulln(zb
, cie
->protocol_address_len
),
173 cie
->protocol_address_len
);
175 sockunion_family(proto
) = AF_UNSPEC
;
181 struct nhrp_extension_header
*nhrp_ext_push(struct zbuf
*zb
, struct nhrp_packet_header
*hdr
, uint16_t type
)
183 struct nhrp_extension_header
*ext
;
184 ext
= zbuf_push(zb
, struct nhrp_extension_header
);
185 if (!ext
) return NULL
;
187 if (!hdr
->extension_offset
)
188 hdr
->extension_offset
= htons(zb
->tail
- (uint8_t*) hdr
- sizeof(struct nhrp_extension_header
));
190 *ext
= (struct nhrp_extension_header
) {
197 void nhrp_ext_complete(struct zbuf
*zb
, struct nhrp_extension_header
*ext
)
199 ext
->length
= htons(zb
->tail
- (uint8_t*)ext
- sizeof(struct nhrp_extension_header
));
202 struct nhrp_extension_header
*nhrp_ext_pull(struct zbuf
*zb
, struct zbuf
*payload
)
204 struct nhrp_extension_header
*ext
;
207 ext
= zbuf_pull(zb
, struct nhrp_extension_header
);
208 if (!ext
) return NULL
;
210 plen
= htons(ext
->length
);
211 zbuf_init(payload
, zbuf_pulln(zb
, plen
), plen
, plen
);
215 void nhrp_ext_request(struct zbuf
*zb
, struct nhrp_packet_header
*hdr
, struct interface
*ifp
)
217 /* Place holders for standard extensions */
218 nhrp_ext_push(zb
, hdr
, NHRP_EXTENSION_FORWARD_TRANSIT_NHS
| NHRP_EXTENSION_FLAG_COMPULSORY
);
219 nhrp_ext_push(zb
, hdr
, NHRP_EXTENSION_REVERSE_TRANSIT_NHS
| NHRP_EXTENSION_FLAG_COMPULSORY
);
220 nhrp_ext_push(zb
, hdr
, NHRP_EXTENSION_RESPONDER_ADDRESS
| NHRP_EXTENSION_FLAG_COMPULSORY
);
223 int nhrp_ext_reply(struct zbuf
*zb
, struct nhrp_packet_header
*hdr
, struct interface
*ifp
, struct nhrp_extension_header
*ext
, struct zbuf
*extpayload
)
225 struct nhrp_interface
*nifp
= ifp
->info
;
226 struct nhrp_afi_data
*ad
= &nifp
->afi
[htons(hdr
->afnum
)];
227 struct nhrp_extension_header
*dst
;
228 struct nhrp_cie_header
*cie
;
231 type
= htons(ext
->type
) & ~NHRP_EXTENSION_FLAG_COMPULSORY
;
232 if (type
== NHRP_EXTENSION_END
)
235 dst
= nhrp_ext_push(zb
, hdr
, htons(ext
->type
));
239 case NHRP_EXTENSION_RESPONDER_ADDRESS
:
240 cie
= nhrp_cie_push(zb
, NHRP_CODE_SUCCESS
, &nifp
->nbma
, &ad
->addr
);
242 cie
->holding_time
= htons(ad
->holdtime
);
245 if (type
& NHRP_EXTENSION_FLAG_COMPULSORY
)
247 case NHRP_EXTENSION_FORWARD_TRANSIT_NHS
:
248 case NHRP_EXTENSION_REVERSE_TRANSIT_NHS
:
249 /* Supported compulsory extensions, and any
250 * non-compulsory that is not explicitly handled,
251 * should be just copied. */
252 zbuf_copy(zb
, extpayload
, zbuf_used(extpayload
));
255 nhrp_ext_complete(zb
, dst
);
262 static int nhrp_packet_recvraw(struct thread
*t
)
264 int fd
= THREAD_FD(t
), ifindex
;
266 struct interface
*ifp
;
268 union sockunion remote_nbma
;
272 thread_add_read(master
, nhrp_packet_recvraw
, 0, fd
);
274 zb
= zbuf_alloc(1500);
278 addrlen
= sizeof(addr
);
279 if (os_recvmsg(zb
->buf
, &len
, &ifindex
, addr
, &addrlen
) < 0)
283 zb
->tail
= zb
->buf
+ len
;
287 sockunion_set(&remote_nbma
, AF_INET
, addr
, addrlen
);
293 ifp
= if_lookup_by_index(ifindex
);
296 p
= nhrp_peer_get(ifp
, &remote_nbma
);
299 nhrp_peer_recv(p
, zb
);
308 int nhrp_packet_init(void)
310 thread_add_read(master
, nhrp_packet_recvraw
, 0, os_socket());