sysmobts-mgr: Respond to ipaccess-find broadcast messages

Bind to port 3006 and listen to incoming IPA requests. Currently
we unconditionally respond with the MAC and IP Address of the unit.
To determine the IP Address the kernel is asked for thesource
address of the route for the destination. In contrast to a nanoBTS
we will reply to the port the initial request came from.

5 files changed, 292 insertions, 14 deletions

diff --git a/src/osmo-bts-sysmo/Makefile.am b/src/osmo-bts-sysmo/Makefile.am
index 96ff49e9..1c08af3a 100644
--- a/src/osmo-bts-sysmo/Makefile.am
+++ b/src/osmo-bts-sysmo/Makefile.am
@@ -1,10 +1,10 @@
 AM_CPPFLAGS = $(all_includes) -I$(top_srcdir)/include -I$(OPENBSC_INCDIR)
 AM_CFLAGS = -Wall $(LIBOSMOCORE_CFLAGS) $(LIBOSMOGSM_CFLAGS) $(LIBOSMOVTY_CFLAGS) $(LIBOSMOTRAU_CFLAGS) $(LIBOSMOABIS_CFLAGS)
-LDADD = $(LIBOSMOCORE_LIBS) $(LIBOSMOGSM_LIBS) $(LIBOSMOVTY_LIBS) $(LIBOSMOTRAU_LIBS) $(LIBOSMOABIS_LIBS) -lortp
+COMMON_LDADD = $(LIBOSMOCORE_LIBS) $(LIBOSMOGSM_LIBS) $(LIBOSMOVTY_LIBS) $(LIBOSMOTRAU_LIBS) $(LIBOSMOABIS_LIBS) -lortp
 
 EXTRA_DIST = misc/sysmobts_mgr.h misc/sysmobts_misc.h misc/sysmobts_par.h \
-	misc/sysmobts_eeprom.h femtobts.h hw_misc.h l1_fwd.h l1_if.h \
-	l1_transp.h eeprom.h utils.h
+	misc/sysmobts_eeprom.h misc/sysmobts_nl.h femtobts.h hw_misc.h \
+	l1_fwd.h l1_if.h l1_transp.h eeprom.h utils.h
 
 bin_PROGRAMS = sysmobts sysmobts-remote l1fwd-proxy sysmobts-mgr sysmobts-util
 
@@ -12,14 +12,18 @@ COMMON_SOURCES = main.c femtobts.c l1_if.c oml.c sysmobts_vty.c tch.c hw_misc.c
 		 eeprom.c calib_fixup.c utils.c misc/sysmobts_par.c
 
 sysmobts_SOURCES = $(COMMON_SOURCES) l1_transp_hw.c
-sysmobts_LDADD = $(top_builddir)/src/common/libbts.a $(LDADD)
+sysmobts_LDADD = $(top_builddir)/src/common/libbts.a $(COMMON_LDADD)
 
 sysmobts_remote_SOURCES = $(COMMON_SOURCES) l1_transp_fwd.c
-sysmobts_remote_LDADD = $(top_builddir)/src/common/libbts.a $(LDADD)
+sysmobts_remote_LDADD = $(top_builddir)/src/common/libbts.a $(COMMON_LDADD)
 
 l1fwd_proxy_SOURCES = l1_fwd_main.c l1_transp_hw.c
-l1fwd_proxy_LDADD = $(top_builddir)/src/common/libbts.a $(LDADD)
+l1fwd_proxy_LDADD = $(top_builddir)/src/common/libbts.a $(COMMON_LDADD)
 
-sysmobts_mgr_SOURCES = misc/sysmobts_mgr.c misc/sysmobts_misc.c misc/sysmobts_par.c
+sysmobts_mgr_SOURCES = \
+		misc/sysmobts_mgr.c misc/sysmobts_misc.c \
+		misc/sysmobts_par.c misc/sysmobts_nl.c
+sysmobts_mgr_LDADD = $(LIBOSMOCORE_LIBS)
 
 sysmobts_util_SOURCES = misc/sysmobts_util.c misc/sysmobts_par.c
+sysmobts_util_LDADD = $(LIBOSMOCORE_LIBS)
diff --git a/src/osmo-bts-sysmo/misc/sysmobts_mgr.c b/src/osmo-bts-sysmo/misc/sysmobts_mgr.c
index b5bbc311..2e031f3c 100644
--- a/src/osmo-bts-sysmo/misc/sysmobts_mgr.c
+++ b/src/osmo-bts-sysmo/misc/sysmobts_mgr.c
@@ -26,6 +26,7 @@
 #include <errno.h>
 #include <getopt.h>
 #include <limits.h>
+#include <arpa/inet.h>
 #include <sys/signal.h>
 #include <sys/types.h>
 #include <sys/stat.h>
@@ -39,6 +40,8 @@
 
 #include "misc/sysmobts_misc.h"
 #include "misc/sysmobts_mgr.h"
+#include "misc/sysmobts_nl.h"
+#include "misc/sysmobts_par.h"
 
 static int no_eeprom_write = 0;
 static int daemonize = 0;
@@ -127,6 +130,10 @@ static void signal_handler(int signal)
 #include <osmocom/core/logging.h>
 #include <osmocom/core/application.h>
 #include <osmocom/core/utils.h>
+#include <osmocom/core/socket.h>
+
+#include <osmocom/gsm/protocol/ipaccess.h>
+#include <osmocom/gsm/tlv.h>
 
 #include <osmo-bts/bts.h>
 #include <osmo-bts/logging.h>
@@ -144,6 +151,12 @@ static struct log_info_cat mgr_log_info_cat[] = {
 		.color = "\033[1;36m",
 		.enabled = 1, .loglevel = LOGL_INFO,
 	},
+	[DFIND] = {
+		.name = "DFIND",
+		.description = "ipaccess-find handling",
+		.color = "\033[1;37m",
+		.enabled = 1, .loglevel = LOGL_INFO,
+	},
 };
 
 static const struct log_info mgr_log_info = {
@@ -157,8 +170,114 @@ static int mgr_log_init(void)
 	return 0;
 }
 
+/*
+ * The TLV structure in IPA messages in UDP packages is a bit
+ * weird. First the header appears to have an extra NULL byte
+ * and second the L16 of the L16TV needs to include +1 for the
+ * tag. The default msgb/tlv and libosmo-abis routines do not
+ * provide this.
+ */
+
+static void ipaccess_prepend_header_quirk(struct msgb *msg, int proto)
+{
+	struct ipaccess_head *hh;
+
+	/* prepend the ip.access header */
+	hh = (struct ipaccess_head *) msgb_push(msg, sizeof(*hh) + 1);
+	hh->len = htons(msg->len - sizeof(*hh) - 1);
+	hh->proto = proto;
+}
+
+static void quirk_l16tv_put(struct msgb *msg, uint16_t len, uint8_t tag,
+			const uint8_t *val)
+{
+	uint8_t *buf = msgb_put(msg, len + 2 + 1);
+
+	*buf++ = (len + 1) >> 8;
+	*buf++ = (len + 1) & 0xff;
+	*buf++ = tag;
+	memcpy(buf, val, len);
+}
+
+/*
+ * We don't look at the content of the request yet and lie
+ * about most of the responses.
+ */
+static void respond_to(struct sockaddr_in *src, struct osmo_fd *fd,
+			uint8_t *data, size_t len)
+{
+	static int fetched_info = 0;
+	static char mac_str[20] = { };
+
+	struct sockaddr_in loc_addr;
+	char loc_ip[INET_ADDRSTRLEN];
+	struct msgb *msg = msgb_alloc_headroom(512, 128, "ipa get response");
+	if (!msg) {
+		LOGP(DFIND, LOGL_ERROR, "Failed to allocate msgb\n");
+		return;
+	}
+
+	if (!fetched_info) {
+		uint8_t mac[6];
+		sysmobts_par_get_buf(SYSMOBTS_PAR_MAC, mac, sizeof(mac));
+		snprintf(mac_str, sizeof(mac_str), "%.2x:%.2x:%.2x:%.2x:%.2x:%.2x",
+				mac[0], mac[1], mac[2],
+				mac[3], mac[4], mac[5]);
+		fetched_info = 1;
+	}
+
+	if (source_for_dest(&src->sin_addr, &loc_addr.sin_addr) != 0) {
+		LOGP(DFIND, LOGL_ERROR, "Failed to determine local source\n");
+		return;
+	}
+
+	msgb_put_u8(msg, IPAC_MSGT_ID_RESP);
+
+	/* append MAC addr */
+	quirk_l16tv_put(msg, strlen(mac_str) + 1, IPAC_IDTAG_MACADDR, (uint8_t *) mac_str);
+
+	/* append ip address */
+	inet_ntop(AF_INET, &loc_addr.sin_addr, loc_ip, sizeof(loc_ip));
+	quirk_l16tv_put(msg, strlen(loc_ip) + 1, IPAC_IDTAG_IPADDR, (uint8_t *) loc_ip);
+
+	/* ip.access nanoBTS would reply to port==3006 */
+	ipaccess_prepend_header_quirk(msg, IPAC_PROTO_IPACCESS);
+	sendto(fd->fd, msg->data, msg->len, 0, (struct sockaddr *)src, sizeof(*src));
+}
+
+static int ipaccess_bcast(struct osmo_fd *fd, unsigned int what)
+{
+	uint8_t data[2048];
+	char src[INET_ADDRSTRLEN];
+	struct sockaddr_in addr = {};
+	socklen_t len = sizeof(addr);
+	int rc;
+
+	rc = recvfrom(fd->fd, data, sizeof(data), 0,
+			(struct sockaddr *) &addr, &len);
+	if (rc <= 0) {
+		LOGP(DFIND, LOGL_ERROR,
+			"Failed to read from socket errno(%d)\n", errno);
+		return -1;
+	}
+
+	LOGP(DFIND, LOGL_DEBUG,
+		"Received request from: %s size %d\n",
+		inet_ntop(AF_INET, &addr.sin_addr, src, sizeof(src)), rc);
+
+	if (rc < 6)
+		return 0;
+
+	if (data[2] != IPAC_PROTO_IPACCESS || data[4] != IPAC_MSGT_ID_GET)
+		return 0;
+
+	respond_to(&addr, fd, data + 6, rc - 6);
+	return 0;
+}
+
 int main(int argc, char **argv)
 {
+	struct osmo_fd fd;
 	void *tall_msgb_ctx;
 	int rc;
 
@@ -178,6 +297,23 @@ int main(int argc, char **argv)
 	if (rc < 0)
 		exit(2);
 
+	/* start temperature check timer */
+	temp_timer.cb = check_temp_timer_cb;
+	check_temp_timer_cb(NULL);
+
+	/* start operational hours timer */
+	hours_timer.cb = hours_timer_cb;
+	hours_timer_cb(NULL);
+
+	/* handle broadcast messages for ipaccess-find */
+	fd.cb = ipaccess_bcast;
+	rc = osmo_sock_init_ofd(&fd, AF_INET, SOCK_DGRAM, IPPROTO_UDP,
+				"0.0.0.0", 3006, OSMO_SOCK_F_BIND);
+	if (rc < 0) {
+		perror("Socket creation");
+		exit(3);
+	}
+
 	if (daemonize) {
 		rc = osmo_daemonize();
 		if (rc < 0) {
@@ -186,13 +322,6 @@ int main(int argc, char **argv)
 		}
 	}
 
-	/* start temperature check timer */
-	temp_timer.cb = check_temp_timer_cb;
-	check_temp_timer_cb(NULL);
-
-	/* start operational hours timer */
-	hours_timer.cb = hours_timer_cb;
-	hours_timer_cb(NULL);
 
 	while (1) {
 		log_reset_context();
diff --git a/src/osmo-bts-sysmo/misc/sysmobts_mgr.h b/src/osmo-bts-sysmo/misc/sysmobts_mgr.h
index 3b948b1c..ddb6774a 100644
--- a/src/osmo-bts-sysmo/misc/sysmobts_mgr.h
+++ b/src/osmo-bts-sysmo/misc/sysmobts_mgr.h
@@ -4,6 +4,7 @@
 enum {
 	DTEMP,
 	DFW,
+	DFIND,
 };
 
 #endif
diff --git a/src/osmo-bts-sysmo/misc/sysmobts_nl.c b/src/osmo-bts-sysmo/misc/sysmobts_nl.c
new file mode 100644
index 00000000..67aa6636
--- /dev/null
+++ b/src/osmo-bts-sysmo/misc/sysmobts_nl.c
@@ -0,0 +1,120 @@
+/* Helper for netlink */
+
+/*
+ * (C) 2014 by Holger Hans Peter Freyther
+ *
+ * All Rights Reserved
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU Affero General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * 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 Affero General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+#include <arpa/inet.h>
+#include <netinet/ip.h>
+
+#include <sys/socket.h>
+
+#include <linux/netlink.h>
+#include <linux/rtnetlink.h>
+
+#include <string.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <unistd.h>
+
+#define NLMSG_TAIL(nmsg) \
+        ((struct rtattr *) (((void *) (nmsg)) + NLMSG_ALIGN((nmsg)->nlmsg_len)))
+
+/**
+ * In case one binds to 0.0.0.0/INADDR_ANY and wants to know which source
+ * address will be used when sending a message this function can be used.
+ * It will ask the routing code of the kernel for the PREFSRC
+ */
+int source_for_dest(const struct in_addr *dest, struct in_addr *loc_source)
+{
+	int fd, rc;
+	struct rtmsg *r;
+	struct rtattr *rta;
+        struct {
+		struct nlmsghdr         n;
+		struct rtmsg            r;
+		char                    buf[1024];
+        } req;
+
+	memset(&req, 0, sizeof(req));
+
+	fd = socket(AF_NETLINK, SOCK_RAW|SOCK_CLOEXEC, NETLINK_ROUTE);
+	if (fd < 0) {
+		perror("nl socket");
+		return -1;
+	}
+
+	/* Send a rtmsg and ask for a response */
+        req.n.nlmsg_len = NLMSG_LENGTH(sizeof(struct rtmsg));
+        req.n.nlmsg_flags = NLM_F_REQUEST | NLM_F_ACK;
+        req.n.nlmsg_type = RTM_GETROUTE;
+        req.n.nlmsg_seq = 1;
+
+	/* Prepare the routing request */
+        req.r.rtm_family = AF_INET;
+
+	/* set the dest */
+	rta = NLMSG_TAIL(&req.n);
+	rta->rta_type = RTA_DST;
+	rta->rta_len = RTA_LENGTH(sizeof(*dest));
+	memcpy(RTA_DATA(rta), dest, sizeof(*dest));
+
+	/* update sizes for dest */
+	req.r.rtm_dst_len = sizeof(*dest) * 8;
+	req.n.nlmsg_len = NLMSG_ALIGN(req.n.nlmsg_len) + RTA_ALIGN(rta->rta_len);
+
+	rc = send(fd, &req, req.n.nlmsg_len, 0);
+	if (rc != req.n.nlmsg_len) {
+		perror("short write");
+		close(fd);
+		return -2;
+	}
+
+
+	/* now receive a response and parse it */
+	rc = recv(fd, &req, sizeof(req), 0);
+	if (rc <= 0) {
+		perror("short read");
+		close(fd);
+		return -3;
+	}
+
+	if (!NLMSG_OK(&req.n, rc) || req.n.nlmsg_type != RTM_NEWROUTE) {
+		close(fd);
+		return -4;
+	}
+
+	r = NLMSG_DATA(&req.n);
+	rc -= NLMSG_LENGTH(sizeof(*r));
+	rta = RTM_RTA(r);
+	while (RTA_OK(rta, rc)) {
+		if (rta->rta_type != RTA_PREFSRC) {
+			rta = RTA_NEXT(rta, rc);
+			continue;
+		}
+
+		/* we are done */
+		memcpy(loc_source, RTA_DATA(rta), RTA_PAYLOAD(rta));
+		close(fd);
+		return 0;
+	}
+
+	close(fd);
+	return -5;
+}
diff --git a/src/osmo-bts-sysmo/misc/sysmobts_nl.h b/src/osmo-bts-sysmo/misc/sysmobts_nl.h
new file mode 100644
index 00000000..84f4d9cc
--- /dev/null
+++ b/src/osmo-bts-sysmo/misc/sysmobts_nl.h
@@ -0,0 +1,24 @@
+/*
+ * (C) 2014 by Holger Hans Peter Freyther
+ *
+ * All Rights Reserved
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU Affero General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * 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 Affero General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+#pragma once
+
+struct in_addr;
+
+int source_for_dest(const struct in_addr *dest, struct in_addr *loc_source);