--- drivers/isimodem/isimodem.c | 2 +- gisi/netlink.c | 242 ++++++++++++++++++++++++++++++++++++++++--- gisi/netlink.h | 11 ++- plugins/usbpnmodem.c | 10 ++- 4 files changed, 244 insertions(+), 21 deletions(-) diff --git a/drivers/isimodem/isimodem.c b/drivers/isimodem/isimodem.c index e6e0bc1..b6ae82b 100644 --- a/drivers/isimodem/isimodem.c +++ b/drivers/isimodem/isimodem.c @@ -256,7 +256,7 @@ static int isi_modem_probe(struct ofono_modem *modem) return -errno; } - link = g_pn_netlink_by_name(ifname); + link = g_pn_netlink_by_modem(idx); if (link) { DBG("%s: %s", ifname, strerror(EBUSY)); return -EBUSY; diff --git a/gisi/netlink.c b/gisi/netlink.c index 1a18b45..0c0a212 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; @@ -98,18 +99,6 @@ GPhonetNetlink *g_pn_netlink_by_modem(GIsiModem *idx) return NULL; } -GPhonetNetlink *g_pn_netlink_by_name(const char *ifname) -{ - if (ifname == NULL) { - return g_pn_netlink_by_modem(make_modem(0)); - } else { - GIsiModem *idx = g_isi_modem_by_name(ifname); - if (!idx) - return NULL; - return g_pn_netlink_by_modem(idx); - } -} - static void bring_up(unsigned ifindex) { struct ifreq req = { .ifr_ifindex = ifindex, }; @@ -124,6 +113,25 @@ 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; + g_warning("sockopt(SO_RCVBUF): %s", strerror(error)); + close(fd), fd = -1; + errno = error; + } + + return fd; +} + static void g_pn_nl_addr(GPhonetNetlink *self, struct nlmsghdr *nlh) { int len; @@ -196,7 +204,7 @@ static gboolean g_pn_nl_process(GIOChannel *channel, GIOCondition cond, { 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, }; @@ -227,8 +235,9 @@ static gboolean g_pn_nl_process(GIOChannel *channel, GIOCondition cond, case NLMSG_ERROR: { const struct nlmsgerr *err; err = (struct nlmsgerr *)NLMSG_DATA(nlh); - g_critical("Netlink error: %s", strerror(-err->error)); - return FALSE; + if (err->error) + g_critical("Netlink error: %s", strerror(-err->error)); + return TRUE; } case RTM_NEWADDR: case RTM_DELADDR: @@ -271,6 +280,7 @@ static int g_pn_netlink_getlink(int fd) (struct sockaddr *)&addr, sizeof(addr)); } + GPhonetNetlink *g_pn_netlink_start(GIsiModem *idx, GPhonetNetlinkFunc callback, void *data) @@ -281,7 +291,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; @@ -332,3 +342,203 @@ 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) { + g_critical("Netlink message of %zu bytes truncated at %zu", + ret, (sizeof resp)); + 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 = (struct nlmsgerr *)NLMSG_DATA(nlh); + return err->error; + } + } + + return -EIO; +} + +/* Set local address */ +static int netlink_setaddr(int fd, 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, + }, + }; + + struct sockaddr_nl addr = { .nl_family = AF_NETLINK, }; + + ifa = (struct ifaddrmsg *)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; + + if (sendto(fd, &req, reqlen, 0, (void *)&addr, sizeof addr) == -1) + return -errno; + + return 0; +} + +int g_pn_netlink_set_address(GIsiModem *idx, uint8_t local) +{ + int fd = -1; + uint32_t ifindex; + int error = 0; + + if (idx == NULL) + return -ENODEV; + + if (local != PN_DEV_PC && local != PN_DEV_SOS) + return -EINVAL; + + ifindex = g_isi_modem_index(idx); + + fd = netlink_socket(); + if (fd == -1) + goto error; + + error = netlink_setaddr(fd, ifindex, local); + if (error) + goto error; + + error = netlink_getack(fd); + if (error) + goto error; + + close(fd); + return 0; + +error: + if (!error) + error = -(errno ? errno : -EIO); + if (fd != -1) + close(fd); + return error; +} + +/* Add remote address */ +static int netlink_addroute(int fd, 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); + + struct sockaddr_nl addr = { .nl_family = AF_NETLINK, }; + + rtm = (struct rtmsg *)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 *)(void *)RTA_DATA(rta) = ifa_index; + + if (sendto(fd, &req, reqlen, 0, (void *)&addr, sizeof addr) == -1) + return -errno; + + return 0; +} + +int g_pn_netlink_add_route(GIsiModem *idx, uint8_t remote) +{ + int fd = -1; + uint32_t ifindex; + int error = 0; + + if (idx == NULL) + return -ENODEV; + + if (remote != PN_DEV_SOS && remote != PN_DEV_HOST) + return -EINVAL; + + ifindex = g_isi_modem_index(idx); + + fd = netlink_socket(); + if (fd == -1) + goto error; + + error = netlink_addroute(fd, ifindex, remote); + if (error) + goto error; + + error = netlink_getack(fd); + if (error) + goto error; + + close(fd); + return 0; + +error: + if (!error) + error = -errno; + if (!error) + error = -EIO; + if (fd != -1) + close(fd); + return error; +} diff --git a/gisi/netlink.h b/gisi/netlink.h index dfade5b..5b58fa4 100644 --- a/gisi/netlink.h +++ b/gisi/netlink.h @@ -41,13 +41,17 @@ 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, void *data); -GPhonetNetlink *g_pn_netlink_by_name(char const *ifname); - GPhonetNetlink *g_pn_netlink_by_modem(GIsiModem *idx); GPhonetNetlink *g_pn_netlink_start(GIsiModem *idx, @@ -56,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 diff --git a/plugins/usbpnmodem.c b/plugins/usbpnmodem.c index 5f06e9d..e1ce4fd 100644 --- a/plugins/usbpnmodem.c +++ b/plugins/usbpnmodem.c @@ -49,6 +49,7 @@ static void usbpn_status_cb(GIsiModem *idx, void *data) { struct ofono_modem *modem; + int error; DBG("Phonet link %s (%u) is %s", ifname, g_isi_modem_index(idx), @@ -63,12 +64,17 @@ static void usbpn_status_cb(GIsiModem *idx, if (st == PN_LINK_REMOVED) return; - link = g_pn_netlink_by_name(ifname); - if (link) { + if (g_pn_netlink_by_modem(idx)) { DBG("Modem for interface %s already exists", ifname); return; } + error = g_pn_netlink_set_address(idx, PN_DEV_SOS); + if (error && error != -EEXIST) { + DBG("g_pn_netlink_set_address: %s\n", strerror(-error)); + return; + } + modem = ofono_modem_create(NULL, "isimodem"); if (!modem) return; -- 1.6.3.3