gisi: Add phonet netlink functions

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

diff --git a/gisi/netlink.c b/gisi/netlink.c
index 59322e3..598ff0e 100644 (file)
--- 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);
+}

diff --git a/gisi/netlink.h b/gisi/netlink.h
index 983e387..5b58fa4 100644 (file)
--- a/gisi/netlink.h
+++ b/gisi/netlink.h
@@ -41,6 +41,12 @@ typedef enum {
        PN_LINK_UP
 } GPhonetLinkState;
 
+enum {
+       PN_DEV_PC = 0x10,       /* PC Suite */
+       PN_DEV_HOST = 0x00,     /* Modem */
+       PN_DEV_SOS = 0x6C,      /* Symbian or Linux */
+};
+
 typedef void (*GPhonetNetlinkFunc)(GIsiModem *idx,
                        GPhonetLinkState st,
                        char const *iface,
@@ -54,6 +60,9 @@ GPhonetNetlink *g_pn_netlink_start(GIsiModem *idx,
 
 void g_pn_netlink_stop(GPhonetNetlink *self);
 
+int g_pn_netlink_set_address(GIsiModem *, uint8_t local);
+int g_pn_netlink_add_route(GIsiModem *, uint8_t remote);
+
 #ifdef __cplusplus
 }
 #endif