1 // SPDX-License-Identifier: GPL-2.0-or-later
2 /* NHRP Multicast Support
3 * Copyright (c) 2020-2021 4RF Limited
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>
27 DEFINE_MTYPE_STATIC(NHRPD
, NHRP_MULTICAST
, "NHRP Multicast");
29 int netlink_mcast_nflog_group
;
30 static int netlink_mcast_log_fd
= -1;
31 static struct thread
*netlink_mcast_log_thread
;
34 struct interface
*ifp
;
38 static void nhrp_multicast_send(struct nhrp_peer
*p
, struct zbuf
*zb
)
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
);
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
),
54 static void nhrp_multicast_forward_nbma(union sockunion
*nbma_addr
,
55 struct interface
*ifp
, struct zbuf
*pkt
)
57 struct nhrp_peer
*p
= nhrp_peer_get(ifp
, nbma_addr
);
61 nhrp_multicast_send(p
, pkt
);
66 static void nhrp_multicast_forward_cache(struct nhrp_cache
*c
, void *pctx
)
68 struct mcast_ctx
*ctx
= (struct mcast_ctx
*)pctx
;
70 if (c
->cur
.type
== NHRP_CACHE_DYNAMIC
&& c
->cur
.peer
)
71 nhrp_multicast_forward_nbma(&c
->cur
.peer
->vc
->remote
.nbma
,
75 static void nhrp_multicast_forward(struct nhrp_multicast
*mcast
, void *pctx
)
77 struct mcast_ctx
*ctx
= (struct mcast_ctx
*)pctx
;
78 struct nhrp_interface
*nifp
= ctx
->ifp
->info
;
84 if (sockunion_family(&mcast
->nbma_addr
) == AF_UNSPEC
) {
85 nhrp_cache_foreach(ctx
->ifp
, nhrp_multicast_forward_cache
,
90 /* Fixed IP Address */
91 nhrp_multicast_forward_nbma(&mcast
->nbma_addr
, ctx
->ifp
, ctx
->pkt
);
94 static void netlink_mcast_log_handler(struct nlmsghdr
*msg
, struct zbuf
*zb
)
99 uint32_t *out_ndx
= NULL
;
101 struct mcast_ctx ctx
;
103 nf
= znl_pull(zb
, sizeof(*nf
));
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
));
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
124 if (!out_ndx
|| !ctx
.pkt
)
127 ctx
.ifp
= if_lookup_by_index(htonl(*out_ndx
), VRF_DEFAULT
);
131 debugf(NHRP_DEBUG_COMMON
,
132 "Intercepted multicast packet leaving %s len %zu",
133 ctx
.ifp
->name
, zbuf_used(ctx
.pkt
));
135 for (afi
= 0; afi
< AFI_MAX
; afi
++) {
136 nhrp_multicast_foreach(ctx
.ifp
, afi
, nhrp_multicast_forward
,
141 static void netlink_mcast_log_recv(struct thread
*t
)
143 uint8_t buf
[65535]; /* Max OSPF Packet size */
144 int fd
= THREAD_FD(t
);
145 struct zbuf payload
, zb
;
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
);
163 thread_add_read(master
, netlink_mcast_log_recv
, 0, netlink_mcast_log_fd
,
164 &netlink_mcast_log_thread
);
167 static void netlink_mcast_log_register(int fd
, int group
)
171 struct nfulnl_msg_config_cmd cmd
;
172 struct zbuf
*zb
= zbuf_alloc(512);
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
),
182 cmd
.command
= NFULNL_CFG_CMD_BIND
;
183 znl_rta_push(zb
, NFULA_CFG_CMD
, &cmd
, sizeof(cmd
));
184 znl_nlmsg_complete(zb
, n
);
190 void netlink_mcast_set_nflog_group(int nlgroup
)
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");
198 netlink_mcast_nflog_group
= nlgroup
;
200 netlink_mcast_log_fd
= znl_open(NETLINK_NETFILTER
, 0);
201 if (netlink_mcast_log_fd
< 0)
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
);
213 static int nhrp_multicast_free(struct interface
*ifp
,
214 struct nhrp_multicast
*mcast
)
216 struct nhrp_interface
*nifp
= ifp
->info
;
218 nhrp_mcastlist_del(&nifp
->afi
[mcast
->afi
].mcastlist_head
, mcast
);
219 XFREE(MTYPE_NHRP_MULTICAST
, mcast
);
223 int nhrp_multicast_add(struct interface
*ifp
, afi_t afi
,
224 union sockunion
*nbma_addr
)
226 struct nhrp_interface
*nifp
= ifp
->info
;
227 struct nhrp_multicast
*mcast
;
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
;
234 mcast
= XMALLOC(MTYPE_NHRP_MULTICAST
, sizeof(struct nhrp_multicast
));
236 *mcast
= (struct nhrp_multicast
){
237 .afi
= afi
, .ifp
= ifp
, .nbma_addr
= *nbma_addr
,
239 nhrp_mcastlist_add_tail(&nifp
->afi
[afi
].mcastlist_head
, mcast
);
241 debugf(NHRP_DEBUG_COMMON
, "Adding multicast entry (%pSU)", nbma_addr
);
246 int nhrp_multicast_del(struct interface
*ifp
, afi_t afi
,
247 union sockunion
*nbma_addr
)
249 struct nhrp_interface
*nifp
= ifp
->info
;
250 struct nhrp_multicast
*mcast
;
252 frr_each_safe (nhrp_mcastlist
, &nifp
->afi
[afi
].mcastlist_head
, mcast
) {
253 if (!sockunion_same(&mcast
->nbma_addr
, nbma_addr
))
256 debugf(NHRP_DEBUG_COMMON
, "Deleting multicast entry (%pSU)",
259 nhrp_multicast_free(ifp
, mcast
);
264 return NHRP_ERR_ENTRY_NOT_FOUND
;
267 void nhrp_multicast_interface_del(struct interface
*ifp
)
269 struct nhrp_interface
*nifp
= ifp
->info
;
270 struct nhrp_multicast
*mcast
;
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
));
277 frr_each_safe (nhrp_mcastlist
, &nifp
->afi
[afi
].mcastlist_head
,
279 nhrp_multicast_free(ifp
, mcast
);
284 void nhrp_multicast_foreach(struct interface
*ifp
, afi_t afi
,
285 void (*cb
)(struct nhrp_multicast
*, void *),
288 struct nhrp_interface
*nifp
= ifp
->info
;
289 struct nhrp_multicast
*mcast
;
291 frr_each (nhrp_mcastlist
, &nifp
->afi
[afi
].mcastlist_head
, mcast
) {