gisi: Add phonet netlink functions

Adding g_pn_netlink_set_address() and g_pn_netlink_add_route().
Automatically configure phonet links without external software.

1 files changed, 192 insertions, 9 deletions

diff --git a/gisi/netlink.c b/gisi/netlink.c
index 59322e36..598ff0e7 100644
--- a/gisi/netlink.c
+++ b/gisi/netlink.c
@@ -68,6 +68,7 @@
 #define RTA_NEXT(rta,attrlen)	((attrlen) -= RTA_ALIGN((rta)->rta_len), \
 	(struct rtattr*)(void*)(((char*)(rta)) + RTA_ALIGN((rta)->rta_len)))
 
+#define SIZE_NLMSG (16384)
 
 struct _GPhonetNetlink {
 	GPhonetNetlinkFunc callback;
@@ -112,6 +113,24 @@ error:
 	close(fd);
 }
 
+static int netlink_socket(void)
+{
+	int fd;
+	int bufsize = SIZE_NLMSG;
+
+	fd = socket(PF_NETLINK, SOCK_DGRAM, NETLINK_ROUTE);
+	if (fd == -1)
+		return -1;
+
+	if (setsockopt(fd, SOL_SOCKET, SO_RCVBUF, &bufsize, sizeof(bufsize))) {
+		int error = errno;
+		close(fd), fd = -1;
+		errno = error;
+	}
+
+	return fd;
+}
+
 static void g_pn_nl_addr(GPhonetNetlink *self, struct nlmsghdr *nlh)
 {
 	int len;
@@ -177,14 +196,13 @@ static void g_pn_nl_link(GPhonetNetlink *self, struct nlmsghdr *nlh)
 		self->callback(idx, st, ifname, self->opaque);
 }
 
-
 /* Parser Netlink messages */
 static gboolean g_pn_nl_process(GIOChannel *channel, GIOCondition cond,
 				gpointer data)
 {
 	struct {
 		struct nlmsghdr nlh;
-		char buf[16384];
+		char buf[SIZE_NLMSG];
 	} resp;
 	struct iovec iov = { &resp, (sizeof resp), };
 	struct msghdr msg = { .msg_iov = &iov, .msg_iovlen = 1, };
@@ -201,8 +219,8 @@ static gboolean g_pn_nl_process(GIOChannel *channel, GIOCondition cond,
 		return TRUE;
 
 	if (msg.msg_flags & MSG_TRUNC) {
-		g_critical("Netlink message of %zu bytes truncated at %zu",
-			   ret, (sizeof resp));
+		g_printerr("Netlink message of %zu bytes truncated at %zu\n",
+			ret, sizeof(resp));
 		return TRUE;
 	}
 
@@ -213,10 +231,11 @@ static gboolean g_pn_nl_process(GIOChannel *channel, GIOCondition cond,
 
 		switch (nlh->nlmsg_type) {
 		case NLMSG_ERROR: {
-			const struct nlmsgerr *err;
-			err = (struct nlmsgerr *)NLMSG_DATA(nlh);
-			g_critical("Netlink error: %s", strerror(-err->error));
-			return FALSE;
+			struct nlmsgerr *err = NLMSG_DATA(nlh);
+			if (err->error)
+				g_printerr("Netlink error: %s",
+					strerror(-err->error));
+			return TRUE;
 		}
 		case RTM_NEWADDR:
 		case RTM_DELADDR:
@@ -269,7 +288,7 @@ GPhonetNetlink *g_pn_netlink_start(GIsiModem *idx,
 	unsigned group = RTNLGRP_LINK;
 	unsigned interface = g_isi_modem_index(idx);
 
-	fd = socket(PF_NETLINK, SOCK_DGRAM, NETLINK_ROUTE);
+	fd = netlink_socket();
 	if (fd == -1)
 		return NULL;
 
@@ -320,3 +339,167 @@ void g_pn_netlink_stop(GPhonetNetlink *self)
 		free(self);
 	}
 }
+
+static int netlink_getack(int fd)
+{
+	struct {
+		struct nlmsghdr nlh;
+		char buf[SIZE_NLMSG];
+	} resp;
+	struct iovec iov = { &resp, sizeof(resp), };
+	struct msghdr msg = { .msg_iov = &iov, .msg_iovlen = 1, };
+	ssize_t ret;
+	struct nlmsghdr *nlh = &resp.nlh;
+
+	ret = recvmsg(fd, &msg, 0);
+	if (ret == -1)
+		return -errno;
+
+	if (msg.msg_flags & MSG_TRUNC)
+		return -EIO;
+
+	for (; NLMSG_OK(nlh, (size_t)ret); nlh = NLMSG_NEXT(nlh, ret)) {
+
+		if (nlh->nlmsg_type == NLMSG_DONE)
+			return 0;
+
+		if (nlh->nlmsg_type == NLMSG_ERROR) {
+			struct nlmsgerr *err = NLMSG_DATA(nlh);
+			return err->error;
+		}
+	}
+
+	return -EIO;
+}
+
+/* Set local address */
+static int netlink_setaddr(uint32_t ifa_index, uint8_t ifa_local)
+{
+	struct ifaddrmsg *ifa;
+	struct rtattr *rta;
+	uint32_t reqlen = NLMSG_LENGTH(NLMSG_ALIGN(sizeof *ifa) + RTA_SPACE(1));
+	struct req {
+		struct nlmsghdr nlh;
+		char buf[512];
+	} req = {
+		.nlh = {
+			.nlmsg_flags = NLM_F_REQUEST | NLM_F_ACK,
+			.nlmsg_type = RTM_NEWADDR,
+			.nlmsg_pid = getpid(),
+			.nlmsg_len = reqlen,
+		},
+	};
+	int fd;
+	int error;
+	struct sockaddr_nl addr = { .nl_family = AF_NETLINK, };
+
+	ifa = NLMSG_DATA(&req.nlh);
+	ifa->ifa_family = AF_PHONET;
+	ifa->ifa_prefixlen = 0;
+	ifa->ifa_index = ifa_index;
+
+	rta = IFA_RTA(ifa);
+	rta->rta_type = IFA_LOCAL;
+	rta->rta_len = RTA_LENGTH(1);
+	*(uint8_t *)RTA_DATA(rta) = ifa_local;
+
+	fd = netlink_socket();
+	if (fd == -1)
+		return -errno;
+
+	if (sendto(fd, &req, reqlen, 0, (void *)&addr, sizeof(addr)) == -1)
+		error = -errno;
+	else
+		error = netlink_getack(fd);
+
+	close(fd);
+
+	return error;
+}
+
+int g_pn_netlink_set_address(GIsiModem *idx, uint8_t local)
+{
+	uint32_t ifindex = g_isi_modem_index(idx);
+
+	if (ifindex == 0)
+		return -ENODEV;
+
+	if (local != PN_DEV_PC && local != PN_DEV_SOS)
+		return -EINVAL;
+
+	return netlink_setaddr(ifindex, local);
+}
+
+/* Add remote address */
+static int netlink_addroute(uint32_t ifa_index, uint8_t remote)
+{
+	struct rtmsg *rtm;
+	struct rtattr *rta;
+	uint32_t reqlen = NLMSG_LENGTH(NLMSG_ALIGN(sizeof *rtm) +
+				RTA_SPACE(1) +
+				RTA_SPACE(sizeof ifa_index));
+	struct req {
+		struct nlmsghdr nlh;
+		char buf[512];
+	} req = {
+		.nlh = {
+			.nlmsg_flags = NLM_F_REQUEST | NLM_F_ACK
+					| NLM_F_CREATE | NLM_F_APPEND,
+			.nlmsg_type = RTM_NEWROUTE,
+			.nlmsg_pid = getpid(),
+			.nlmsg_len = reqlen,
+		},
+	};
+	size_t buflen = sizeof(req.buf) - sizeof(*rtm);
+	int fd;
+	int error;
+	struct sockaddr_nl addr = { .nl_family = AF_NETLINK, };
+
+	rtm = NLMSG_DATA(&req.nlh);
+	rtm->rtm_family = AF_PHONET;
+	rtm->rtm_dst_len = 6;
+	rtm->rtm_src_len = 0;
+	rtm->rtm_tos = 0;
+
+	rtm->rtm_table = RT_TABLE_MAIN;
+	rtm->rtm_protocol = RTPROT_STATIC;
+	rtm->rtm_scope = RT_SCOPE_UNIVERSE;
+	rtm->rtm_type = RTN_UNICAST;
+	rtm->rtm_flags = 0;
+
+	rta = IFA_RTA(rtm);
+	rta->rta_type = RTA_DST;
+	rta->rta_len = RTA_LENGTH(1);
+	*(uint8_t *)RTA_DATA(rta) = remote;
+
+	rta = RTA_NEXT(rta, buflen);
+	rta->rta_type = RTA_OIF;
+	rta->rta_len = RTA_LENGTH(sizeof(ifa_index));
+	*(uint32_t *)RTA_DATA(rta) = ifa_index;
+
+	fd = netlink_socket();
+	if (fd == -1)
+		return -errno;
+
+	if (sendto(fd, &req, reqlen, 0, (void *)&addr, sizeof(addr)) == -1)
+		error = -errno;
+	else
+		error = netlink_getack(fd);
+
+	close(fd);
+
+	return error;
+}
+
+int g_pn_netlink_add_route(GIsiModem *idx, uint8_t remote)
+{
+	uint32_t ifindex = g_isi_modem_index(idx);
+
+	if (ifindex == 0)
+		return -ENODEV;
+
+	if (remote != PN_DEV_SOS && remote != PN_DEV_HOST)
+		return -EINVAL;
+
+	return netlink_addroute(ifindex, remote);
+}