zebra: Add fpm_listener

Add a tiny fpm_listener program to allow FRR to test the
fpm.

Signed-off-by: Donald Sharp <sharpd@nvidia.com>

7 files changed, 652 insertions, 0 deletions

diff --git a/configure.ac b/configure.ac
index c73131751f..ba11e8996e 100644
--- a/configure.ac
+++ b/configure.ac
@@ -703,6 +703,8 @@ AC_ARG_ENABLE([mgmtd_local_validations],
   AS_HELP_STRING([--enable-mgmtd-local-validations], [dev: unimplemented local validation]))
 AC_ARG_ENABLE([mgmtd_test_be_client],
   AS_HELP_STRING([--enable-mgmtd-test-be-client], [build test backend client]))
+AC_ARG_ENABLE([fpm_listener],
+  AS_HELP_STRING([--enable-fpm-listener], [build fpm listener test program]))
 AC_ARG_ENABLE([ripd],
   AS_HELP_STRING([--disable-ripd], [do not build ripd]))
 AC_ARG_ENABLE([ripngd],
@@ -1811,6 +1813,10 @@ AS_IF([test "$enable_mgmtd_test_be_client" = "yes"], [
   AC_DEFINE([HAVE_MGMTD_TESTC], [1], [mgmtd_testc])
 ])
 
+AS_IF([test "$enable_fpm_listener" = "yes"], [
+  AC_DEFINE([HAVE_FPM_LISTENER], [1], [fpm_listener])
+])
+
 AS_IF([test "$enable_ripd" != "no"], [
   AC_DEFINE([HAVE_RIPD], [1], [ripd])
 ])
@@ -2773,6 +2779,7 @@ AM_CONDITIONAL([ZEBRA], [test "$enable_zebra" != "no"])
 AM_CONDITIONAL([BGPD], [test "$enable_bgpd" != "no"])
 AM_CONDITIONAL([MGMTD], [test "$enable_mgmtd" != "no"])
 AM_CONDITIONAL([MGMTD_TESTC], [test "$enable_mgmtd_test_be_client" = "yes"])
+AM_CONDITIONAL([FPM_LISTENER], [test "enable_fpm_listener" = "yes"])
 AM_CONDITIONAL([RIPD], [test "$enable_ripd" != "no"])
 AM_CONDITIONAL([OSPFD], [test "$enable_ospfd" != "no"])
 AM_CONDITIONAL([LDPD], [test "$enable_ldpd" != "no"])
diff --git a/debian/not-installed b/debian/not-installed
index 8999dd948b..c7766ac257 100644
--- a/debian/not-installed
+++ b/debian/not-installed
@@ -1,4 +1,5 @@
 usr/include
 usr/lib/frr/ospfclient
+usr/lib/frr/fpm_listener
 usr/lib/frr/rfptest
 usr/lib/*/frr/modules/dplane_sample_plugin.so
diff --git a/doc/user/installation.rst b/doc/user/installation.rst
index 70f82353b7..f07bade52c 100644
--- a/doc/user/installation.rst
+++ b/doc/user/installation.rst
@@ -274,6 +274,10 @@ options from the list below.
 
    Build with FPM module support.
 
+.. option:: --enable-fpm-listener
+
+   Build a small fpm listener for testing.
+
 .. option:: --with-service-timeout=X
 
    Set timeout value for FRR service. The time of restarting or reloading FRR
diff --git a/redhat/frr.spec.in b/redhat/frr.spec.in
index 3371a3ed4d..433aeacebb 100644
--- a/redhat/frr.spec.in
+++ b/redhat/frr.spec.in
@@ -677,6 +677,7 @@ fi
     %{_sbindir}/mgmtd_testc
 %endif
 %exclude %{_sbindir}/ssd
+%exclude %{_sbindir}/fpm_listener
 %if %{with_watchfrr}
     %{_sbindir}/watchfrr
 %endif
diff --git a/zebra/.gitignore b/zebra/.gitignore
index 41a86e7d75..f10240db43 100644
--- a/zebra/.gitignore
+++ b/zebra/.gitignore
@@ -1,3 +1,4 @@
 zebra
 zebra.conf
 client
+fpm_listener
diff --git a/zebra/fpm_listener.c b/zebra/fpm_listener.c
new file mode 100644
index 0000000000..d50e40e9d8
--- /dev/null
+++ b/zebra/fpm_listener.c
@@ -0,0 +1,632 @@
+// SPDX-License-Identifier: ISC
+/*
+ * Copyright (C) 2012  Internet Systems Consortium, Inc. ("ISC")
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND ISC DISCLAIMS ALL WARRANTIES WITH
+ * REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY
+ * AND FITNESS.  IN NO EVENT SHALL ISC BE LIABLE FOR ANY SPECIAL, DIRECT,
+ * INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM
+ * LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE
+ * OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR
+ * PERFORMANCE OF THIS SOFTWARE.
+ */
+#include "config.h"
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+
+#ifdef GNU_LINUX
+#include <stdint.h>
+#include <memory.h>
+#include <sys/socket.h>
+#include <arpa/inet.h>
+#include <errno.h>
+#include <assert.h>
+#include <err.h>
+#include <sys/types.h>
+
+#include <linux/netlink.h>
+#include <linux/rtnetlink.h>
+
+#include "fpm/fpm.h"
+#include "lib/libfrr.h"
+
+struct glob {
+	int server_sock;
+	int sock;
+};
+
+struct glob glob_space;
+struct glob *glob = &glob_space;
+
+#define ARRAY_SIZE(x) (sizeof(x) / sizeof(x[0]))
+
+/*
+ * get_print_buf
+ */
+static char *
+get_print_buf(size_t *buf_len)
+{
+	static char print_bufs[16][128];
+	static int counter;
+
+	counter++;
+	if (counter >= 16)
+		counter = 0;
+
+	*buf_len = 128;
+	return &print_bufs[counter][0];
+}
+
+/*
+ * create_listen_sock
+ */
+static int create_listen_sock(int port, int *sock_p)
+{
+	int sock;
+	struct sockaddr_in addr;
+	int reuse;
+
+	sock = socket(PF_INET, SOCK_STREAM, IPPROTO_TCP);
+	if (sock < 0) {
+		fprintf(stderr, "Failed to create socket: %s\n", strerror(errno));
+		return 0;
+	}
+
+	reuse = 1;
+	if (setsockopt(sock, SOL_SOCKET, SO_REUSEADDR, &reuse, sizeof(reuse)) <
+	    0) {
+		fprintf(stderr, "Failed to set reuse addr option: %s\n",
+			 strerror(errno));
+	}
+
+	memset(&addr, 0, sizeof(addr));
+	addr.sin_family = AF_INET;
+	addr.sin_addr.s_addr = htonl(INADDR_ANY);
+	addr.sin_port = htons(port);
+
+	if (bind(sock, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
+		fprintf(stderr, "Failed to bind to port %d: %s\n", port, strerror(errno));
+		close(sock);
+		return 0;
+	}
+
+	if (listen(sock, 5)) {
+		fprintf(stderr, "Failed to listen on socket: %s\n", strerror(errno));
+		close(sock);
+		return 0;
+	}
+
+	*sock_p = sock;
+	return 1;
+}
+
+/*
+ * accept_conn
+ */
+static int accept_conn(int listen_sock)
+{
+	int sock;
+	struct sockaddr_in client_addr = { 0 };
+	unsigned int client_len;
+
+	while (1) {
+		char buf[120];
+
+		fprintf(stdout, "Waiting for client connection...\n");
+		client_len = sizeof(client_addr);
+		sock = accept(listen_sock, (struct sockaddr *)&client_addr,
+			      &client_len);
+
+		if (sock >= 0) {
+			fprintf(stdout, "Accepted client %s\n",
+				inet_ntop(AF_INET, &client_addr.sin_addr, buf, sizeof(buf)));
+			return sock;
+		}
+		fprintf(stderr, "Failed to accept socket: %s\n", strerror(errno));
+	}
+}
+
+/*
+ * read_fpm_msg
+ */
+static fpm_msg_hdr_t *
+read_fpm_msg(char *buf, size_t buf_len)
+{
+	char *cur, *end;
+	long need_len, bytes_read, have_len;
+	fpm_msg_hdr_t *hdr;
+	int reading_full_msg;
+
+	end = buf + buf_len;
+	cur = buf;
+	hdr = (fpm_msg_hdr_t *)buf;
+
+	while (1) {
+		reading_full_msg = 0;
+
+		have_len = cur - buf;
+
+		if (have_len < (long)FPM_MSG_HDR_LEN) {
+			need_len = FPM_MSG_HDR_LEN - have_len;
+		} else {
+			need_len = fpm_msg_len(hdr) - have_len;
+			assert(need_len >= 0 && need_len <= (end - cur));
+
+			if (!need_len)
+				return hdr;
+
+			reading_full_msg = 1;
+		}
+
+		bytes_read = read(glob->sock, cur, need_len);
+
+		if (bytes_read == 0) {
+			fprintf(stdout,
+				"Socket closed as that read returned 0\n");
+			return NULL;
+		}
+
+		if (bytes_read < 0) {
+			fprintf(stderr, "Error reading from socket: %s\n",
+				strerror(errno));
+			return NULL;
+		}
+
+		cur += bytes_read;
+
+		if (bytes_read < need_len) {
+			fprintf(stderr,
+				"Read %lu bytes but expected to read %lu bytes instead\n",
+				bytes_read, need_len);
+			return NULL;
+		}
+
+		if (reading_full_msg)
+			return hdr;
+
+		if (!fpm_msg_ok(hdr, buf_len)) {
+			assert(0);
+			fprintf(stderr, "Malformed fpm message\n");
+			return NULL;
+		}
+	}
+}
+
+/*
+ * netlink_msg_type_to_s
+ */
+static const char *
+netlink_msg_type_to_s(uint16_t type)
+{
+	switch (type) {
+
+	case RTM_NEWROUTE:
+		return "New route";
+
+	case RTM_DELROUTE:
+		return "Del route";
+
+	default:
+		return "Unknown";
+	}
+}
+
+/*
+ * netlink_prot_to_s
+ */
+static const char *
+netlink_prot_to_s(unsigned char prot)
+{
+	switch (prot) {
+
+	case RTPROT_KERNEL:
+		return "Kernel";
+
+	case RTPROT_BOOT:
+		return "Boot";
+
+	case RTPROT_STATIC:
+		return "Static";
+
+	case RTPROT_ZEBRA:
+		return "Zebra";
+
+	case RTPROT_DHCP:
+		return "Dhcp";
+
+	default:
+		return "Unknown";
+	}
+}
+
+#define MAX_NHS 16
+
+struct netlink_nh {
+	struct rtattr *gateway;
+	int if_index;
+};
+
+struct netlink_msg_ctx {
+	struct nlmsghdr *hdr;
+
+	/*
+	 * Stuff pertaining to route messages.
+	 */
+	struct rtmsg *rtmsg;
+	struct rtattr *rtattrs[RTA_MAX + 1];
+
+	/*
+	 * Nexthops.
+	 */
+	struct netlink_nh nhs[MAX_NHS];
+	unsigned long num_nhs;
+
+	struct rtattr *dest;
+	struct rtattr *src;
+	int *metric;
+
+	const char *err_msg;
+};
+
+/*
+ * netlink_msg_ctx_init
+ */
+static inline void netlink_msg_ctx_init(struct netlink_msg_ctx *ctx)
+{
+	memset(ctx, 0, sizeof(*ctx));
+}
+
+/*
+ * netlink_msg_ctx_set_err
+ */
+static inline void netlink_msg_ctx_set_err(struct netlink_msg_ctx *ctx,
+					   const char *err_msg)
+{
+	if (ctx->err_msg)
+		return;
+
+	ctx->err_msg = err_msg;
+}
+
+/*
+ * parse_rtattrs_
+ */
+static int parse_rtattrs_(struct rtattr *rta, size_t len, struct rtattr **rtas,
+		   int num_rtas, const char **err_msg)
+{
+	memset(rtas, 0, num_rtas * sizeof(rtas[0]));
+
+	for (; len > 0; rta = RTA_NEXT(rta, len)) {
+		if (!RTA_OK(rta, len)) {
+			*err_msg = "Malformed rta";
+			return 0;
+		}
+
+		if (rta->rta_type >= num_rtas) {
+			warn("Unknown rtattr type %d", rta->rta_type);
+			continue;
+		}
+
+		rtas[rta->rta_type] = rta;
+	}
+
+	return 1;
+}
+
+/*
+ * parse_rtattrs
+ */
+static int parse_rtattrs(struct netlink_msg_ctx *ctx, struct rtattr *rta,
+			 size_t len)
+{
+	const char *err_msg;
+
+	err_msg = NULL;
+
+	if (!parse_rtattrs_(rta, len, ctx->rtattrs, ARRAY_SIZE(ctx->rtattrs),
+			    &err_msg)) {
+		netlink_msg_ctx_set_err(ctx, err_msg);
+		return 0;
+	}
+
+	return 1;
+}
+
+/*
+ * netlink_msg_ctx_add_nh
+ */
+static int netlink_msg_ctx_add_nh(struct netlink_msg_ctx *ctx, int if_index,
+				  struct rtattr *gateway)
+{
+	struct netlink_nh *nh;
+
+	if (ctx->num_nhs + 1 >= ARRAY_SIZE(ctx->nhs)) {
+		warn("Too many next hops");
+		return 0;
+	}
+	nh = &ctx->nhs[ctx->num_nhs];
+	ctx->num_nhs++;
+
+	nh->gateway = gateway;
+	nh->if_index = if_index;
+	return 1;
+}
+
+/*
+ * parse_multipath_attr
+ */
+static int parse_multipath_attr(struct netlink_msg_ctx *ctx,
+				struct rtattr *mpath_rtattr)
+{
+	size_t len;
+	struct rtnexthop *rtnh;
+	struct rtattr *rtattrs[RTA_MAX + 1];
+	struct rtattr *gateway;
+	const char *err_msg;
+
+	rtnh = RTA_DATA(mpath_rtattr);
+	len = RTA_PAYLOAD(mpath_rtattr);
+
+	for (; len > 0;
+	     len -= NLMSG_ALIGN(rtnh->rtnh_len), rtnh = RTNH_NEXT(rtnh)) {
+
+		if (!RTNH_OK(rtnh, len)) {
+			netlink_msg_ctx_set_err(ctx, "Malformed nh");
+			return 0;
+		}
+
+		if (rtnh->rtnh_len <= sizeof(*rtnh)) {
+			netlink_msg_ctx_set_err(ctx, "NH len too small");
+			return 0;
+		}
+
+		/*
+		 * Parse attributes included in the nexthop.
+		 */
+		err_msg = NULL;
+		if (!parse_rtattrs_(RTNH_DATA(rtnh),
+				    rtnh->rtnh_len - sizeof(*rtnh), rtattrs,
+				    ARRAY_SIZE(rtattrs), &err_msg)) {
+			netlink_msg_ctx_set_err(ctx, err_msg);
+			return 0;
+		}
+
+		gateway = rtattrs[RTA_GATEWAY];
+		netlink_msg_ctx_add_nh(ctx, rtnh->rtnh_ifindex, gateway);
+	}
+
+	return 1;
+}
+
+/*
+ * parse_route_msg
+ */
+static int parse_route_msg(struct netlink_msg_ctx *ctx)
+{
+	int len;
+	struct rtattr **rtattrs, *rtattr, *gateway, *oif;
+	int if_index;
+
+	ctx->rtmsg = NLMSG_DATA(ctx->hdr);
+
+	len = ctx->hdr->nlmsg_len - NLMSG_LENGTH(sizeof(struct rtmsg));
+	if (len < 0) {
+		netlink_msg_ctx_set_err(ctx, "Bad message length");
+		return 0;
+	}
+
+	if (!parse_rtattrs(ctx, RTM_RTA(ctx->rtmsg), len))
+		return 0;
+
+	rtattrs = ctx->rtattrs;
+
+	ctx->dest = rtattrs[RTA_DST];
+	ctx->src = rtattrs[RTA_PREFSRC];
+
+	rtattr = rtattrs[RTA_PRIORITY];
+	if (rtattr)
+		ctx->metric = (int *)RTA_DATA(rtattr);
+
+	gateway = rtattrs[RTA_GATEWAY];
+	oif = rtattrs[RTA_OIF];
+	if (gateway || oif) {
+		if_index = 0;
+		if (oif)
+			if_index = *((int *)RTA_DATA(oif));
+
+		netlink_msg_ctx_add_nh(ctx, if_index, gateway);
+	}
+
+	rtattr = rtattrs[RTA_MULTIPATH];
+	if (rtattr)
+		parse_multipath_attr(ctx, rtattr);
+
+	return 1;
+}
+
+/*
+ * addr_to_s
+ */
+static const char *
+addr_to_s(unsigned char family, void *addr)
+{
+	size_t buf_len;
+	char *buf;
+
+	buf = get_print_buf(&buf_len);
+
+	return inet_ntop(family, addr, buf, buf_len);
+}
+
+/*
+ * netlink_msg_ctx_print
+ */
+static int netlink_msg_ctx_snprint(struct netlink_msg_ctx *ctx, char *buf,
+				   size_t buf_len)
+{
+	struct nlmsghdr *hdr;
+	struct rtmsg *rtmsg;
+	struct netlink_nh *nh;
+	char *cur, *end;
+	unsigned long i;
+
+	hdr = ctx->hdr;
+	rtmsg = ctx->rtmsg;
+
+	cur = buf;
+	end = buf + buf_len;
+
+	cur += snprintf(cur, end - cur, "%s %s/%d, Prot: %s",
+			netlink_msg_type_to_s(hdr->nlmsg_type),
+			addr_to_s(rtmsg->rtm_family, RTA_DATA(ctx->dest)),
+			rtmsg->rtm_dst_len,
+			netlink_prot_to_s(rtmsg->rtm_protocol));
+
+	if (ctx->metric)
+		cur += snprintf(cur, end - cur, ", Metric: %d", *ctx->metric);
+
+	for (i = 0; i < ctx->num_nhs; i++) {
+		cur += snprintf(cur, end - cur, "\n ");
+		nh = &ctx->nhs[i];
+
+		if (nh->gateway) {
+			cur += snprintf(cur, end - cur, " %s",
+					addr_to_s(rtmsg->rtm_family,
+						  RTA_DATA(nh->gateway)));
+		}
+
+		if (nh->if_index) {
+			cur += snprintf(cur, end - cur, " via interface %d",
+					nh->if_index);
+		}
+	}
+
+	return cur - buf;
+}
+
+/*
+ * print_netlink_msg_ctx
+ */
+static void print_netlink_msg_ctx(struct netlink_msg_ctx *ctx)
+{
+	char buf[1024];
+
+	netlink_msg_ctx_snprint(ctx, buf, sizeof(buf));
+	printf("%s\n", buf);
+}
+
+/*
+ * parse_netlink_msg
+ */
+static void
+parse_netlink_msg(char *buf, size_t buf_len)
+{
+	struct netlink_msg_ctx ctx_space, *ctx;
+	struct nlmsghdr *hdr;
+	unsigned int len;
+
+	ctx = &ctx_space;
+
+	hdr = (struct nlmsghdr *)buf;
+	len = buf_len;
+	for (; NLMSG_OK(hdr, len); hdr = NLMSG_NEXT(hdr, len)) {
+
+		netlink_msg_ctx_init(ctx);
+		ctx->hdr = (struct nlmsghdr *)buf;
+
+		switch (hdr->nlmsg_type) {
+
+		case RTM_DELROUTE:
+		case RTM_NEWROUTE:
+
+			parse_route_msg(ctx);
+			if (ctx->err_msg) {
+				fprintf(stderr,
+					"Error parsing route message: %s\n",
+					ctx->err_msg);
+			}
+
+			print_netlink_msg_ctx(ctx);
+			break;
+
+		default:
+			fprintf(stdout, "Ignoring unknown netlink message - Type: %d\n",
+			      hdr->nlmsg_type);
+		}
+	}
+}
+
+/*
+ * process_fpm_msg
+ */
+static void process_fpm_msg(fpm_msg_hdr_t *hdr)
+{
+	fprintf(stdout, "FPM message - Type: %d, Length %d\n", hdr->msg_type,
+	      ntohs(hdr->msg_len));
+
+	if (hdr->msg_type != FPM_MSG_TYPE_NETLINK) {
+		fprintf(stderr, "Unknown fpm message type %u\n", hdr->msg_type);
+		return;
+	}
+
+	parse_netlink_msg(fpm_msg_data(hdr), fpm_msg_data_len(hdr));
+}
+
+/*
+ * fpm_serve
+ */
+static void fpm_serve(void)
+{
+	char buf[FPM_MAX_MSG_LEN * 4];
+	fpm_msg_hdr_t *hdr;
+
+	while (1) {
+
+		hdr = read_fpm_msg(buf, sizeof(buf));
+		if (!hdr)
+			return;
+
+		process_fpm_msg(hdr);
+	}
+}
+
+int main(int argc, char **argv)
+{
+	pid_t daemon;
+	int d;
+
+	d = getopt(argc, argv, "d");
+	if (d == 'd') {
+		daemon = fork();
+
+		if (daemon)
+			exit(0);
+	}
+
+	memset(glob, 0, sizeof(*glob));
+
+	if (!create_listen_sock(FPM_DEFAULT_PORT, &glob->server_sock))
+		exit(1);
+
+	/*
+	 * Server forever.
+	 */
+	while (1) {
+		glob->sock = accept_conn(glob->server_sock);
+		fpm_serve();
+		fprintf(stdout, "Done serving client");
+	}
+}
+#else
+
+int main(int argc, char **argv)
+{
+	fprintf(stderr, "This program only works on linux");
+	exit(-1);
+}
+#endif
diff --git a/zebra/subdir.am b/zebra/subdir.am
index d9c8d9045e..41088e2c4c 100644
--- a/zebra/subdir.am
+++ b/zebra/subdir.am
@@ -19,6 +19,12 @@ if LINUX
 module_LTLIBRARIES += zebra/zebra_cumulus_mlag.la
 endif
 
+#if FPM_LISTENER
+sbin_PROGRAMS += zebra/fpm_listener
+zebra_fpm_listener_SOURCES = zebra/fpm_listener.c
+zebra_fpm_listener_LDADD = lib/libfrr.la
+#endf
+
 # Dataplane sample plugin
 if DEV_BUILD
 module_LTLIBRARIES += zebra/dplane_sample_plugin.la