nhrpd: Add support for forwarding multicast packets

Forwarding multicast is a pre-requisite for allowing multicast based routing
protocols such as OSPF to work with DMVPN

This code relies on externally adding iptables rule. For example:
iptables -A OUTPUT -d 224.0.0.0/24 -o gre1 -j NFLOG --nflog-group 224

Signed-off-by: Reuben Dowle <reuben.dowle@4rf.com>

diff --git a/nhrpd/linux.c b/nhrpd/linux.c
index 59c82b1c554ca8adba51c5c9377c4da4bdaa9a5c..20825149b850cf8ec8cc83bf31626ac7e1497e94 100644 (file)
--- 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 a6880054fdf141f3aae6e7aceac758d0ad026aaf..e1215c29575da402d5130be4fb20e08959359501 100644 (file)
--- 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 (executable)
index 0000000..0c5de83
--- /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 199f3332d238bfb52eb55dd34a111472b2d813e2..81746807601574fbcbd7e4d35ef6ee2386611dff 100644 (file)
--- 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 4358605e2b03b14511ec00a34d140ec109e84a90..e649dc34e31d809bc9621608e88bc091ce9df0da 100644 (file)
--- 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 e4afb22f803f35f07341d1e3d270347cb3881469..1d0d9991575f125784b02d2f1c3022e0b00baaa4 100644 (file)
--- 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 dd65d3cbe104e8746bfc88e97846a32cbca368c8..2b9e07fa6ef5655b32939f0881ee45ddcd961f4d 100644 (file)
--- 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 61b1fe31b2f203ba4293ec8d5b942bf29eb774ab..d00aecc1ead75d4600d8b393f9f92bb2b6bea5f6 100644 (file)
--- 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 \