diff --git a/include/Makefile.am b/include/Makefile.am index e2a1b1278..0383d7ad0 100644 --- a/include/Makefile.am +++ b/include/Makefile.am @@ -77,6 +77,7 @@ nobase_include_HEADERS = \ osmocom/coding/gsm0503_interleaving.h \ osmocom/coding/gsm0503_coding.h \ osmocom/gsm/gsm0808.h \ + osmocom/gsm/gsm0808_utils.h \ osmocom/gsm/gsm23003.h \ osmocom/gsm/gsm48.h \ osmocom/gsm/gsm48_ie.h \ diff --git a/include/osmocom/gsm/gsm0808_utils.h b/include/osmocom/gsm/gsm0808_utils.h new file mode 100644 index 000000000..5fc1c8808 --- /dev/null +++ b/include/osmocom/gsm/gsm0808_utils.h @@ -0,0 +1,30 @@ +/* (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 . + * + */ +#pragma once + +#include + +/* Encode AoIP transport address element */ +uint8_t gsm0808_enc_aoip_trasp_addr(struct msgb *msg, + const struct sockaddr_storage *ss); + +/* Decode AoIP transport address element */ +int gsm0808_dec_aoip_trasp_addr(struct sockaddr_storage *ss, + const uint8_t *elem, uint8_t len); diff --git a/src/gsm/Makefile.am b/src/gsm/Makefile.am index 3a4a0cd3b..e64c9e799 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 000000000..df24e2b85 --- /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 . + * + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +#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 5649e715d..3ad847dea 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; diff --git a/tests/gsm0808/gsm0808_test.c b/tests/gsm0808/gsm0808_test.c index 98502b75e..26bd1d607 100644 --- a/tests/gsm0808/gsm0808_test.c +++ b/tests/gsm0808/gsm0808_test.c @@ -19,9 +19,14 @@ */ #include +#include +#include #include #include +#include +#include +#include #define VERIFY(msg, data, len) \ if (msgb_l3len(msg) != len) { \ @@ -247,6 +252,63 @@ static void test_prepend_dtap() msgb_free(in_msg); } +static void test_enc_dec_aoip_trasp_addr_v4() +{ + struct sockaddr_storage enc_addr; + struct sockaddr_storage dec_addr; + struct sockaddr_in enc_addr_in; + struct msgb *msg; + uint8_t rc_enc; + int rc_dec; + + memset(&enc_addr_in, 0, sizeof(enc_addr_in)); + enc_addr_in.sin_family = AF_INET; + enc_addr_in.sin_port = htons(1234); + inet_aton("255.0.255.255", &enc_addr_in.sin_addr); + + memset(&enc_addr, 0, sizeof(enc_addr)); + memcpy(&enc_addr, &enc_addr_in, sizeof(enc_addr_in)); + + msg = msgb_alloc(1024, "output buffer"); + rc_enc = gsm0808_enc_aoip_trasp_addr(msg, &enc_addr); + OSMO_ASSERT(rc_enc == 8); + rc_dec = + gsm0808_dec_aoip_trasp_addr(&dec_addr, msg->data + 2, msg->len - 2); + OSMO_ASSERT(rc_dec == 6); + OSMO_ASSERT(memcmp(&enc_addr, &dec_addr, sizeof(enc_addr)) == 0); + + msgb_free(msg); +} + +static void test_enc_dec_aoip_trasp_addr_v6() +{ + struct sockaddr_storage enc_addr; + struct sockaddr_storage dec_addr; + struct sockaddr_in6 enc_addr_in; + struct msgb *msg; + uint8_t rc_enc; + int rc_dec; + + memset(&enc_addr_in, 0, sizeof(enc_addr_in)); + enc_addr_in.sin6_family = AF_INET6; + enc_addr_in.sin6_port = htons(4567); + inet_pton(AF_INET6, "2001:0db8:85a3:08d3:1319:8a2e:0370:7344", + &enc_addr_in.sin6_addr); + + memset(&enc_addr, 0, sizeof(enc_addr)); + memcpy(&enc_addr, &enc_addr_in, sizeof(enc_addr_in)); + + msg = msgb_alloc(1024, "output buffer"); + rc_enc = gsm0808_enc_aoip_trasp_addr(msg, &enc_addr); + OSMO_ASSERT(rc_enc == 20); + rc_dec = + gsm0808_dec_aoip_trasp_addr(&dec_addr, msg->data + 2, msg->len - 2); + OSMO_ASSERT(rc_dec == 18); + OSMO_ASSERT(memcmp(&enc_addr, &dec_addr, sizeof(enc_addr)) == 0); + + msgb_free(msg); +} + int main(int argc, char **argv) { printf("Testing generation of GSM0808 messages\n"); @@ -263,6 +325,8 @@ int main(int argc, char **argv) test_create_clear_rqst(); test_create_dtap(); test_prepend_dtap(); + test_enc_dec_aoip_trasp_addr_v4(); + test_enc_dec_aoip_trasp_addr_v6(); printf("Done\n"); return EXIT_SUCCESS;