source: src/router/quagga/nhrpd/nhrp_packet.c @ 31640

Last change on this file since 31640 was 31640, checked in by brainslayer, 2 months ago

update quagga

File size: 7.9 KB
Line 
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
19struct nhrp_reqid_pool nhrp_packet_reqid;
20
21static uint16_t family2proto(int family)
22{
23        switch (family) {
24        case AF_INET: return ETH_P_IP;
25        case AF_INET6: return ETH_P_IPV6;
26        }
27        return 0;
28}
29
30static int proto2family(uint16_t proto)
31{
32        switch (proto) {
33        case ETH_P_IP: return AF_INET;
34        case ETH_P_IPV6: return AF_INET6;
35        }
36        return AF_UNSPEC;
37}
38
39struct 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)
44{
45        struct nhrp_packet_header *hdr;
46
47        hdr = zbuf_push(zb, struct nhrp_packet_header);
48        if (!hdr) return NULL;
49
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,
54                .type = type,
55                .hop_count = 64,
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),
59        };
60
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);
64
65        return hdr;
66}
67
68struct nhrp_packet_header *nhrp_packet_pull(
69        struct zbuf *zb,
70        union sockunion *src_nbma,
71        union sockunion *src_proto,
72        union sockunion *dst_proto)
73{
74        struct nhrp_packet_header *hdr;
75
76        hdr = zbuf_pull(zb, struct nhrp_packet_header);
77        if (!hdr) return NULL;
78
79        sockunion_set(
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);
83        sockunion_set(
84                src_proto, proto2family(htons(hdr->protocol_type)),
85                zbuf_pulln(zb, hdr->src_protocol_address_len),
86                hdr->src_protocol_address_len);
87        sockunion_set(
88                dst_proto, proto2family(htons(hdr->protocol_type)),
89                zbuf_pulln(zb, hdr->dst_protocol_address_len),
90                hdr->dst_protocol_address_len);
91
92        return hdr;
93}
94
95uint16_t nhrp_packet_calculate_checksum(const uint8_t *pdu, uint16_t len)
96{
97        const uint16_t *pdu16 = (const uint16_t *) pdu;
98        uint32_t csum = 0;
99        int i;
100
101        for (i = 0; i < len / 2; i++)
102                csum += pdu16[i];
103        if (len & 1)
104                csum += htons(pdu[len - 1]);
105
106        while (csum & 0xffff0000)
107                csum = (csum & 0xffff) + (csum >> 16);
108
109        return (~csum) & 0xffff;
110}
111
112void nhrp_packet_complete(struct zbuf *zb, struct nhrp_packet_header *hdr)
113{
114        unsigned short size;
115
116        if (hdr->extension_offset)
117                nhrp_ext_push(zb, hdr, NHRP_EXTENSION_END | NHRP_EXTENSION_FLAG_COMPULSORY);
118
119        size = zb->tail - (uint8_t *)hdr;
120        hdr->packet_size = htons(size);
121        hdr->checksum = 0;
122        hdr->checksum = nhrp_packet_calculate_checksum((uint8_t *) hdr, size);
123}
124
125struct nhrp_cie_header *nhrp_cie_push(
126        struct zbuf *zb,
127        uint8_t code,
128        const union sockunion *nbma,
129        const union sockunion *proto)
130{
131        struct nhrp_cie_header *cie;
132
133        cie = zbuf_push(zb, struct nhrp_cie_header);
134        *cie = (struct nhrp_cie_header) {
135                .code = code,
136        };
137        if (nbma) {
138                cie->nbma_address_len = sockunion_get_addrlen(nbma);
139                zbuf_put(zb, sockunion_get_addr(nbma), cie->nbma_address_len);
140        }
141        if (proto) {
142                cie->protocol_address_len = sockunion_get_addrlen(proto);
143                zbuf_put(zb, sockunion_get_addr(proto), cie->protocol_address_len);
144        }
145
146        return cie;
147}
148
149struct nhrp_cie_header *nhrp_cie_pull(
150        struct zbuf *zb,
151        struct nhrp_packet_header *hdr,
152        union sockunion *nbma,
153        union sockunion *proto)
154{
155        struct nhrp_cie_header *cie;
156
157        cie = zbuf_pull(zb, struct nhrp_cie_header);
158        if (!cie) return NULL;
159
160        if (cie->nbma_address_len + cie->nbma_subaddress_len) {
161                sockunion_set(
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);
165        } else {
166                sockunion_family(nbma) = AF_UNSPEC;
167        }
168
169        if (cie->protocol_address_len) {
170                sockunion_set(
171                        proto, proto2family(htons(hdr->protocol_type)),
172                        zbuf_pulln(zb, cie->protocol_address_len),
173                        cie->protocol_address_len);
174        } else {
175                sockunion_family(proto) = AF_UNSPEC;
176        }
177
178        return cie;
179}
180
181struct nhrp_extension_header *nhrp_ext_push(struct zbuf *zb, struct nhrp_packet_header *hdr, uint16_t type)
182{
183        struct nhrp_extension_header *ext;
184        ext = zbuf_push(zb, struct nhrp_extension_header);
185        if (!ext) return NULL;
186
187        if (!hdr->extension_offset)
188                hdr->extension_offset = htons(zb->tail - (uint8_t*) hdr - sizeof(struct nhrp_extension_header));
189
190        *ext = (struct nhrp_extension_header) {
191                .type = htons(type),
192                .length = 0,
193        };
194        return ext;
195}
196
197void nhrp_ext_complete(struct zbuf *zb, struct nhrp_extension_header *ext)
198{
199        ext->length = htons(zb->tail - (uint8_t*)ext - sizeof(struct nhrp_extension_header));
200}
201
202struct nhrp_extension_header *nhrp_ext_pull(struct zbuf *zb, struct zbuf *payload)
203{
204        struct nhrp_extension_header *ext;
205        uint16_t plen;
206
207        ext = zbuf_pull(zb, struct nhrp_extension_header);
208        if (!ext) return NULL;
209
210        plen = htons(ext->length);
211        zbuf_init(payload, zbuf_pulln(zb, plen), plen, plen);
212        return ext;
213}
214
215void nhrp_ext_request(struct zbuf *zb, struct nhrp_packet_header *hdr, struct interface *ifp)
216{
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);
221}
222
223int nhrp_ext_reply(struct zbuf *zb, struct nhrp_packet_header *hdr, struct interface *ifp, struct nhrp_extension_header *ext, struct zbuf *extpayload)
224{
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;
229        uint16_t type;
230
231        type = htons(ext->type) & ~NHRP_EXTENSION_FLAG_COMPULSORY;
232        if (type == NHRP_EXTENSION_END)
233                return 0;
234
235        dst = nhrp_ext_push(zb, hdr, htons(ext->type));
236        if (!dst) goto err;
237
238        switch (type) {
239        case NHRP_EXTENSION_RESPONDER_ADDRESS:
240                cie = nhrp_cie_push(zb, NHRP_CODE_SUCCESS, &nifp->nbma, &ad->addr);
241                if (!cie) goto err;
242                cie->holding_time = htons(ad->holdtime);
243                break;
244        default:
245                if (type & NHRP_EXTENSION_FLAG_COMPULSORY)
246                        goto err;
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));
253                break;
254        }
255        nhrp_ext_complete(zb, dst);
256        return 0;
257err:
258        zbuf_set_werror(zb);
259        return -1;
260}
261
262static int nhrp_packet_recvraw(struct thread *t)
263{
264        int fd = THREAD_FD(t), ifindex;
265        struct zbuf *zb;
266        struct interface *ifp;
267        struct nhrp_peer *p;
268        union sockunion remote_nbma;
269        uint8_t addr[64];
270        size_t len, addrlen;
271
272        thread_add_read(master, nhrp_packet_recvraw, 0, fd);
273
274        zb = zbuf_alloc(1500);
275        if (!zb) return 0;
276
277        len = zbuf_size(zb);
278        addrlen = sizeof(addr);
279        if (os_recvmsg(zb->buf, &len, &ifindex, addr, &addrlen) < 0)
280                goto err;
281
282        zb->head = zb->buf;
283        zb->tail = zb->buf + len;
284
285        switch (addrlen) {
286        case 4:
287                sockunion_set(&remote_nbma, AF_INET, addr, addrlen);
288                break;
289        default:
290                goto err;
291        }
292
293        ifp = if_lookup_by_index(ifindex);
294        if (!ifp) goto err;
295
296        p = nhrp_peer_get(ifp, &remote_nbma);
297        if (!p) goto err;
298
299        nhrp_peer_recv(p, zb);
300        nhrp_peer_unref(p);
301        return 0;
302
303err:
304        zbuf_free(zb);
305        return 0;
306}
307
308int nhrp_packet_init(void)
309{
310        thread_add_read(master, nhrp_packet_recvraw, 0, os_socket());
311        return 0;
312}
Note: See TracBrowser for help on using the repository browser.