netlink.c

/*
 * netlink.c - netlink interface.
 *
 * Copyright (C) 2023 IOPSYS Software Solutions AB. All rights reserved.
 *
 * See LICENSE file for license related information.
 *
 */

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <errno.h>
#include <net/if.h>
#include <sys/ioctl.h>

#include <linux/netlink.h>
#include <linux/rtnetlink.h>
#include <linux/if.h>

#include <netlink/netlink.h>
#include <netlink/utils.h>

#include <netlink/route/rtnl.h>
#include <netlink/route/neighbour.h>
#include <netlink/route/addr.h>
#include <netlink/route/link.h>
#include <netlink/genl/ctrl.h>
#include <netlink/genl/genl.h>
#include <netlink/attr.h>

#include <json-c/json.h>
#include <libubox/blobmsg.h>
#include <libubox/blobmsg_json.h>
#include <libubox/uloop.h>
#include <libubox/ustream.h>
#include <libubox/utils.h>
#include <libubus.h>


#include <easy/easy.h>

#include "debug.h"
#include "util.h"
#include "timer.h"
#include "neigh.h"
#include "hostmngr.h"

struct hostmngr_nlevent {
	struct uloop_fd uloop;
	void (*error_cb)(struct hostmngr_nlevent *e, int error);
	void (*event_cb)(struct hostmngr_nlevent *e);
};

struct event_socket {
	struct hostmngr_nlevent ev;
	struct nl_sock *sock;
	int sock_bufsize;
};

static int hostmngr_nlevents_cb(struct nl_msg *msg, void *arg);

static void handle_error(struct hostmngr_nlevent *e, int error)
{
	struct event_socket *ev_sock = container_of(e, struct event_socket, ev);

	if (error != ENOBUFS)
		goto err;

	ev_sock->sock_bufsize *= 2;
	if (nl_socket_set_buffer_size(ev_sock->sock, ev_sock->sock_bufsize, 0))
		goto err;

	return;

err:
	e->uloop.cb = NULL;
	uloop_fd_delete(&e->uloop);
}

static void recv_nlevents(struct hostmngr_nlevent *e)
{
	struct event_socket *ev_sock = container_of(e, struct event_socket, ev);

	nl_recvmsgs_default(ev_sock->sock);
}

static struct event_socket rtnl_event = {
	.ev = {
		.uloop = {.fd = - 1, },
		.error_cb = handle_error,
		.event_cb = recv_nlevents,
	},
	.sock = NULL,
	.sock_bufsize = 0x20000,
};

struct br_fdb_entry {
	uint8_t macaddr[6];
	uint16_t port;
};

/* defined in linux/if_bridge.h */
struct __fdb_entry {
        __u8 mac_addr[6];
        __u8 port_no;
        __u8 is_local;
        __u32 ageing_timer_value;
        __u8 port_hi;
        __u8 pad0;
        __u16 unused;
};

int hostmngr_update_neigh_brport(struct hostmngr_private *priv, char *brname)
{
	struct br_fdb_entry fdbs[512];
	struct __fdb_entry fdb[256];
	char path[512] = {0};
	long offset = 0;
	int i, n;
	int num = 0;
	FILE *f;


	snprintf(path, 512, "/sys/class/net/%s/brforward", brname);
	f = fopen(path, "r");
	if (!f)
		return -1;

	do {
		memset(fdb, 0, sizeof(fdb));
		fseek(f, offset * sizeof(struct __fdb_entry), SEEK_SET);
		n = fread(fdb, sizeof(struct __fdb_entry), 256, f);
		if (n <= 0)
			break;

		//TODO: extend when more than 256 entries
		if (num > 255)
			break;

		for (i = 0; i < n; i++) {
			if (fdb[i].is_local == 1)
				continue;

			memcpy(fdbs[num].macaddr, fdb[i].mac_addr, 6);
			fdbs[num].port = fdb[i].port_no;
			num++;
		}
		offset += n;
	} while (n > 0);

	fclose(f);

	for (i = 0; i < num; i++) {
		struct neigh_entry *t;
		char *ifname = NULL;

		dbg("FDB[%d] : " MACFMT "  port = %hu\n", i,
		    MAC2STR(fdbs[i].macaddr), fdbs[i].port);

		t = neigh_lookup(&priv->neigh_q, fdbs[i].macaddr);
		ifname = hostmngr_brport_to_ifname(priv, fdbs[i].port);
		if (t) {
			t->brport = fdbs[i].port;

			if (ifname) {
				memset(t->ifname, 0, 16);
				strncpy(t->ifname, ifname, 16);
			}
		}
		else {
			/* create a new neigh just learnt from br-fdb */
			struct neigh_entry *new = NULL;
			enum if_mediatype mtype;
			if (ifname) {
				if_getmediatype(ifname, &mtype);
				if (mtype == IF_MEDIA_ETH) {
					new = neigh_enqueue(&priv->neigh_q,
							fdbs[i].macaddr,
							NEIGH_STATE_REACHABLE,
							ifname,
							NEIGH_TYPE_ETH,
							NULL,
							NEIGH_AGEOUT_DEFAULT,
							NULL);
					if (new) {
						new->brport = fdbs[i].port;
						dbg("%s: Added new neighbor: " MACFMT " on port %hu\n", __func__, MAC2STR(fdbs[i].macaddr), fdbs[i].port);
					}
				}
			}
		}
	}

	return 0;
}


static int hostmngr_handle_neigh_tbl_change(struct hostmngr_private *priv, bool add,
					 char *ifname, uint8_t *macaddr,
					 struct ip_address *ip,
					 uint16_t state)
{
	struct neigh_history_entry *eh;
	struct neigh_entry *new = NULL;


	if (hwaddr_is_zero(macaddr))
		return 0;

	if (!add) {
		dbg("%s: TODO? DEL-NEIGH\n", __func__);
		return 0;
	}

	eh = neigh_history_lookup(&priv->neigh_q, macaddr);
	if (eh && eh->type == NEIGH_TYPE_WIFI && eh->is1905 == false) {
		struct neigh_entry *e;

		e = neigh_lookup(&priv->neigh_q, macaddr);
		if (!e) {
			dbg("Skip enqueue WiFi neigh through neigh table change\n");
			return 0;
		}

		if (e->event_pending && !ipaddr_is_zero(&e->ipv4)) {
			e->event_pending = 0;
			hostmngr_host_event(priv, HOST_EVENT_CONNECT, e);
		}
	}

	new = neigh_enqueue(&priv->neigh_q, macaddr, state, ifname,
			    NEIGH_TYPE_UNKNOWN, ip, NEIGH_AGEOUT_DEFAULT, NULL);

	if (new) {
		/* new neigh added */
		hostmngr_get_neigh_hostname(&priv->neigh_q, macaddr);

		/* add/update history cache for this neigh */
		neigh_history_enqueue(priv, new, priv->cfg.history_ageout);

		hostmngr_host_event(priv, HOST_EVENT_CONNECT, new);
	}


#if 0	//def NEIGH_DEBUG
	if (priv->neigh_q.pending_cnt > 0) {
		neigh_queue_print(&priv->neigh_q);
	}
#endif
	return 0;
}

static int hostmngr_handle_nlevents_neigh(struct hostmngr_private *priv,
				       struct nlmsghdr *hdr, bool add)
{
	struct ndmsg *ndm = nlmsg_data(hdr);
	struct local_interface *iface;
	struct nlattr *nla[__NDA_MAX];
	uint8_t macaddr[6] = {0};
	struct ip_address ip;
	char ipbuf[256] = {0};
	char ifname[16] = {0};
	char state[128] = {0};


	if (!nlmsg_valid_hdr(hdr, sizeof(*ndm)))
		return NL_SKIP;

	nlmsg_parse(hdr, sizeof(*ndm), nla, __NDA_MAX - 1, NULL);
	if (!nla[NDA_DST])
		return NL_SKIP;


	nla_memcpy(&ip.addr, nla[NDA_DST], sizeof(ip.addr));
	nla_memcpy(macaddr, nla[NDA_LLADDR], sizeof(macaddr));
	if (hwaddr_is_zero(macaddr))
		return NL_SKIP;

	ip.family = ndm->ndm_family;
	if (IN6_IS_ADDR_LINKLOCAL(&ip.addr) || IN6_IS_ADDR_MULTICAST(&ip.addr))
		return NL_SKIP;

	if (ndm->ndm_family == AF_INET || ndm->ndm_family == AF_INET6)
		inet_ntop(ip.family, &ip.addr, ipbuf, sizeof(ipbuf));

	if_indextoname(ndm->ndm_ifindex, ifname);

	/* ignore events for interfaces that we don't care */
	iface = hostmngr_ifname_to_interface(priv, ifname);
	if (!iface || iface->exclude)
		return NL_SKIP;

	dbg("%s: [ %s ] Neigh " MACFMT " ip = %s on %s, state = %s\n",
	     __func__, add ? "NEW" : "DEL", MAC2STR(macaddr), ipbuf, ifname,
		rtnl_neigh_state2str(ndm->ndm_state, state, sizeof(state)));


	hostmngr_handle_neigh_tbl_change(priv, add, ifname, macaddr, &ip,
				      ndm->ndm_state);

	return NL_OK;
}

static int hostmngr_handle_nlevents_link(struct hostmngr_private *priv,
				      struct nlmsghdr *hdr, bool add)
{
	struct ifinfomsg *ifi = nlmsg_data(hdr);
	struct nlattr *nla[__IFLA_MAX];
	struct local_interface *iface;
	uint8_t macaddr[6] = {0};
	char flagstr[256] = {0};
	char ifname[16] = {0};
	int operstate_xchg = 0;	/* 0 = no change, 1 = down->up, -1 = up->down */
	int carrier_xchg = 0; /* 0 = no change, 1 = off->on, -1 = on->off */
	int operstate = -1;
	bool addif = false;
	bool delif = false;
	int carrier = -1;
	int master = -1;
	int br_ifindex;


	trace("%s: ------------->\n", __func__);

	if (!nlmsg_valid_hdr(hdr, sizeof(*ifi)))
		return NL_SKIP;

	nlmsg_parse(hdr, sizeof(*ifi), nla, __IFLA_MAX - 1, NULL);
	if (!nla[IFLA_IFNAME])
		return NL_SKIP;

	nla_memcpy(ifname, nla[IFLA_IFNAME], 15);
	iface = hostmngr_ifname_to_interface(priv, ifname);
	if (!iface) {
		iface = hostmngr_alloc_interface(ifname);
		if (iface) {
			list_add_tail(&iface->list, &priv->iflist);
			priv->num_local_interfaces++;
		} else
			return NL_SKIP;	/* not interested */
	}

	nla_memcpy(macaddr, nla[IFLA_ADDRESS], sizeof(macaddr));
	iface->ifflags = ifi->ifi_flags;
	rtnl_link_flags2str(ifi->ifi_flags, flagstr, sizeof(flagstr));

	if (nla[IFLA_OPERSTATE])
		operstate = nla_get_u8(nla[IFLA_OPERSTATE]);

	if (nla[IFLA_CARRIER])
		carrier = nla_get_u8(nla[IFLA_CARRIER]);

	if (nla[IFLA_MASTER])
		master = nla_get_u32(nla[IFLA_MASTER]);

	if (iface->is_brif) {
		if (!add && master > 0 && master == iface->br_ifindex) {
			iface->nomaster = 1;
			delif = true;
			iface->brport = 0;
			iface->is_brif = 0;
		} else if (add && master == iface->br_ifindex && iface->nomaster) {
			iface->nomaster = 0;
			addif = true;
		}
	}

	if (operstate != -1) {
		if (iface->operstate == IF_OPER_DOWN && operstate == IF_OPER_UP)
			operstate_xchg = 1;
		else if (iface->operstate == IF_OPER_UP && operstate == IF_OPER_DOWN)
			operstate_xchg = -1;

		iface->operstate = operstate;
	}

	if (carrier != -1) {
		if (iface->carrier == 0 && carrier == 1)
			carrier_xchg = 1;
		else if (iface->carrier == 1 && carrier == 0)
			carrier_xchg = -1;

		iface->carrier = carrier;
	}


	dbg("%s: %s (%d) family = %d, flags = 0x%x %s, opstate = %d, carrier = %d, master (%d) --->\n",
	    add ? "NEWLINK" : "DELLINK", ifname, ifi->ifi_index,
	    ifi->ifi_family, ifi->ifi_flags, flagstr, operstate, carrier, master);

	if (if_isbridge(ifname)) {
		//TODO
		return NL_SKIP;
	}

	/* unplug: operstate = IF_OPER_DOWN && carrier = 0
	 * plug: operstate = IF_OPER_UP && carrier = 1
	 *
	 * delif: operstate = IF_OPER_UP && carrier = 1 && nomaster
	 * addif: operstate = IF_OPER_UP && carrier = 1 && master
	 *
	 * ifdown: operstate = IF_OPER_DOWN
	 * ifup: operstate = IF_OPER_UP
	 */

        br_ifindex = if_isbridge_interface(ifname);
        if (br_ifindex > 0) {
                iface->is_brif = true;
                iface->brport = if_brportnum(iface->ifname);
                iface->br_ifindex = br_ifindex;
        }

	if (operstate_xchg == -1 || carrier_xchg == -1 || delif) {
		dbg("%s: %s either went down, link-lost or removed from bridge."
		    "Set hosts unreachable through it\n", __func__, ifname);

		neigh_set_unreachable(&priv->neigh_q, ifname);
	} else if (operstate_xchg == 1 || carrier_xchg == 1 || addif) {
		dbg("%s: %s is up or added to bridge\n", __func__, ifname);


		neigh_probe_unreachable(&priv->neigh_q, ifname);
	}

	return NL_OK;
}

static int hostmngr_nlevents_cb(struct nl_msg *msg, void *arg)
{
	struct nlmsghdr *hdr = nlmsg_hdr(msg);
	struct hostmngr_private *priv = arg;
	int ret = NL_SKIP;
	bool add = false;


	switch (hdr->nlmsg_type) {
	case RTM_NEWLINK:
		add = true;
	case RTM_DELLINK:
		ret = hostmngr_handle_nlevents_link(priv, hdr, add);
		break;
#if 0	//TODO: when needed
	case RTM_NEWADDR:
		add = true;
	case RTM_DELADDR:
		ret = hostmngr_handle_nlevents_addr(priv, hdr, add);
		break;
#endif
	case RTM_NEWNEIGH:
		add = true;
	case RTM_DELNEIGH:
		ret = hostmngr_handle_nlevents_neigh(priv, hdr, add);
		break;

	default:
		break;
	}

	return ret;
}


static void hostmngr_receive_nlevents(struct uloop_fd *u, unsigned int events)
{
	struct hostmngr_nlevent *e = container_of(u, struct hostmngr_nlevent, uloop);

	if (u->error) {
		int ret = -1;
		socklen_t ret_len = sizeof(ret);

		u->error = false;
		if (e->error_cb &&
		    getsockopt(u->fd, SOL_SOCKET, SO_ERROR, &ret, &ret_len) == 0) {
			e->error_cb(e, ret);
		}
	}

	if (e->event_cb) {
		e->event_cb(e);
		return;
	}
}

int hostmngr_register_nlevents(struct hostmngr_private *priv)
{
	struct nl_sock *sk;


	sk = nl_socket_alloc();
	if (!sk) {
		err("Unable to open nl event socket: %m");
		return -1;
	}

	if (nl_connect(sk, NETLINK_ROUTE) < 0) {
		nl_socket_free(sk);
		return -1;
	}

	rtnl_event.sock = sk;

	if (nl_socket_set_buffer_size(rtnl_event.sock, rtnl_event.sock_bufsize, 0)) {
		err("%s: %d\n", __func__, __LINE__);
		goto out_err;
	}

	nl_socket_disable_seq_check(rtnl_event.sock);

	nl_socket_modify_cb(rtnl_event.sock, NL_CB_VALID, NL_CB_CUSTOM,
			    hostmngr_nlevents_cb, priv);

	if (nl_socket_add_memberships(rtnl_event.sock,
				      RTNLGRP_NEIGH, RTNLGRP_LINK, 0))
		goto out_err;

	rtnl_event.ev.uloop.fd = nl_socket_get_fd(rtnl_event.sock);
	rtnl_event.ev.uloop.cb = hostmngr_receive_nlevents;
	uloop_fd_add(&rtnl_event.ev.uloop, ULOOP_READ |
		     ((rtnl_event.ev.error_cb) ? ULOOP_ERROR_CB : 0));

	return 0;

out_err:
	if (rtnl_event.sock) {
		nl_socket_free(rtnl_event.sock);
		rtnl_event.sock = NULL;
		rtnl_event.ev.uloop.fd = -1;
	}

	return -1;
}

void hostmngr_unregister_nlevents(struct hostmngr_private *priv)
{
	UNUSED(priv);

	if (rtnl_event.sock) {
		uloop_fd_delete(&rtnl_event.ev.uloop);
		rtnl_event.ev.uloop.fd = -1;
		nl_socket_free(rtnl_event.sock);
		rtnl_event.sock = NULL;
	}
}

int hostmngr_get_known_neighbors(struct hostmngr_private *priv, char *ifname)
{
	struct rtnl_neigh *neigh;
	struct nl_object *nobj;
	struct nl_cache *cache;
	uint32_t ifindex = 0;
	struct nl_sock *sk;
	int i, num;
	int ret;



	ifindex = if_nametoindex(ifname);
	if (!ifindex)
		return -1;

	sk = nl_socket_alloc();
	if (!sk) {
		err("Unable to open nl event socket\n");
		return -1;
	}

	if (nl_connect(sk, NETLINK_ROUTE) < 0) {
		nl_socket_free(sk);
		return -1;
	}

	ret = rtnl_neigh_alloc_cache(sk, &cache);
	if (ret) {
		nl_socket_free(sk);
		return -1;
	}

	num = nl_cache_nitems(cache);
	nobj = nl_cache_get_first(cache);
	neigh = (struct rtnl_neigh *)nobj;

	for (i = 0; i < num; i++) {
		if (rtnl_neigh_get_ifindex(neigh) == ifindex) {
			struct nl_addr *lladdr;
			struct nl_addr *ipaddr;
			struct ip_address ip = {0};
			uint8_t hwaddr[6] = {0};
			uint16_t state;
			enum neigh_type type = NEIGH_TYPE_UNKNOWN;
			struct neigh_entry *new = NULL;

			nl_object_get((struct nl_object *)neigh);

			state = rtnl_neigh_get_state(neigh);
			lladdr = rtnl_neigh_get_lladdr(neigh);
			if (lladdr)
				memcpy(hwaddr, nl_addr_get_binary_addr(lladdr),
					nl_addr_get_len(lladdr));

			if (hwaddr_is_zero(hwaddr) || hwaddr_is_mcast(hwaddr)) {
				nl_object_put((struct nl_object *) neigh);
				nobj = nl_cache_get_next(nobj);
				neigh = (struct rtnl_neigh *)nobj;
				continue;
			}

			ipaddr = rtnl_neigh_get_dst(neigh);
			if (ipaddr) {
				ip.family = nl_addr_get_family(ipaddr);
				if (ip.family == AF_INET6 || ip.family == AF_INET) {
					memcpy(&ip.addr, nl_addr_get_binary_addr(ipaddr),
					       nl_addr_get_len(ipaddr));
				}

				/* ignore states for ipv6 entries */
				if (ip.family == AF_INET6) {
					nl_object_put((struct nl_object *)neigh);
					nobj = nl_cache_get_next(nobj);
					neigh = (struct rtnl_neigh *)nobj;
					continue;
				}
			}

			if (neigh_is_wifi_type(priv, hwaddr)) {
				type = NEIGH_TYPE_WIFI;
			} else {
				struct neigh_history_entry *eh;

				eh = neigh_history_lookup(&priv->neigh_q, hwaddr);
				if (eh && eh->type == NEIGH_TYPE_WIFI) {
					dbg("Skip enqueue WiFi neigh not in assoclist\n");
					nl_object_put((struct nl_object *)neigh);
					nobj = nl_cache_get_next(nobj);
					neigh = (struct rtnl_neigh *)nobj;
					continue;
				}
			}

			new = neigh_enqueue(&priv->neigh_q, hwaddr, state,
					    ifname, type, &ip,
					    NEIGH_AGEOUT_DEFAULT, NULL);
			if (new) {
				/* new neigh added */
				ret = hostmngr_get_neigh_hostname(&priv->neigh_q, hwaddr);  //TODO: cond

				/* add/update history cache for this neigh */
				neigh_history_enqueue(priv, new, priv->cfg.history_ageout);
			}

			nl_object_put((struct nl_object *)neigh);
		}

		nobj = nl_cache_get_next(nobj);
		neigh = (struct rtnl_neigh *)nobj;
	}

	nl_cache_free(cache);
	nl_socket_free(sk);

	if (if_isbridge(ifname)) {
		/* bridge port_nos on which the hosts are last seen */
		hostmngr_update_neigh_brport(priv, ifname);
	}

#if 0	//def NEIGH_DEBUG
	if (priv->neigh_q.pending_cnt > 0) {
		neigh_queue_print(&priv->neigh_q);
	}
#endif

	return 0;
}