gsm0808: Add utils for AoIP Transport Layer Address

The planned support for true A over IP requires the encoding and
decoding of a so called "AoIP Transport Layer Address" element.

This commt adds parsing functionality and tests for the element
mentioned above, however, it is not yet actively used.

Change-Id: I57933b0a06a3f54ec2a41e6ecb6ced9fbbc89332

3 files changed, 125 insertions, 1 deletions

diff --git a/src/gsm/Makefile.am b/src/gsm/Makefile.am
index 3a4a0cd3..e64c9e79 100644
--- a/src/gsm/Makefile.am
+++ b/src/gsm/Makefile.am
@@ -25,7 +25,7 @@ libgsmint_la_SOURCES =  a5.c rxlev_stat.c tlv_parser.c comp128.c comp128v23.c \
 			auth_milenage.c milenage/aes-encblock.c gea.c \
 			milenage/aes-internal.c milenage/aes-internal-enc.c \
 			milenage/milenage.c gan.c ipa.c gsm0341.c apn.c \
-			gsup.c gprs_gea.c gsm0503_conv.c oap.c
+			gsup.c gprs_gea.c gsm0503_conv.c oap.c gsm0808_utils.c
 libgsmint_la_LDFLAGS = -no-undefined
 libgsmint_la_LIBADD = $(top_builddir)/src/libosmocore.la
 
diff --git a/src/gsm/gsm0808_utils.c b/src/gsm/gsm0808_utils.c
new file mode 100644
index 00000000..df24e2b8
--- /dev/null
+++ b/src/gsm/gsm0808_utils.c
@@ -0,0 +1,122 @@
+/* (C) 2016 by Sysmocom s.f.m.c. GmbH
+ * All Rights Reserved
+ *
+ * Author: Philipp Maier
+ *
+ * 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 Affero 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 <osmocom/core/utils.h>
+#include <osmocom/core/msgb.h>
+#include <string.h>
+#include <sys/socket.h>
+#include <netinet/in.h>
+#include <arpa/inet.h>
+#include <errno.h>
+#include <osmocom/gsm/protocol/gsm_08_08.h>
+
+#define IP_V4_ADDR_LEN 4
+#define IP_V6_ADDR_LEN 16
+#define IP_PORT_LEN 2
+
+/* Encode AoIP transport address element */
+uint8_t gsm0808_enc_aoip_trasp_addr(struct msgb *msg,
+				    const struct sockaddr_storage *ss)
+{
+	/* See also 3GPP TS 48.008 3.2.2.102 AoIP Transport Layer Address */
+	struct sockaddr_in *sin;
+	struct sockaddr_in6 *sin6;
+	uint16_t port = 0;
+	uint8_t *ptr;
+	uint8_t *old_tail;
+	uint8_t *tlv_len;
+
+	OSMO_ASSERT(msg);
+	OSMO_ASSERT(ss);
+	OSMO_ASSERT(ss->ss_family == AF_INET || ss->ss_family == AF_INET6);
+
+	msgb_put_u8(msg, GSM0808_IE_AOIP_TRASP_ADDR);
+	tlv_len = msgb_put(msg,1);
+	old_tail = msg->tail;
+
+	switch (ss->ss_family) {
+	case AF_INET:
+		sin = (struct sockaddr_in *)ss;
+		port = ntohs(sin->sin_port);
+		ptr = msgb_put(msg, IP_V4_ADDR_LEN);
+		memcpy(ptr, &sin->sin_addr.s_addr, IP_V4_ADDR_LEN);
+		break;
+	case AF_INET6:
+		sin6 = (struct sockaddr_in6 *)ss;
+		port = ntohs(sin6->sin6_port);
+		ptr = msgb_put(msg, IP_V6_ADDR_LEN);
+		memcpy(ptr, sin6->sin6_addr.s6_addr, IP_V6_ADDR_LEN);
+		break;
+	}
+
+	msgb_put_u16(msg, port);
+
+	*tlv_len = (uint8_t) (msg->tail - old_tail);
+	return *tlv_len + 2;
+}
+
+/* Decode AoIP transport address element */
+int gsm0808_dec_aoip_trasp_addr(struct sockaddr_storage *ss,
+				const uint8_t *elem, uint8_t len)
+{
+	/* See also 3GPP TS 48.008 3.2.2.102 AoIP Transport Layer Address */
+	struct sockaddr_in sin;
+	struct sockaddr_in6 sin6;
+	const uint8_t *old_elem = elem;
+
+	OSMO_ASSERT(ss);
+	if (!elem)
+		return -EINVAL;
+	if (len <= 0)
+		return -EINVAL;
+
+	memset(ss, 0, sizeof(*ss));
+
+	switch (len) {
+	case IP_V4_ADDR_LEN + IP_PORT_LEN:
+		memset(&sin, 0, sizeof(sin));
+		sin.sin_family = AF_INET;
+
+		memcpy(&sin.sin_addr.s_addr, elem, IP_V4_ADDR_LEN);
+		elem += IP_V4_ADDR_LEN;
+		sin.sin_port = osmo_load16le(elem);
+		elem += IP_PORT_LEN;
+
+		memcpy(ss, &sin, sizeof(sin));
+		break;
+	case IP_V6_ADDR_LEN + IP_PORT_LEN:
+		memset(&sin6, 0, sizeof(sin6));
+		sin6.sin6_family = AF_INET6;
+
+		memcpy(sin6.sin6_addr.s6_addr, elem, IP_V6_ADDR_LEN);
+		elem += IP_V6_ADDR_LEN;
+		sin6.sin6_port = osmo_load16le(elem);
+		elem += IP_PORT_LEN;
+
+		memcpy(ss, &sin6, sizeof(sin6));
+		break;
+	default:
+		/* Malformed element! */
+		return -EINVAL;
+		break;
+	}
+
+	return (int)(elem - old_elem);
+}
diff --git a/src/gsm/libosmogsm.map b/src/gsm/libosmogsm.map
index 5649e715..3ad847de 100644
--- a/src/gsm/libosmogsm.map
+++ b/src/gsm/libosmogsm.map
@@ -137,6 +137,8 @@ gsm0808_create_layer3;
 gsm0808_create_reset;
 gsm0808_create_sapi_reject;
 gsm0808_prepend_dtap_header;
+gsm0808_enc_aoip_trasp_addr;
+gsm0808_dec_aoip_trasp_addr;
 
 gsm0858_rsl_ul_meas_enc;