sysmocom
/
ofono
mirror of git://git.sysmocom.de/ofono
11
0
Fork 0
Code Issues Releases Activity

gisi: Add phonet netlink functions 

Adding g_pn_netlink_set_address() and g_pn_netlink_add_route().
Automatically configure phonet links without external software.
This commit is contained in:
Pekka Pessi 2010-04-28 16:37:10 +03:00 committed by Aki Niemi
parent 2cda1a32da
commit cb6c7972c0
2 changed files with 201 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

201
gisi/netlink.c
View File

@ -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);
}

9
gisi/netlink.h
View File

@ -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