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); +} diff --git a/gisi/netlink.h b/gisi/netlink.h index 983e387e..5b58fa48 100644 --- 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