nhrpd/nhrp_multicast.c

   1 // SPDX-License-Identifier: GPL-2.0-or-later
   2 /* NHRP Multicast Support
   3  * Copyright (c) 2020-2021 4RF Limited
   4  */
   5
   6 #ifdef HAVE_CONFIG_H
   7 #include "config.h"
   8 #endif
   9
  10 #include <fcntl.h>
  11 #include <net/if.h>
  12 #include <net/ethernet.h>
  13 #include <netinet/if_ether.h>
  14 #include <linux/netlink.h>
  15 #include <linux/neighbour.h>
  16 #include <linux/netfilter/nfnetlink_log.h>
  17 #include <linux/if_packet.h>
  18 #include <sys/types.h>
  19 #include <sys/socket.h>
  20
  21 #include "thread.h"
  22 #include "nhrpd.h"
  23 #include "netlink.h"
  24 #include "znl.h"
  25 #include "os.h"
  26
  27 DEFINE_MTYPE_STATIC(NHRPD, NHRP_MULTICAST, "NHRP Multicast");
  28
  29 int netlink_mcast_nflog_group;
  30 static int netlink_mcast_log_fd = -1;
  31 static struct thread *netlink_mcast_log_thread;
  32
  33 struct mcast_ctx {
  34         struct interface *ifp;
  35         struct zbuf *pkt;
  36 };
  37
  38 static void nhrp_multicast_send(struct nhrp_peer *p, struct zbuf *zb)
  39 {
  40         size_t addrlen;
  41         int ret;
  42
  43         addrlen = sockunion_get_addrlen(&p->vc->remote.nbma);
  44         ret = os_sendmsg(zb->head, zbuf_used(zb), p->ifp->ifindex,
  45                          sockunion_get_addr(&p->vc->remote.nbma), addrlen,
  46                          addrlen == 4 ? ETH_P_IP : ETH_P_IPV6);
  47
  48         debugf(NHRP_DEBUG_COMMON,
  49                "Multicast Packet: %pSU -> %pSU, ret = %d, size = %zu, addrlen = %zu",
  50                &p->vc->local.nbma, &p->vc->remote.nbma, ret, zbuf_used(zb),
  51                addrlen);
  52 }
  53
  54 static void nhrp_multicast_forward_nbma(union sockunion *nbma_addr,
  55                                         struct interface *ifp, struct zbuf *pkt)
  56 {
  57         struct nhrp_peer *p = nhrp_peer_get(ifp, nbma_addr);
  58
  59         if (p && p->online) {
  60                 /* Send packet */
  61                 nhrp_multicast_send(p, pkt);
  62         }
  63         nhrp_peer_unref(p);
  64 }
  65
  66 static void nhrp_multicast_forward_cache(struct nhrp_cache *c, void *pctx)
  67 {
  68         struct mcast_ctx *ctx = (struct mcast_ctx *)pctx;
  69
  70         if (c->cur.type == NHRP_CACHE_DYNAMIC && c->cur.peer)
  71                 nhrp_multicast_forward_nbma(&c->cur.peer->vc->remote.nbma,
  72                                             ctx->ifp, ctx->pkt);
  73 }
  74
  75 static void nhrp_multicast_forward(struct nhrp_multicast *mcast, void *pctx)
  76 {
  77         struct mcast_ctx *ctx = (struct mcast_ctx *)pctx;
  78         struct nhrp_interface *nifp = ctx->ifp->info;
  79
  80         if (!nifp->enabled)
  81                 return;
  82
  83         /* dynamic */
  84         if (sockunion_family(&mcast->nbma_addr) == AF_UNSPEC) {
  85                 nhrp_cache_foreach(ctx->ifp, nhrp_multicast_forward_cache,
  86                                    pctx);
  87                 return;
  88         }
  89
  90         /* Fixed IP Address */
  91         nhrp_multicast_forward_nbma(&mcast->nbma_addr, ctx->ifp, ctx->pkt);
  92 }
  93
  94 static void netlink_mcast_log_handler(struct nlmsghdr *msg, struct zbuf *zb)
  95 {
  96         struct nfgenmsg *nf;
  97         struct rtattr *rta;
  98         struct zbuf rtapl;
  99         uint32_t *out_ndx = NULL;
 100         afi_t afi;
 101         struct mcast_ctx ctx;
 102
 103         nf = znl_pull(zb, sizeof(*nf));
 104         if (!nf)
 105                 return;
 106
 107         ctx.pkt = NULL;
 108         while ((rta = znl_rta_pull(zb, &rtapl)) != NULL) {
 109                 switch (rta->rta_type) {
 110                 case NFULA_IFINDEX_OUTDEV:
 111                         out_ndx = znl_pull(&rtapl, sizeof(*out_ndx));
 112                         break;
 113                 case NFULA_PAYLOAD:
 114                         ctx.pkt = &rtapl;
 115                         break;
 116                         /* NFULA_HWHDR exists and is supposed to contain source
 117                          * hardware address. However, for ip_gre it seems to be
 118                          * the nexthop destination address if the packet matches
 119                          * route.
 120                          */
 121                 }
 122         }
 123
 124         if (!out_ndx || !ctx.pkt)
 125                 return;
 126
 127         ctx.ifp = if_lookup_by_index(htonl(*out_ndx), VRF_DEFAULT);
 128         if (!ctx.ifp)
 129                 return;
 130
 131         debugf(NHRP_DEBUG_COMMON,
 132                "Intercepted multicast packet leaving %s len %zu",
 133                ctx.ifp->name, zbuf_used(ctx.pkt));
 134
 135         for (afi = 0; afi < AFI_MAX; afi++) {
 136                 nhrp_multicast_foreach(ctx.ifp, afi, nhrp_multicast_forward,
 137                                        (void *)&ctx);
 138         }
 139 }
 140
 141 static void netlink_mcast_log_recv(struct thread *t)
 142 {
 143         uint8_t buf[65535]; /* Max OSPF Packet size */
 144         int fd = THREAD_FD(t);
 145         struct zbuf payload, zb;
 146         struct nlmsghdr *n;
 147
 148
 149         zbuf_init(&zb, buf, sizeof(buf), 0);
 150         while (zbuf_recv(&zb, fd) > 0) {
 151                 while ((n = znl_nlmsg_pull(&zb, &payload)) != NULL) {
 152                         debugf(NHRP_DEBUG_COMMON,
 153                                "Netlink-mcast-log: Received msg_type %u, msg_flags %u",
 154                                n->nlmsg_type, n->nlmsg_flags);
 155                         switch (n->nlmsg_type) {
 156                         case (NFNL_SUBSYS_ULOG << 8) | NFULNL_MSG_PACKET:
 157                                 netlink_mcast_log_handler(n, &payload);
 158                                 break;
 159                         }
 160                 }
 161         }
 162
 163         thread_add_read(master, netlink_mcast_log_recv, 0, netlink_mcast_log_fd,
 164                         &netlink_mcast_log_thread);
 165 }
 166
 167 static void netlink_mcast_log_register(int fd, int group)
 168 {
 169         struct nlmsghdr *n;
 170         struct nfgenmsg *nf;
 171         struct nfulnl_msg_config_cmd cmd;
 172         struct zbuf *zb = zbuf_alloc(512);
 173
 174         n = znl_nlmsg_push(zb, (NFNL_SUBSYS_ULOG << 8) | NFULNL_MSG_CONFIG,
 175                            NLM_F_REQUEST | NLM_F_ACK);
 176         nf = znl_push(zb, sizeof(*nf));
 177         *nf = (struct nfgenmsg){
 178                 .nfgen_family = AF_UNSPEC,
 179                 .version = NFNETLINK_V0,
 180                 .res_id = htons(group),
 181         };
 182         cmd.command = NFULNL_CFG_CMD_BIND;
 183         znl_rta_push(zb, NFULA_CFG_CMD, &cmd, sizeof(cmd));
 184         znl_nlmsg_complete(zb, n);
 185
 186         zbuf_send(zb, fd);
 187         zbuf_free(zb);
 188 }
 189
 190 void netlink_mcast_set_nflog_group(int nlgroup)
 191 {
 192         if (netlink_mcast_log_fd >= 0) {
 193                 THREAD_OFF(netlink_mcast_log_thread);
 194                 close(netlink_mcast_log_fd);
 195                 netlink_mcast_log_fd = -1;
 196                 debugf(NHRP_DEBUG_COMMON, "De-register nflog group");
 197         }
 198         netlink_mcast_nflog_group = nlgroup;
 199         if (nlgroup) {
 200                 netlink_mcast_log_fd = znl_open(NETLINK_NETFILTER, 0);
 201                 if (netlink_mcast_log_fd < 0)
 202                         return;
 203
 204                 netlink_mcast_log_register(netlink_mcast_log_fd, nlgroup);
 205                 thread_add_read(master, netlink_mcast_log_recv, 0,
 206                                 netlink_mcast_log_fd,
 207                                 &netlink_mcast_log_thread);
 208                 debugf(NHRP_DEBUG_COMMON, "Register nflog group: %d",
 209                        netlink_mcast_nflog_group);
 210         }
 211 }
 212
 213 static int nhrp_multicast_free(struct interface *ifp,
 214                                struct nhrp_multicast *mcast)
 215 {
 216         struct nhrp_interface *nifp = ifp->info;
 217
 218         nhrp_mcastlist_del(&nifp->afi[mcast->afi].mcastlist_head, mcast);
 219         XFREE(MTYPE_NHRP_MULTICAST, mcast);
 220         return 0;
 221 }
 222
 223 int nhrp_multicast_add(struct interface *ifp, afi_t afi,
 224                        union sockunion *nbma_addr)
 225 {
 226         struct nhrp_interface *nifp = ifp->info;
 227         struct nhrp_multicast *mcast;
 228
 229         frr_each (nhrp_mcastlist, &nifp->afi[afi].mcastlist_head, mcast) {
 230                 if (sockunion_same(&mcast->nbma_addr, nbma_addr))
 231                         return NHRP_ERR_ENTRY_EXISTS;
 232         }
 233
 234         mcast = XMALLOC(MTYPE_NHRP_MULTICAST, sizeof(struct nhrp_multicast));
 235
 236         *mcast = (struct nhrp_multicast){
 237                 .afi = afi, .ifp = ifp, .nbma_addr = *nbma_addr,
 238         };
 239         nhrp_mcastlist_add_tail(&nifp->afi[afi].mcastlist_head, mcast);
 240
 241         debugf(NHRP_DEBUG_COMMON, "Adding multicast entry (%pSU)", nbma_addr);
 242
 243         return NHRP_OK;
 244 }
 245
 246 int nhrp_multicast_del(struct interface *ifp, afi_t afi,
 247                        union sockunion *nbma_addr)
 248 {
 249         struct nhrp_interface *nifp = ifp->info;
 250         struct nhrp_multicast *mcast;
 251
 252         frr_each_safe (nhrp_mcastlist, &nifp->afi[afi].mcastlist_head, mcast) {
 253                 if (!sockunion_same(&mcast->nbma_addr, nbma_addr))
 254                         continue;
 255
 256                 debugf(NHRP_DEBUG_COMMON, "Deleting multicast entry (%pSU)",
 257                        nbma_addr);
 258
 259                 nhrp_multicast_free(ifp, mcast);
 260
 261                 return NHRP_OK;
 262         }
 263
 264         return NHRP_ERR_ENTRY_NOT_FOUND;
 265 }
 266
 267 void nhrp_multicast_interface_del(struct interface *ifp)
 268 {
 269         struct nhrp_interface *nifp = ifp->info;
 270         struct nhrp_multicast *mcast;
 271         afi_t afi;
 272
 273         for (afi = 0; afi < AFI_MAX; afi++) {
 274                 debugf(NHRP_DEBUG_COMMON, "Cleaning up multicast entries (%zu)",
 275                        nhrp_mcastlist_count(&nifp->afi[afi].mcastlist_head));
 276
 277                 frr_each_safe (nhrp_mcastlist, &nifp->afi[afi].mcastlist_head,
 278                                mcast) {
 279                         nhrp_multicast_free(ifp, mcast);
 280                 }
 281         }
 282 }
 283
 284 void nhrp_multicast_foreach(struct interface *ifp, afi_t afi,
 285                             void (*cb)(struct nhrp_multicast *, void *),
 286                             void *ctx)
 287 {
 288         struct nhrp_interface *nifp = ifp->info;
 289         struct nhrp_multicast *mcast;
 290
 291         frr_each (nhrp_mcastlist, &nifp->afi[afi].mcastlist_head, mcast) {
 292                 cb(mcast, ctx);
 293         }
 294 }