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);
}
GPhonetNetlink *g_pn_netlink_start(GIsiModem *idx,
GPhonetNetlinkFunc callback,
void *data)
{
GIOChannel *chan;
GPhonetNetlink *self;
int fd;
unsigned group = RTNLGRP_LINK;
unsigned interface = g_isi_modem_index(idx);
fd = netlink_socket();
if (fd == -1)
return NULL;
self = calloc(1, sizeof(*self));
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;
if (interface)
bring_up(interface);
g_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 = callback;
self->opaque = data;
self->interface = interface;
self->watch = g_io_add_watch(chan, G_IO_IN|G_IO_ERR|G_IO_HUP,
g_pn_nl_process, self);
g_io_channel_unref(chan);
netlink_list = g_slist_prepend(netlink_list, self);
return self;
error:
close(fd);
free(self);
return NULL;
}
GPhonetNetlink *g_pn_netlink_by_modem(GIsiModem *idx)
{
GSList *m;
unsigned index = g_isi_modem_index(idx);
for (m = netlink_list; m; m = m->next) {
GPhonetNetlink *self = m->data;
if (index == self->interface)
return self;
}
return NULL;
}
GIsiPEP *g_isi_pep_create(GIsiModem *modem, GIsiPEPCallback cb, void *opaque)
{
unsigned ifi = g_isi_modem_index(modem);
GIsiPEP *pep = NULL;
GIOChannel *channel;
int fd;
char buf[IF_NAMESIZE];
fd = socket(PF_PHONET, SOCK_SEQPACKET, 0);
if (fd == -1)
return NULL;
fcntl(fd, F_SETFD, FD_CLOEXEC);
fcntl(fd, F_SETFL, O_NONBLOCK|fcntl(fd, F_GETFL));
if (if_indextoname(ifi, buf) == NULL)
goto error;
if (setsockopt(fd, SOL_SOCKET, SO_BINDTODEVICE, buf, IF_NAMESIZE) != 0)
goto error;
pep = g_try_malloc(sizeof(GIsiPEP));
if (pep == NULL)
goto error;
pep->ready = cb;
pep->opaque = opaque;
pep->gprs_fd = -1;
pep->handle = 0;
if (listen(fd, 1) || ioctl(fd, SIOCPNGETOBJECT, &pep->handle))
goto error;
channel = g_io_channel_unix_new(fd);
g_io_channel_set_close_on_unref(channel, TRUE);
g_io_channel_set_encoding(channel, NULL, NULL);
g_io_channel_set_buffered(channel, FALSE);
pep->source = g_io_add_watch(channel,
G_IO_IN|G_IO_ERR|G_IO_HUP|G_IO_NVAL,
g_isi_pep_callback, pep);
g_io_channel_unref(channel);
return pep;
error:
close(fd);
g_free(pep);
return NULL;
}
static void phonet_status_cb(GIsiModem *modem, enum GIsiPhonetLinkState st,
char const *ifname, void *data)
{
struct ofono_modem *om = data;
struct isi_data *isi = ofono_modem_get_data(om);
DBG("Link %s (%u) is %s", isi->ifname, g_isi_modem_index(isi->modem),
st == PN_LINK_REMOVED ? "removed" :
st == PN_LINK_DOWN ? "down" : "up");
isi->linkstate = st;
if (st == PN_LINK_UP)
g_isi_client_verify(isi->client, reachable_cb, om, NULL);
else if (st == PN_LINK_DOWN)
set_power_by_mce_state(om, isi, MCE_POWER_OFF);
}
/*
* Add or remove isimodems
* when usbpn* phonet interfaces are added/removed
*/
static void usbpn_status_cb(GIsiModem *idx,
GPhonetLinkState st,
char const ifname[],
void *data)
{
struct ofono_modem *modem;
int error;
DBG("Phonet link %s (%u) is %s",
ifname, g_isi_modem_index(idx),
st == PN_LINK_REMOVED ? "removed" :
st == PN_LINK_DOWN ? "down" : "up");
/* Expect phonet interface name usbpn<idx> */
if (strncmp(ifname, "usbpn", 5) ||
ifname[5 + strspn(ifname + 5, "0123456789")])
return;
if (st == PN_LINK_REMOVED)
return;
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_PC);
if (error && error != -EEXIST) {
DBG("g_pn_netlink_set_address: %s\n", strerror(-error));
return;
}
modem = ofono_modem_create(NULL, "isimodem");
if (!modem)
return;
ofono_modem_set_string(modem, "Interface", ifname);
if (ofono_modem_register(modem) == 0)
DBG("Done regging modem %s", ofono_modem_get_path(modem));
else
ofono_modem_remove(modem);
}
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);
}