mirror of git://git.sysmocom.de/ofono
402 lines
8.5 KiB
C
402 lines
8.5 KiB
C
/*
|
|
*
|
|
* oFono - Open Source Telephony
|
|
*
|
|
* Copyright (C) 2009-2010 Nokia Corporation and/or its subsidiary(-ies).
|
|
*
|
|
* This program is free software; you can redistribute it and/or modify
|
|
* it under the terms of the GNU General Public License version 2 as
|
|
* published by the Free Software Foundation.
|
|
*
|
|
* This program is distributed in the hope that it will be useful,
|
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
* GNU General Public License for more details.
|
|
*
|
|
* You should have received a copy of the GNU General Public License
|
|
* along with this program; if not, write to the Free Software
|
|
* Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
|
|
*
|
|
*/
|
|
|
|
#ifdef HAVE_CONFIG_H
|
|
#include <config.h>
|
|
#endif
|
|
|
|
#include <stdint.h>
|
|
#include <stdlib.h>
|
|
#include <string.h>
|
|
#include <sys/types.h>
|
|
#include <unistd.h>
|
|
#include <fcntl.h>
|
|
#include <sys/socket.h>
|
|
#ifndef SOL_NETLINK
|
|
#define SOL_NETLINK 270 /* libc!? */
|
|
#endif
|
|
#include "phonet.h"
|
|
#include <linux/rtnetlink.h>
|
|
#include <linux/if.h>
|
|
#include <sys/ioctl.h>
|
|
#include <errno.h>
|
|
#include <glib.h>
|
|
|
|
#include "netlink.h"
|
|
|
|
#ifndef ARPHRD_PHONET
|
|
#define ARPHRD_PHONET (820)
|
|
#endif
|
|
|
|
/*
|
|
* GCC -Wcast-align does not like rtlink alignment macros,
|
|
* fixed macros by Andrzej Zaborowski <balrogg@gmail.com>.
|
|
*/
|
|
#undef IFA_RTA
|
|
#define IFA_RTA(r) ((struct rtattr *)(void *)(((char *)(r)) \
|
|
+ NLMSG_ALIGN(sizeof(struct ifaddrmsg))))
|
|
|
|
#undef IFLA_RTA
|
|
#define IFLA_RTA(r) ((struct rtattr *)(void *)(((char *)(r)) \
|
|
+ NLMSG_ALIGN(sizeof(struct ifinfomsg))))
|
|
|
|
#undef NLMSG_NEXT
|
|
#define NLMSG_NEXT(nlh, len) ((len) -= NLMSG_ALIGN((nlh)->nlmsg_len), \
|
|
(struct nlmsghdr *)(void *)(((char *)(nlh)) \
|
|
+ NLMSG_ALIGN((nlh)->nlmsg_len)))
|
|
|
|
#undef RTA_NEXT
|
|
#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 _GIsiPhonetNetlink {
|
|
GIsiModem *modem;
|
|
GIsiPhonetNetlinkFunc callback;
|
|
void *opaque;
|
|
guint watch;
|
|
};
|
|
|
|
static GSList *netlink_list;
|
|
|
|
static void bring_up(unsigned ifindex)
|
|
{
|
|
struct ifreq req = { .ifr_ifindex = ifindex, };
|
|
int fd = socket(PF_LOCAL, SOCK_DGRAM, 0);
|
|
|
|
if (ioctl(fd, SIOCGIFNAME, &req)
|
|
|| ioctl(fd, SIOCGIFFLAGS, &req))
|
|
goto error;
|
|
|
|
req.ifr_flags |= IFF_UP | IFF_RUNNING;
|
|
ioctl(fd, SIOCSIFFLAGS, &req);
|
|
error:
|
|
close(fd);
|
|
}
|
|
|
|
static int pn_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 pn_netlink_link(GIsiPhonetNetlink *self, struct nlmsghdr *nlh)
|
|
{
|
|
const struct ifinfomsg *ifi;
|
|
const struct rtattr *rta;
|
|
int len;
|
|
const char *ifname = NULL;
|
|
enum GIsiPhonetLinkState st;
|
|
unsigned interface;
|
|
|
|
ifi = NLMSG_DATA(nlh);
|
|
len = IFA_PAYLOAD(nlh);
|
|
|
|
if (ifi->ifi_type != ARPHRD_PHONET)
|
|
return;
|
|
|
|
interface = g_isi_modem_index(self->modem);
|
|
if (interface != 0 && interface != (unsigned)ifi->ifi_index)
|
|
return;
|
|
|
|
#define UP (IFF_UP | IFF_LOWER_UP | IFF_RUNNING)
|
|
|
|
if (nlh->nlmsg_type == RTM_DELLINK)
|
|
st = PN_LINK_REMOVED;
|
|
else if ((ifi->ifi_flags & UP) != UP)
|
|
st = PN_LINK_DOWN;
|
|
else
|
|
st = PN_LINK_UP;
|
|
|
|
for (rta = IFLA_RTA(ifi); RTA_OK(rta, len);
|
|
rta = RTA_NEXT(rta, len)) {
|
|
|
|
if (rta->rta_type == IFLA_IFNAME)
|
|
ifname = RTA_DATA(rta);
|
|
}
|
|
|
|
if (ifname && self->modem)
|
|
self->callback(self->modem, st, ifname, self->opaque);
|
|
|
|
#undef UP
|
|
}
|
|
|
|
/* Parser Netlink messages */
|
|
static gboolean pn_netlink_process(GIOChannel *channel, GIOCondition cond,
|
|
gpointer data)
|
|
{
|
|
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;
|
|
int fd = g_io_channel_unix_get_fd(channel);
|
|
GIsiPhonetNetlink *self = data;
|
|
|
|
if (cond & (G_IO_NVAL|G_IO_HUP))
|
|
return FALSE;
|
|
|
|
ret = recvmsg(fd, &msg, 0);
|
|
if (ret == -1)
|
|
return TRUE;
|
|
|
|
if (msg.msg_flags & MSG_TRUNC) {
|
|
g_printerr("Netlink message of %zu bytes truncated at %zu\n",
|
|
ret, sizeof(resp));
|
|
return TRUE;
|
|
}
|
|
|
|
for (nlh = &resp.nlh; NLMSG_OK(nlh, (size_t)ret);
|
|
nlh = NLMSG_NEXT(nlh, ret)) {
|
|
|
|
if (nlh->nlmsg_type == NLMSG_DONE)
|
|
break;
|
|
|
|
switch (nlh->nlmsg_type) {
|
|
case NLMSG_ERROR: {
|
|
struct nlmsgerr *err = NLMSG_DATA(nlh);
|
|
if (err->error)
|
|
g_printerr("Netlink error: %s",
|
|
strerror(-err->error));
|
|
return TRUE;
|
|
}
|
|
case RTM_NEWLINK:
|
|
case RTM_DELLINK:
|
|
pn_netlink_link(self, nlh);
|
|
break;
|
|
}
|
|
}
|
|
return TRUE;
|
|
}
|
|
|
|
/* Dump current links */
|
|
static int pn_netlink_getlink(int fd)
|
|
{
|
|
struct {
|
|
struct nlmsghdr nlh;
|
|
struct ifinfomsg ifi;
|
|
} req = {
|
|
.nlh = {
|
|
.nlmsg_type = RTM_GETLINK,
|
|
.nlmsg_len = sizeof(req),
|
|
.nlmsg_flags = NLM_F_REQUEST | NLM_F_ROOT | NLM_F_MATCH,
|
|
.nlmsg_pid = getpid(),
|
|
},
|
|
.ifi = {
|
|
.ifi_family = AF_UNSPEC,
|
|
.ifi_type = ARPHRD_PHONET,
|
|
.ifi_change = 0xffFFffFF,
|
|
}
|
|
};
|
|
|
|
struct sockaddr_nl addr = { .nl_family = AF_NETLINK, };
|
|
|
|
return sendto(fd, &req, sizeof(req), 0,
|
|
(struct sockaddr *)&addr, sizeof(addr));
|
|
}
|
|
|
|
GIsiPhonetNetlink *g_isi_pn_netlink_by_modem(GIsiModem *modem)
|
|
{
|
|
GSList *m;
|
|
|
|
for (m = netlink_list; m; m = m->next) {
|
|
GIsiPhonetNetlink *self = m->data;
|
|
|
|
if (g_isi_modem_index(modem) == g_isi_modem_index(self->modem))
|
|
return self;
|
|
}
|
|
|
|
return NULL;
|
|
}
|
|
|
|
GIsiPhonetNetlink *g_isi_pn_netlink_start(GIsiModem *modem,
|
|
GIsiPhonetNetlinkFunc cb,
|
|
void *data)
|
|
{
|
|
GIOChannel *chan;
|
|
GIsiPhonetNetlink *self;
|
|
int fd;
|
|
unsigned group = RTNLGRP_LINK;
|
|
unsigned interface;
|
|
|
|
fd = pn_netlink_socket();
|
|
if (fd == -1)
|
|
return NULL;
|
|
|
|
self = g_try_new0(GIsiPhonetNetlink, 1);
|
|
if (self == NULL)
|
|
goto error;
|
|
|
|
fcntl(fd, F_SETFL, O_NONBLOCK | fcntl(fd, F_GETFL));
|
|
|
|
if (setsockopt(fd, SOL_NETLINK, NETLINK_ADD_MEMBERSHIP,
|
|
&group, sizeof(group)))
|
|
goto error;
|
|
|
|
interface = g_isi_modem_index(modem);
|
|
if (interface)
|
|
bring_up(interface);
|
|
|
|
pn_netlink_getlink(fd);
|
|
|
|
chan = g_io_channel_unix_new(fd);
|
|
if (chan == NULL)
|
|
goto error;
|
|
|
|
g_io_channel_set_close_on_unref(chan, TRUE);
|
|
g_io_channel_set_encoding(chan, NULL, NULL);
|
|
g_io_channel_set_buffered(chan, FALSE);
|
|
|
|
self->callback = cb;
|
|
self->opaque = data;
|
|
self->modem = modem;
|
|
self->watch = g_io_add_watch(chan, G_IO_IN|G_IO_ERR|G_IO_HUP,
|
|
pn_netlink_process, self);
|
|
g_io_channel_unref(chan);
|
|
|
|
netlink_list = g_slist_prepend(netlink_list, self);
|
|
|
|
return self;
|
|
|
|
error:
|
|
close(fd);
|
|
free(self);
|
|
return NULL;
|
|
}
|
|
|
|
void g_isi_pn_netlink_stop(GIsiPhonetNetlink *self)
|
|
{
|
|
if (self == NULL)
|
|
return;
|
|
|
|
netlink_list = g_slist_remove(netlink_list, self);
|
|
g_source_remove(self->watch);
|
|
g_free(self);
|
|
}
|
|
|
|
static int pn_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 pn_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 = pn_netlink_socket();
|
|
if (fd == -1)
|
|
return -errno;
|
|
|
|
if (sendto(fd, &req, reqlen, 0, (void *)&addr, sizeof(addr)) == -1)
|
|
error = -errno;
|
|
else
|
|
error = pn_netlink_getack(fd);
|
|
|
|
close(fd);
|
|
|
|
return error;
|
|
}
|
|
|
|
int g_isi_pn_netlink_set_address(GIsiModem *modem, uint8_t local)
|
|
{
|
|
uint32_t ifindex = g_isi_modem_index(modem);
|
|
|
|
if (ifindex == 0)
|
|
return -ENODEV;
|
|
|
|
if (local != PN_DEV_PC && local != PN_DEV_SOS)
|
|
return -EINVAL;
|
|
|
|
return pn_netlink_setaddr(ifindex, local);
|
|
}
|