[2/8] rmnet: Add logic to dump out RMNet interfaces

Commit Message

Denis Kenzior Nov. 15, 2024, 10 p.m. UTC

This logic is triggered at startup to dump RMNet only links.  This is
accomplished by using RTM_GETLINK dump with a filter based on
IFLA_LINKINFO attribute setup for IFLA_INFO_KIND being "rmnet".

For now, links are simply printed and not acted upon.
---
 src/rmnet.c | 162 ++++++++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 162 insertions(+)

Patch

diff --git a/src/rmnet.c b/src/rmnet.c
index 58f841a49a59..52f47a03b27c 100644
--- a/src/rmnet.c
+++ b/src/rmnet.c
@@ -14,13 +14,19 @@ 
 #include <unistd.h>
 #include <linux/rtnetlink.h>
 #include <net/if.h>
+#include <linux/if_link.h>
+#include <linux/if_arp.h>
 
 #include <ell/ell.h>
 
 #include "ofono.h"
 #include "rmnet.h"
 
+#define RMNET_TYPE "rmnet"
+#define MAX_MUX_IDS 254U
+
 static struct l_netlink *rtnl;
+static uint32_t dump_id;
 
 int rmnet_get_interfaces(uint32_t parent_ifindex, unsigned int n_interfaces,
 				rmnet_new_interfaces_func_t cb,
@@ -40,15 +46,171 @@  int rmnet_cancel(int id)
 	return -ENOTSUP;
 }
 
+static int rmnet_parse_info_data(struct l_netlink_attr *linkinfo,
+					uint16_t *out_mux_id)
+{
+	struct l_netlink_attr info_data;
+	bool have_info_data = false;
+	uint16_t rta_type;
+	uint16_t rta_len;
+	const void *rta_data;
+	uint16_t mux_id;
+
+	while (!l_netlink_attr_next(linkinfo, &rta_type, &rta_len, &rta_data)) {
+		switch (rta_type) {
+		case IFLA_INFO_KIND:
+			if (strncmp(rta_data, RMNET_TYPE, rta_len))
+				return -EPROTOTYPE;
+
+			break;
+		case IFLA_INFO_DATA:
+			if (l_netlink_attr_recurse(linkinfo, &info_data) < 0)
+				return -EBADMSG;
+
+			have_info_data = true;
+			break;
+		}
+	}
+
+	if (!have_info_data)
+		return -ENOENT;
+
+	while (!l_netlink_attr_next(&info_data,
+					&rta_type, &rta_len, &rta_data)) {
+		if (rta_type != IFLA_RMNET_MUX_ID)
+			continue;
+
+		if (rta_len != sizeof(uint16_t))
+			return -EBADMSG;
+
+		mux_id = l_get_u16(rta_data);
+		if (mux_id > MAX_MUX_IDS || !mux_id)
+			return -ERANGE;
+
+		if (out_mux_id)
+			*out_mux_id = mux_id;
+
+		return 0;
+	}
+
+	return -ENOENT;
+}
+
+static int rmnet_parse_link(const void *data, uint32_t len,
+				char *out_ifname,
+				uint32_t *out_ifindex,
+				uint16_t *out_mux_id)
+{
+	const struct ifaddrmsg *ifa = data;
+	struct l_netlink_attr attr;
+	struct l_netlink_attr linkinfo;
+	bool have_linkinfo = false;
+	const char *ifname = NULL;
+	uint16_t ifnamelen = 0;
+	uint16_t rta_type;
+	uint16_t rta_len;
+	const void *rta_data;
+	int r;
+
+	if (l_netlink_attr_init(&attr, sizeof(struct ifinfomsg),
+							data, len) < 0)
+		return -EBADMSG;
+
+	while (!l_netlink_attr_next(&attr, &rta_type, &rta_len, &rta_data)) {
+		switch (rta_type) {
+		case IFLA_IFNAME:
+			ifname = rta_data;
+			ifnamelen = rta_len;
+			break;
+		case IFLA_LINKINFO:
+			if (l_netlink_attr_recurse(&attr, &linkinfo) < 0)
+				return -EBADMSG;
+
+			have_linkinfo = true;
+			break;
+		}
+	}
+
+	if (!have_linkinfo || !ifname || !ifnamelen)
+		return -ENOENT;
+
+	r = rmnet_parse_info_data(&linkinfo, out_mux_id);
+	if (r < 0)
+		return r;
+
+	if (out_ifname)
+		l_strlcpy(out_ifname, ifname, MIN(ifnamelen + 1, IF_NAMESIZE));
+
+	if (out_ifindex)
+		*out_ifindex = ifa->ifa_index;
+
+	return 0;
+}
+
+static void rmnet_link_dump_destroy(void *user_data)
+{
+	dump_id = 0;
+}
+
+static void rmnet_link_dump_cb(int error,
+				uint16_t type, const void *data,
+				uint32_t len, void *user_data)
+{
+	struct rmnet_ifinfo info;
+
+	/* Check conditions that can't happen on a dump */
+	if (error || type != RTM_NEWLINK)
+		return;
+
+	if (rmnet_parse_link(data, len,
+				info.ifname, &info.ifindex, &info.mux_id) < 0)
+		return;
+
+	DBG("Existing rmnet link: %s(%u) mux_id: %u",
+			info.ifname, info.ifindex, info.mux_id);
+}
+
+static int rmnet_link_dump()
+{
+	struct ifinfomsg ifi;
+	struct l_netlink_message *nlm =
+		l_netlink_message_new_sized(RTM_GETLINK, NLM_F_DUMP,
+								sizeof(ifi));
+
+	memset(&ifi, 0, sizeof(ifi));
+	l_netlink_message_add_header(nlm, &ifi, sizeof(ifi));
+
+	l_netlink_message_enter_nested(nlm, IFLA_LINKINFO);
+	l_netlink_message_append_string(nlm, IFLA_INFO_KIND, RMNET_TYPE);
+	l_netlink_message_leave_nested(nlm);
+
+	dump_id = l_netlink_send(rtnl, nlm, rmnet_link_dump_cb, NULL,
+				rmnet_link_dump_destroy);
+	if (dump_id > 0)
+		return 0;
+
+	l_netlink_message_unref(nlm);
+	return -EIO;
+}
+
 static int rmnet_init(void)
 {
+	int r;
+
 	DBG("");
 
 	rtnl = l_netlink_new(NETLINK_ROUTE);
 	if (!rtnl)
 		return -EIO;
 
+	r = rmnet_link_dump();
+	if (r < 0)
+		goto dump_failed;
+
 	return 0;
+dump_failed:
+	l_netlink_destroy(rtnl);
+	return r;
 }
 
 static void rmnet_exit(void)