[mirror_frr.git] / nhrpd / nhrp_multicast.c

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