diff options
Diffstat (limited to 'nhrpd/nhrp_packet.c')
| -rw-r--r-- | nhrpd/nhrp_packet.c | 312 |
1 files changed, 312 insertions, 0 deletions
diff --git a/nhrpd/nhrp_packet.c b/nhrpd/nhrp_packet.c new file mode 100644 index 00000000..5d2866a6 --- /dev/null +++ b/nhrpd/nhrp_packet.c @@ -0,0 +1,312 @@ +/* NHRP packet handling functions + * Copyright (c) 2014-2015 Timo Teräs + * + * This file is free software: you may copy, redistribute and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 2 of the License, or + * (at your option) any later version. + */ + +#include <netinet/if_ether.h> +#include "nhrpd.h" +#include "zbuf.h" +#include "thread.h" +#include "hash.h" + +#include "nhrp_protocol.h" +#include "os.h" + +struct nhrp_reqid_pool nhrp_packet_reqid; + +static uint16_t family2proto(int family) +{ + switch (family) { + case AF_INET: return ETH_P_IP; + case AF_INET6: return ETH_P_IPV6; + } + return 0; +} + +static int proto2family(uint16_t proto) +{ + switch (proto) { + case ETH_P_IP: return AF_INET; + case ETH_P_IPV6: return AF_INET6; + } + return AF_UNSPEC; +} + +struct nhrp_packet_header *nhrp_packet_push( + struct zbuf *zb, uint8_t type, + const union sockunion *src_nbma, + const union sockunion *src_proto, + const union sockunion *dst_proto) +{ + struct nhrp_packet_header *hdr; + + hdr = zbuf_push(zb, struct nhrp_packet_header); + if (!hdr) return NULL; + + *hdr = (struct nhrp_packet_header) { + .afnum = htons(family2afi(sockunion_family(src_nbma))), + .protocol_type = htons(family2proto(sockunion_family(src_proto))), + .version = NHRP_VERSION_RFC2332, + .type = type, + .hop_count = 64, + .src_nbma_address_len = sockunion_get_addrlen(src_nbma), + .src_protocol_address_len = sockunion_get_addrlen(src_proto), + .dst_protocol_address_len = sockunion_get_addrlen(dst_proto), + }; + + zbuf_put(zb, sockunion_get_addr(src_nbma), hdr->src_nbma_address_len); + zbuf_put(zb, sockunion_get_addr(src_proto), hdr->src_protocol_address_len); + zbuf_put(zb, sockunion_get_addr(dst_proto), hdr->dst_protocol_address_len); + + return hdr; +} + +struct nhrp_packet_header *nhrp_packet_pull( + struct zbuf *zb, + union sockunion *src_nbma, + union sockunion *src_proto, + union sockunion *dst_proto) +{ + struct nhrp_packet_header *hdr; + + hdr = zbuf_pull(zb, struct nhrp_packet_header); + if (!hdr) return NULL; + + sockunion_set( + src_nbma, afi2family(htons(hdr->afnum)), + zbuf_pulln(zb, hdr->src_nbma_address_len + hdr->src_nbma_subaddress_len), + hdr->src_nbma_address_len + hdr->src_nbma_subaddress_len); + sockunion_set( + src_proto, proto2family(htons(hdr->protocol_type)), + zbuf_pulln(zb, hdr->src_protocol_address_len), + hdr->src_protocol_address_len); + sockunion_set( + dst_proto, proto2family(htons(hdr->protocol_type)), + zbuf_pulln(zb, hdr->dst_protocol_address_len), + hdr->dst_protocol_address_len); + + return hdr; +} + +uint16_t nhrp_packet_calculate_checksum(const uint8_t *pdu, uint16_t len) +{ + const uint16_t *pdu16 = (const uint16_t *) pdu; + uint32_t csum = 0; + int i; + + for (i = 0; i < len / 2; i++) + csum += pdu16[i]; + if (len & 1) + csum += htons(pdu[len - 1]); + + while (csum & 0xffff0000) + csum = (csum & 0xffff) + (csum >> 16); + + return (~csum) & 0xffff; +} + +void nhrp_packet_complete(struct zbuf *zb, struct nhrp_packet_header *hdr) +{ + unsigned short size; + + if (hdr->extension_offset) + nhrp_ext_push(zb, hdr, NHRP_EXTENSION_END | NHRP_EXTENSION_FLAG_COMPULSORY); + + size = zb->tail - (uint8_t *)hdr; + hdr->packet_size = htons(size); + hdr->checksum = 0; + hdr->checksum = nhrp_packet_calculate_checksum((uint8_t *) hdr, size); +} + +struct nhrp_cie_header *nhrp_cie_push( + struct zbuf *zb, + uint8_t code, + const union sockunion *nbma, + const union sockunion *proto) +{ + struct nhrp_cie_header *cie; + + cie = zbuf_push(zb, struct nhrp_cie_header); + *cie = (struct nhrp_cie_header) { + .code = code, + }; + if (nbma) { + cie->nbma_address_len = sockunion_get_addrlen(nbma); + zbuf_put(zb, sockunion_get_addr(nbma), cie->nbma_address_len); + } + if (proto) { + cie->protocol_address_len = sockunion_get_addrlen(proto); + zbuf_put(zb, sockunion_get_addr(proto), cie->protocol_address_len); + } + + return cie; +} + +struct nhrp_cie_header *nhrp_cie_pull( + struct zbuf *zb, + struct nhrp_packet_header *hdr, + union sockunion *nbma, + union sockunion *proto) +{ + struct nhrp_cie_header *cie; + + cie = zbuf_pull(zb, struct nhrp_cie_header); + if (!cie) return NULL; + + if (cie->nbma_address_len + cie->nbma_subaddress_len) { + sockunion_set( + nbma, afi2family(htons(hdr->afnum)), + zbuf_pulln(zb, cie->nbma_address_len + cie->nbma_subaddress_len), + cie->nbma_address_len + cie->nbma_subaddress_len); + } else { + sockunion_family(nbma) = AF_UNSPEC; + } + + if (cie->protocol_address_len) { + sockunion_set( + proto, proto2family(htons(hdr->protocol_type)), + zbuf_pulln(zb, cie->protocol_address_len), + cie->protocol_address_len); + } else { + sockunion_family(proto) = AF_UNSPEC; + } + + return cie; +} + +struct nhrp_extension_header *nhrp_ext_push(struct zbuf *zb, struct nhrp_packet_header *hdr, uint16_t type) +{ + struct nhrp_extension_header *ext; + ext = zbuf_push(zb, struct nhrp_extension_header); + if (!ext) return NULL; + + if (!hdr->extension_offset) + hdr->extension_offset = htons(zb->tail - (uint8_t*) hdr - sizeof(struct nhrp_extension_header)); + + *ext = (struct nhrp_extension_header) { + .type = htons(type), + .length = 0, + }; + return ext; +} + +void nhrp_ext_complete(struct zbuf *zb, struct nhrp_extension_header *ext) +{ + ext->length = htons(zb->tail - (uint8_t*)ext - sizeof(struct nhrp_extension_header)); +} + +struct nhrp_extension_header *nhrp_ext_pull(struct zbuf *zb, struct zbuf *payload) +{ + struct nhrp_extension_header *ext; + uint16_t plen; + + ext = zbuf_pull(zb, struct nhrp_extension_header); + if (!ext) return NULL; + + plen = htons(ext->length); + zbuf_init(payload, zbuf_pulln(zb, plen), plen, plen); + return ext; +} + +void nhrp_ext_request(struct zbuf *zb, struct nhrp_packet_header *hdr, struct interface *ifp) +{ + /* Place holders for standard extensions */ + nhrp_ext_push(zb, hdr, NHRP_EXTENSION_FORWARD_TRANSIT_NHS | NHRP_EXTENSION_FLAG_COMPULSORY); + nhrp_ext_push(zb, hdr, NHRP_EXTENSION_REVERSE_TRANSIT_NHS | NHRP_EXTENSION_FLAG_COMPULSORY); + nhrp_ext_push(zb, hdr, NHRP_EXTENSION_RESPONDER_ADDRESS | NHRP_EXTENSION_FLAG_COMPULSORY); +} + +int nhrp_ext_reply(struct zbuf *zb, struct nhrp_packet_header *hdr, struct interface *ifp, struct nhrp_extension_header *ext, struct zbuf *extpayload) +{ + struct nhrp_interface *nifp = ifp->info; + struct nhrp_afi_data *ad = &nifp->afi[htons(hdr->afnum)]; + struct nhrp_extension_header *dst; + struct nhrp_cie_header *cie; + uint16_t type; + + type = htons(ext->type) & ~NHRP_EXTENSION_FLAG_COMPULSORY; + if (type == NHRP_EXTENSION_END) + return 0; + + dst = nhrp_ext_push(zb, hdr, htons(ext->type)); + if (!dst) goto err; + + switch (type) { + case NHRP_EXTENSION_RESPONDER_ADDRESS: + cie = nhrp_cie_push(zb, NHRP_CODE_SUCCESS, &nifp->nbma, &ad->addr); + if (!cie) goto err; + cie->holding_time = htons(ad->holdtime); + break; + default: + if (type & NHRP_EXTENSION_FLAG_COMPULSORY) + goto err; + case NHRP_EXTENSION_FORWARD_TRANSIT_NHS: + case NHRP_EXTENSION_REVERSE_TRANSIT_NHS: + /* Supported compulsory extensions, and any + * non-compulsory that is not explicitly handled, + * should be just copied. */ + zbuf_copy(zb, extpayload, zbuf_used(extpayload)); + break; + } + nhrp_ext_complete(zb, dst); + return 0; +err: + zbuf_set_werror(zb); + return -1; +} + +static int nhrp_packet_recvraw(struct thread *t) +{ + int fd = THREAD_FD(t), ifindex; + struct zbuf *zb; + struct interface *ifp; + struct nhrp_peer *p; + union sockunion remote_nbma; + uint8_t addr[64]; + size_t len, addrlen; + + thread_add_read(master, nhrp_packet_recvraw, 0, fd); + + zb = zbuf_alloc(1500); + if (!zb) return 0; + + len = zbuf_size(zb); + addrlen = sizeof(addr); + if (os_recvmsg(zb->buf, &len, &ifindex, addr, &addrlen) < 0) + goto err; + + zb->head = zb->buf; + zb->tail = zb->buf + len; + + switch (addrlen) { + case 4: + sockunion_set(&remote_nbma, AF_INET, addr, addrlen); + break; + default: + goto err; + } + + ifp = if_lookup_by_index(ifindex); + if (!ifp) goto err; + + p = nhrp_peer_get(ifp, &remote_nbma); + if (!p) goto err; + + nhrp_peer_recv(p, zb); + nhrp_peer_unref(p); + return 0; + +err: + zbuf_free(zb); + return 0; +} + +int nhrp_packet_init(void) +{ + thread_add_read(master, nhrp_packet_recvraw, 0, os_socket()); + return 0; +} |