[PATCH] usbpnmodem: configure usbpn interfaces automatically

[PATCH] usbpnmodem: configure usbpn interfaces automatically

[Pekka Pessi]

[-- Attachment #1: Type: text/plain, Size: 9611 bytes --]

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

Re: [PATCH] usbpnmodem: configure usbpn interfaces automatically

[Denis Kenzior]

[-- Attachment #1: Type: text/plain, Size: 918 bytes --]

Hi Pekka,

> ---
>  drivers/isimodem/isimodem.c |    2 +-
>  gisi/netlink.c              |  242
>  ++++++++++++++++++++++++++++++++++++++++--- gisi/netlink.h              | 
>   11 ++-
>  plugins/usbpnmodem.c        |   10 ++-
>  4 files changed, 244 insertions(+), 21 deletions(-)
> 
> 

We're quite serious about coding style here:

WARNING: line over 80 characters
#156: FILE: gisi/netlink.c:239:
+				g_critical("Netlink error: %s", strerror(-err->error));

ERROR: code indent should use tabs where possible
#204: FILE: gisi/netlink.c:367:
+        for (; NLMSG_OK(nlh, (size_t)ret); nlh = NLMSG_NEXT(nlh, ret)) {$

WARNING: line over 80 characters
#210: FILE: gisi/netlink.c:373:
+			struct nlmsgerr *err = (struct nlmsgerr *)NLMSG_DATA(nlh);

ERROR: need consistent spacing around '|' (ctx:VxW)
#306: FILE: gisi/netlink.c:469:
+					| NLM_F_CREATE| NLM_F_APPEND,

Regards,
-Denis

[PATCH] usbpnmodem: configure usbpn interfaces automatically

[Pekka Pessi]

[-- Attachment #1: Type: text/plain, Size: 9643 bytes --]

---
 drivers/isimodem/isimodem.c |    2 +-
 gisi/netlink.c              |  246 +++++++++++++++++++++++++++++++++++++++---
 gisi/netlink.h              |   11 ++-
 plugins/usbpnmodem.c        |   10 ++-
 4 files changed, 246 insertions(+), 23 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..36bf273 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, };
@@ -225,10 +233,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;
+			const struct nlmsgerr *err = NLMSG_DATA(nlh);
+			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 = 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 = 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 = 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

Re: [PATCH] usbpnmodem: configure usbpn interfaces automatically

[Denis Kenzior]

[-- Attachment #1: Type: text/plain, Size: 10982 bytes --]

Hi Pekka,

> ---
>  drivers/isimodem/isimodem.c |    2 +-
>  gisi/netlink.c              |  246
>  +++++++++++++++++++++++++++++++++++++++--- gisi/netlink.h              |  
>  11 ++-
>  plugins/usbpnmodem.c        |   10 ++-
>  4 files changed, 246 insertions(+), 23 deletions(-)

So again, you're mixing library and driver / plugin patches.  There's nothing 
wrong with sending a series of several...

> 
> 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..36bf273 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);
> -	}
> -}
> -

Removal of this function can be done in a separate patch so that it is much 
easier to review.

>  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, };
> @@ -225,10 +233,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;
> +			const struct nlmsgerr *err = NLMSG_DATA(nlh);
> +			if (err->error)
> +				g_critical("Netlink error: %s",
> +					strerror(-err->error));

Since oFono is a daemon, you really might want to rethink the use of 
g_critical.  See how gatchat handles this.

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

Why exactly is there an extra space?

>  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 = 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 = 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 = 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;
> +

I'm running away scared from this one...

> +	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;

This initialization is pointless

> +	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;

Ok, what in the world does this do? Seriously please re-do this logic.

> +	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;
> 

Regards,
-Denis

Re: [PATCH] usbpnmodem: configure usbpn interfaces automatically

[Pekka Pessi]

[-- Attachment #1: Type: text/plain, Size: 308 bytes --]

2010/4/23 Denis Kenzior <denkenz@gmail.com>:
>> +     *(uint32_t *)(void *)RTA_DATA(rta) = ifa_index;

> I'm running away scared from this one...

This is the GCC -Wcast-align problem. It does not grok the alignment
logic in RTA_DATA() macro from Linux kernel.

-- 
Pekka.Pessi mail at nokia.com

Re: [PATCH] usbpnmodem: configure usbpn interfaces automatically

[=?unknown-8bit?q?R=C3=A9mi?= Denis-Courmont]

[-- Attachment #1: Type: text/plain, Size: 582 bytes --]


On Fri, 23 Apr 2010 18:05:18 +0300, Pekka Pessi <Pekka.Pessi@nokia.com>
wrote:
> ---
>  drivers/isimodem/isimodem.c |    2 +-
>  gisi/netlink.c              |  246
> +++++++++++++++++++++++++++++++++++++++---
>  gisi/netlink.h              |   11 ++-
>  plugins/usbpnmodem.c        |   10 ++-
>  4 files changed, 246 insertions(+), 23 deletions(-)

I fail to see why we need this. usbpn is already autoconfigured by the
kernel. It only needs to be brought administratively up.

-- 
Rémi Denis-Courmont
http://www.remlab.net
http://fi.linkedin.com/in/remidenis

Re: [PATCH] usbpnmodem: configure usbpn interfaces automatically

[Pekka Pessi]

[-- Attachment #1: Type: text/plain, Size: 293 bytes --]

2010/4/28 Rémi Denis-Courmont <remi@remlab.net>:
> I fail to see why we need this. usbpn is already autoconfigured by the
> kernel. It only needs to be brought administratively up.

This is not the case with 2.6.31. Does it hurt with later kernels?

-- 
Pekka.Pessi mail at nokia.com

Re: [PATCH] usbpnmodem: configure usbpn interfaces automatically

[=?unknown-8bit?q?R=C3=A9mi?= Denis-Courmont]

[-- Attachment #1: Type: text/plain, Size: 965 bytes --]

Le mercredi 28 avril 2010 17:15:54 Pekka Pessi, vous avez écrit :
> 2010/4/28 Rémi Denis-Courmont <remi@remlab.net>:
> > I fail to see why we need this. usbpn is already autoconfigured by the
> > kernel. It only needs to be brought administratively up.
> 
> This is not the case with 2.6.31. Does it hurt with later kernels?

Not all kernel versions support routing. And I'd hate to duplicate existing 
kernel functionality in oFono user-space forever just for 2.6.31 (long term 
support is 2.6.32 by the way). There's probably two users in the entire world 
who care about the 2.6.31 + USB CDC Phonet + oFono - phonet-utils combination.

Not to mention that it is done in a much cleaner and compact way in kernel 
space. Also this would prevent me or someone else from ever improving the 
Phonet (auto)configuration later, which is certainly on my roadmap.

-- 
Rémi Denis-Courmont
http://www.remlab.net/
http://fi.linkedin.com/in/remidenis

Re: [PATCH] usbpnmodem: configure usbpn interfaces automatically

[Pekka Pessi]

[-- Attachment #1: Type: text/plain, Size: 917 bytes --]

2010/4/28 Rémi Denis-Courmont <remi@remlab.net>:
> Le mercredi 28 avril 2010 17:15:54 Pekka Pessi, vous avez écrit :
>> 2010/4/28 Rémi Denis-Courmont <remi@remlab.net>:
>> > I fail to see why we need this. usbpn is already autoconfigured by the
>> > kernel. It only needs to be brought administratively up.
>>
>> This is not the case with 2.6.31. Does it hurt with later kernels?
>
> Not all kernel versions support routing.

You are referring to the route setting with "this", not to the local
address setting? ;-o

> And I'd hate to duplicate existing
> kernel functionality in oFono user-space forever just for 2.6.31 (long term
> support is 2.6.32 by the way). There's probably two users in the entire world
> who care about the 2.6.31 + USB CDC Phonet + oFono - phonet-utils combination.

I'd like to think we have larger developer space for Meego. ;)

-- 
Pekka.Pessi mail at nokia.com