diff options
| -rw-r--r-- | nhrpd/linux.c | 11 | ||||
| -rw-r--r-- | nhrpd/nhrp_interface.c | 2 | ||||
| -rwxr-xr-x | nhrpd/nhrp_multicast.c | 307 | ||||
| -rw-r--r-- | nhrpd/nhrp_peer.c | 3 | ||||
| -rw-r--r-- | nhrpd/nhrp_vty.c | 63 | ||||
| -rw-r--r-- | nhrpd/nhrpd.h | 16 | ||||
| -rw-r--r-- | nhrpd/os.h | 2 | ||||
| -rw-r--r-- | nhrpd/subdir.am | 1 |
8 files changed, 398 insertions, 7 deletions
diff --git a/nhrpd/linux.c b/nhrpd/linux.c index 59c82b1c5..20825149b 100644 --- a/nhrpd/linux.c +++ b/nhrpd/linux.c @@ -15,6 +15,7 @@ #include <stdio.h> #include <unistd.h> #include <string.h> +#include <errno.h> #include <sys/ioctl.h> #include <sys/socket.h> #include <sys/types.h> @@ -42,7 +43,7 @@ int os_socket(void) } int os_sendmsg(const uint8_t *buf, size_t len, int ifindex, const uint8_t *addr, - size_t addrlen) + size_t addrlen, uint16_t protocol) { struct sockaddr_ll lladdr; struct iovec iov = { @@ -61,16 +62,16 @@ int os_sendmsg(const uint8_t *buf, size_t len, int ifindex, const uint8_t *addr, memset(&lladdr, 0, sizeof(lladdr)); lladdr.sll_family = AF_PACKET; - lladdr.sll_protocol = htons(ETH_P_NHRP); + lladdr.sll_protocol = htons(protocol); lladdr.sll_ifindex = ifindex; lladdr.sll_halen = addrlen; memcpy(lladdr.sll_addr, addr, addrlen); - status = sendmsg(nhrp_socket_fd, &msg, 0); + status = sendmsg(os_socket(), &msg, 0); if (status < 0) - return -1; + return -errno; - return 0; + return status; } int os_recvmsg(uint8_t *buf, size_t *len, int *ifindex, uint8_t *addr, diff --git a/nhrpd/nhrp_interface.c b/nhrpd/nhrp_interface.c index a6880054f..e1215c295 100644 --- a/nhrpd/nhrp_interface.c +++ b/nhrpd/nhrp_interface.c @@ -42,6 +42,7 @@ static int nhrp_if_new_hook(struct interface *ifp) struct nhrp_afi_data *ad = &nifp->afi[afi]; ad->holdtime = NHRPD_DEFAULT_HOLDTIME; list_init(&ad->nhslist_head); + list_init(&ad->mcastlist_head); } return 0; @@ -55,6 +56,7 @@ static int nhrp_if_delete_hook(struct interface *ifp) nhrp_cache_interface_del(ifp); nhrp_nhs_interface_del(ifp); + nhrp_multicast_interface_del(ifp); nhrp_peer_interface_del(ifp); if (nifp->ipsec_profile) diff --git a/nhrpd/nhrp_multicast.c b/nhrpd/nhrp_multicast.c new file mode 100755 index 000000000..0c5de838a --- /dev/null +++ b/nhrpd/nhrp_multicast.c @@ -0,0 +1,307 @@ +/* NHRP Multicast Support + * Copyright (c) 2020-2021 4RF Limited + * + * 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 <fcntl.h> +#include <net/if.h> +#include <net/ethernet.h> +#include <netinet/if_ether.h> +#include <linux/netlink.h> +#include <linux/neighbour.h> +#include <linux/netfilter/nfnetlink_log.h> +#include <linux/if_packet.h> +#include <sys/types.h> +#include <sys/socket.h> + +#include "thread.h" +#include "nhrpd.h" +#include "netlink.h" +#include "znl.h" +#include "os.h" + +DEFINE_MTYPE_STATIC(NHRPD, NHRP_MULTICAST, "NHRP Multicast") + +static int netlink_mcast_nflog_group; +static int netlink_mcast_log_fd = -1; +static struct thread *netlink_mcast_log_thread; + +struct mcast_ctx { + struct interface *ifp; + struct zbuf *pkt; +}; + +static void nhrp_multicast_send(struct nhrp_peer *p, struct zbuf *zb) +{ + char buf[2][256]; + size_t addrlen; + int ret; + + addrlen = sockunion_get_addrlen(&p->vc->remote.nbma); + ret = os_sendmsg(zb->head, zbuf_used(zb), p->ifp->ifindex, + sockunion_get_addr(&p->vc->remote.nbma), + addrlen, addrlen == 4 ? 0x0800 : 0x86DD); + + debugf(NHRP_DEBUG_COMMON, "Multicast Packet: %s -> %s, ret = %d, size = %d, addrlen = %d", + sockunion2str(&p->vc->local.nbma, buf[0], sizeof(buf[0])), + sockunion2str(&p->vc->remote.nbma, buf[1], sizeof(buf[1])), + ret, zbuf_used(zb), addrlen); +} + +static void nhrp_multicast_forward_nbma(union sockunion *nbma_addr, struct interface *ifp, struct zbuf *pkt) +{ + struct nhrp_peer *p = nhrp_peer_get(ifp, nbma_addr); + if(p && p->online) { + /* Send packet */ + nhrp_multicast_send(p, pkt); + nhrp_peer_unref(p); + } +} + +static void nhrp_multicast_forward_cache(struct nhrp_cache *c, void *pctx) +{ + struct mcast_ctx *ctx = (struct mcast_ctx *)pctx; + + if (c->cur.type == NHRP_CACHE_DYNAMIC && c->cur.peer) + nhrp_multicast_forward_nbma(&c->cur.peer->vc->remote.nbma, ctx->ifp, ctx->pkt); +} + +static void nhrp_multicast_forward(struct nhrp_multicast *mcast, void *pctx) +{ + struct mcast_ctx *ctx = (struct mcast_ctx *)pctx; + struct nhrp_interface *nifp = ctx->ifp->info; + + if (!nifp->enabled) + return; + + /* dynamic */ + if (sockunion_family(&mcast->nbma_addr) == AF_UNSPEC) { + nhrp_cache_foreach(ctx->ifp, nhrp_multicast_forward_cache, pctx); + return; + } + + /* Fixed IP Address */ + nhrp_multicast_forward_nbma(&mcast->nbma_addr, ctx->ifp, ctx->pkt); +} + +static void netlink_mcast_log_handler(struct nlmsghdr *msg, struct zbuf *zb) +{ + struct nfgenmsg *nf; + struct rtattr *rta; + struct zbuf rtapl, pktpl; + struct interface *ifp; + uint32_t *out_ndx = NULL; + afi_t afi; + struct mcast_ctx ctx; + + debugf(NHRP_DEBUG_COMMON,"Inside %s\n", __func__); + + nf = znl_pull(zb, sizeof(*nf)); + if (!nf) + return; + + memset(&pktpl, 0, sizeof(pktpl)); + while ((rta = znl_rta_pull(zb, &rtapl)) != NULL) { + switch (rta->rta_type) { + case NFULA_IFINDEX_OUTDEV: + out_ndx = znl_pull(&rtapl, sizeof(*out_ndx)); + break; + case NFULA_PAYLOAD: + pktpl = rtapl; + break; + /* NFULA_HWHDR exists and is supposed to contain source + * hardware address. However, for ip_gre it seems to be + * the nexthop destination address if the packet matches + * route. */ + } + } + + if (!out_ndx || !zbuf_used(&pktpl)) + return; + + ifp = if_lookup_by_index(htonl(*out_ndx), VRF_DEFAULT); + if (!ifp) + return; + + debugf(NHRP_DEBUG_COMMON,"Outgoing interface = %s\n", ifp->name); + + ctx = (struct mcast_ctx) { + .ifp = ifp, + .pkt = &pktpl, + }; + + for (afi = 0; afi < AFI_MAX; afi++) { + nhrp_multicast_foreach(ifp, afi, nhrp_multicast_forward, (void *)&ctx); + } +} + +static int netlink_mcast_log_recv(struct thread *t) +{ + uint8_t buf[65535]; /* Max OSPF Packet size */ + int fd = THREAD_FD(t); + struct zbuf payload, zb; + struct nlmsghdr *n; + + netlink_mcast_log_thread = NULL; + + zbuf_init(&zb, buf, sizeof(buf), 0); + while (zbuf_recv(&zb, fd) > 0) { + while ((n = znl_nlmsg_pull(&zb, &payload)) != NULL) { + debugf(NHRP_DEBUG_COMMON, + "Netlink-mcast-log: Received msg_type %u, msg_flags %u", + n->nlmsg_type, n->nlmsg_flags); + switch (n->nlmsg_type) { + case (NFNL_SUBSYS_ULOG << 8) | NFULNL_MSG_PACKET: + netlink_mcast_log_handler(n, &payload); + break; + } + } + } + + thread_add_read(master, netlink_mcast_log_recv, 0, netlink_mcast_log_fd, + &netlink_mcast_log_thread); + + return 0; +} + +static void netlink_mcast_log_register(int fd, int group) +{ + struct nlmsghdr *n; + struct nfgenmsg *nf; + struct nfulnl_msg_config_cmd cmd; + struct zbuf *zb = zbuf_alloc(512); + + n = znl_nlmsg_push(zb, (NFNL_SUBSYS_ULOG << 8) | NFULNL_MSG_CONFIG, + NLM_F_REQUEST | NLM_F_ACK); + nf = znl_push(zb, sizeof(*nf)); + *nf = (struct nfgenmsg){ + .nfgen_family = AF_UNSPEC, + .version = NFNETLINK_V0, + .res_id = htons(group), + }; + cmd.command = NFULNL_CFG_CMD_BIND; + znl_rta_push(zb, NFULA_CFG_CMD, &cmd, sizeof(cmd)); + znl_nlmsg_complete(zb, n); + + zbuf_send(zb, fd); + zbuf_free(zb); +} + +static int nhrp_multicast_free(struct interface *ifp, struct nhrp_multicast *mcast) +{ + list_del(&mcast->list_entry); + XFREE(MTYPE_NHRP_MULTICAST, mcast); + return 0; +} + +static void netlink_mcast_set_nflog_group(struct interface *ifp, int nlgroup) +{ + if (netlink_mcast_log_fd >= 0) { + THREAD_OFF(netlink_mcast_log_thread); + close(netlink_mcast_log_fd); + netlink_mcast_log_fd = -1; + debugf(NHRP_DEBUG_COMMON, "De-register nflog group"); + } + netlink_mcast_nflog_group = nlgroup; + if (nlgroup) { + netlink_mcast_log_fd = znl_open(NETLINK_NETFILTER, 0); + if (netlink_mcast_log_fd < 0) + return; + + netlink_mcast_log_register(netlink_mcast_log_fd, nlgroup); + thread_add_read(master, netlink_mcast_log_recv, 0, netlink_mcast_log_fd, + &netlink_mcast_log_thread); + debugf(NHRP_DEBUG_COMMON, "Register nflog group: %d", netlink_mcast_nflog_group); + } +} + +int nhrp_multicast_add(struct interface *ifp, afi_t afi, union sockunion *nbma_addr) +{ + struct nhrp_interface *nifp = ifp->info; + struct nhrp_multicast *mcast; + char buf[SU_ADDRSTRLEN]; + + list_for_each_entry(mcast, &nifp->afi[afi].mcastlist_head, list_entry) + { + if (sockunion_same(&mcast->nbma_addr, nbma_addr)) + return NHRP_ERR_ENTRY_EXISTS; + } + + mcast = XMALLOC(MTYPE_NHRP_MULTICAST, sizeof(struct nhrp_multicast)); + + *mcast = (struct nhrp_multicast){ + .afi = afi, + .ifp = ifp, + .nbma_addr = *nbma_addr, + }; + list_add_tail(&mcast->list_entry, &nifp->afi[afi].mcastlist_head); + + if (netlink_mcast_log_fd == -1) + netlink_mcast_set_nflog_group(ifp, MCAST_NFLOG_GROUP); + + sockunion2str(nbma_addr, buf, sizeof(buf)); + debugf(NHRP_DEBUG_COMMON, "Adding multicast entry (%s)", buf); + + return NHRP_OK; +} + +int nhrp_multicast_del(struct interface *ifp, afi_t afi, union sockunion *nbma_addr) +{ + struct nhrp_interface *nifp = ifp->info; + struct nhrp_multicast *mcast, *tmp; + char buf[SU_ADDRSTRLEN]; + + list_for_each_entry_safe(mcast, tmp, &nifp->afi[afi].mcastlist_head, + list_entry) + { + if (!sockunion_same(&mcast->nbma_addr, nbma_addr)) + continue; + + sockunion2str(nbma_addr, buf, sizeof(buf)); + debugf(NHRP_DEBUG_COMMON, "Deleting multicast entry (%s)", buf); + + nhrp_multicast_free(ifp, mcast); + + return NHRP_OK; + } + + return NHRP_ERR_ENTRY_NOT_FOUND; +} + +void nhrp_multicast_interface_del(struct interface *ifp) +{ + struct nhrp_interface *nifp = ifp->info; + struct nhrp_multicast *mcast, *tmp; + afi_t afi; + + for (afi = 0; afi < AFI_MAX; afi++) { + debugf(NHRP_DEBUG_COMMON, "Cleaning up multicast entries (%d)", !list_empty(&nifp->afi[afi].mcastlist_head)); + + list_for_each_entry_safe( + mcast, tmp, &nifp->afi[afi].mcastlist_head, + list_entry) { + nhrp_multicast_free(ifp, mcast); + } + } +} + +void nhrp_multicast_foreach(struct interface *ifp, afi_t afi, + void (*cb)(struct nhrp_multicast *, void *), + void *ctx) +{ + struct nhrp_interface *nifp = ifp->info; + struct nhrp_multicast *mcast; + + list_for_each_entry(mcast, &nifp->afi[afi].mcastlist_head, list_entry) + { + cb (mcast, ctx); + } +} diff --git a/nhrpd/nhrp_peer.c b/nhrpd/nhrp_peer.c index 199f3332d..817468076 100644 --- a/nhrpd/nhrp_peer.c +++ b/nhrpd/nhrp_peer.c @@ -375,7 +375,8 @@ void nhrp_peer_send(struct nhrp_peer *p, struct zbuf *zb) os_sendmsg(zb->head, zbuf_used(zb), p->ifp->ifindex, sockunion_get_addr(&p->vc->remote.nbma), - sockunion_get_addrlen(&p->vc->remote.nbma)); + sockunion_get_addrlen(&p->vc->remote.nbma), + ETH_P_NHRP); zbuf_reset(zb); } diff --git a/nhrpd/nhrp_vty.c b/nhrpd/nhrp_vty.c index 4358605e2..e649dc34e 100644 --- a/nhrpd/nhrp_vty.c +++ b/nhrpd/nhrp_vty.c @@ -570,6 +570,53 @@ DEFUN(if_no_nhrp_map, if_no_nhrp_map_cmd, return CMD_SUCCESS; } +DEFUN(if_nhrp_map_multicast, if_nhrp_map_multicast_cmd, + AFI_CMD " nhrp map multicast <A.B.C.D|X:X::X:X|dynamic>", + AFI_STR + NHRP_STR + "Multicast NBMA Configuration\n" + "Use this NBMA mapping for multicasts\n" + "IPv4 NBMA address\n" + "IPv6 NBMA address\n" + "Dynamically learn destinations from client registrations on hub\n") +{ + VTY_DECLVAR_CONTEXT(interface, ifp); + afi_t afi = cmd_to_afi(argv[0]); + union sockunion nbma_addr; + int ret; + + if (str2sockunion(argv[4]->arg, &nbma_addr) < 0) + sockunion_family(&nbma_addr) = AF_UNSPEC; + + ret = nhrp_multicast_add(ifp, afi, &nbma_addr); + + return nhrp_vty_return(vty, ret); +} + +DEFUN(if_no_nhrp_map_multicast, if_no_nhrp_map_multicast_cmd, + "no " AFI_CMD " nhrp map multicast <A.B.C.D|X:X::X:X|dynamic>", + NO_STR + AFI_STR + NHRP_STR + "Multicast NBMA Configuration\n" + "Use this NBMA mapping for multicasts\n" + "IPv4 NBMA address\n" + "IPv6 NBMA address\n" + "Dynamically learn destinations from client registrations on hub\n") +{ + VTY_DECLVAR_CONTEXT(interface, ifp); + afi_t afi = cmd_to_afi(argv[1]); + union sockunion nbma_addr; + int ret; + + if (str2sockunion(argv[5]->arg, &nbma_addr) < 0) + sockunion_family(&nbma_addr) = AF_UNSPEC; + + ret = nhrp_multicast_del(ifp, afi, &nbma_addr); + + return nhrp_vty_return(vty, ret); +} + DEFUN(if_nhrp_nhs, if_nhrp_nhs_cmd, AFI_CMD " nhrp nhs <A.B.C.D|X:X::X:X|dynamic> nbma <A.B.C.D|FQDN>", AFI_STR @@ -1069,6 +1116,7 @@ static int interface_config_write(struct vty *vty) struct interface *ifp; struct nhrp_interface *nifp; struct nhrp_nhs *nhs; + struct nhrp_multicast *mcast; const char *aficmd; afi_t afi; char buf[SU_ADDRSTRLEN]; @@ -1138,6 +1186,19 @@ static int interface_config_write(struct vty *vty) sizeof(buf)), nhs->nbma_fqdn); } + + list_for_each_entry(mcast, &ad->mcastlist_head, + list_entry) + { + vty_out(vty, " %s nhrp map multicast %s\n", + aficmd, + sockunion_family(&mcast->nbma_addr) + == AF_UNSPEC + ? "dynamic" + : sockunion2str( + &mcast->nbma_addr, buf, + sizeof(buf))); + } } vty_endframe(vty, "!\n"); @@ -1192,6 +1253,8 @@ void nhrp_config_init(void) install_element(INTERFACE_NODE, &if_no_nhrp_reg_flags_cmd); install_element(INTERFACE_NODE, &if_nhrp_map_cmd); install_element(INTERFACE_NODE, &if_no_nhrp_map_cmd); + install_element(INTERFACE_NODE, &if_nhrp_map_multicast_cmd); + install_element(INTERFACE_NODE, &if_no_nhrp_map_multicast_cmd); install_element(INTERFACE_NODE, &if_nhrp_nhs_cmd); install_element(INTERFACE_NODE, &if_no_nhrp_nhs_cmd); } diff --git a/nhrpd/nhrpd.h b/nhrpd/nhrpd.h index e4afb22f8..1d0d99915 100644 --- a/nhrpd/nhrpd.h +++ b/nhrpd/nhrpd.h @@ -24,6 +24,7 @@ DECLARE_MGROUP(NHRPD); #define NHRP_VTY_PORT 2610 #define NHRP_DEFAULT_CONFIG "nhrpd.conf" +#define MCAST_NFLOG_GROUP 224 extern struct thread_master *master; @@ -264,6 +265,13 @@ struct nhrp_nhs { struct list_head reglist_head; }; +struct nhrp_multicast { + struct interface *ifp; + struct list_head list_entry; + afi_t afi; + union sockunion nbma_addr; /* IP-address */ +}; + struct nhrp_registration { struct list_head reglist_entry; struct thread *t_register; @@ -309,6 +317,7 @@ struct nhrp_interface { unsigned short mtu; unsigned int holdtime; struct list_head nhslist_head; + struct list_head mcastlist_head; } afi[AFI_MAX]; }; @@ -350,6 +359,13 @@ void nhrp_nhs_foreach(struct interface *ifp, afi_t afi, void *ctx); void nhrp_nhs_interface_del(struct interface *ifp); +int nhrp_multicast_add(struct interface *ifp, afi_t afi, union sockunion *nbma_addr); +int nhrp_multicast_del(struct interface *ifp, afi_t afi, union sockunion *nbma_addr); +void nhrp_multicast_interface_del(struct interface *ifp); +void nhrp_multicast_foreach(struct interface *ifp, afi_t afi, + void (*cb)(struct nhrp_multicast *, void *), + void *ctx); + void nhrp_route_update_nhrp(const struct prefix *p, struct interface *ifp); void nhrp_route_announce(int add, enum nhrp_cache_type type, const struct prefix *p, struct interface *ifp, diff --git a/nhrpd/os.h b/nhrpd/os.h index dd65d3cbe..2b9e07fa6 100644 --- a/nhrpd/os.h +++ b/nhrpd/os.h @@ -1,7 +1,7 @@ int os_socket(void); int os_sendmsg(const uint8_t *buf, size_t len, int ifindex, const uint8_t *addr, - size_t addrlen); + size_t addrlen, uint16_t protocol); int os_recvmsg(uint8_t *buf, size_t *len, int *ifindex, uint8_t *addr, size_t *addrlen); int os_configure_dmvpn(unsigned int ifindex, const char *ifname, int af); diff --git a/nhrpd/subdir.am b/nhrpd/subdir.am index 61b1fe31b..d00aecc1e 100644 --- a/nhrpd/subdir.am +++ b/nhrpd/subdir.am @@ -22,6 +22,7 @@ nhrpd_nhrpd_SOURCES = \ nhrpd/nhrp_nhs.c \ nhrpd/nhrp_packet.c \ nhrpd/nhrp_peer.c \ + nhrpd/nhrp_multicast.c \ nhrpd/nhrp_route.c \ nhrpd/nhrp_shortcut.c \ nhrpd/nhrp_vc.c \ |