summaryrefslogtreecommitdiffstats
path: root/nhrpd/nhrp_packet.c
diff options
context:
space:
mode:
Diffstat (limited to 'nhrpd/nhrp_packet.c')
-rw-r--r--nhrpd/nhrp_packet.c345
1 files changed, 345 insertions, 0 deletions
diff --git a/nhrpd/nhrp_packet.c b/nhrpd/nhrp_packet.c
new file mode 100644
index 0000000..2407a8d
--- /dev/null
+++ b/nhrpd/nhrp_packet.c
@@ -0,0 +1,345 @@
+/* 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.
+ */
+
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#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 > 0) {
+ 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->mtu = htons(ad->mtu);
+ cie->holding_time = htons(ad->holdtime);
+ break;
+ default:
+ if (type & NHRP_EXTENSION_FLAG_COMPULSORY)
+ goto err;
+ /* fallthru */
+ 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 void 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, NULL);
+
+ zb = zbuf_alloc(1500);
+ if (!zb)
+ return;
+
+ 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, VRF_DEFAULT);
+ 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;
+
+err:
+ zbuf_free(zb);
+}
+
+int nhrp_packet_init(void)
+{
+ thread_add_read(master, nhrp_packet_recvraw, 0, os_socket(), NULL);
+ return 0;
+}