diff --git a/configure.ac b/configure.ac index 42a9f97c88..1f0f8597de 100644 --- a/configure.ac +++ b/configure.ac @@ -265,7 +265,7 @@ AC_CHECK_HEADERS(netinet/ip.h netinet/ip6.h netinet/ip_icmp.h net/route.h,,,[[ #endif ]]) -AC_CHECK_HEADERS(netinet/ip_icmp.h,,,[[ +AC_CHECK_HEADERS(netinet/ip_icmp.h netinet/icmp6.h,,,[[ #include #if HAVE_SYS_SOCKET_H #include diff --git a/lib/core/include/core.h.in b/lib/core/include/core.h.in index 755a840475..7db7d0ff21 100644 --- a/lib/core/include/core.h.in +++ b/lib/core/include/core.h.in @@ -144,6 +144,10 @@ #include #endif +#if HAVE_NETINET_ICMP6_H +#include +#endif + #if HAVE_NETINET_UDP_H #include #endif diff --git a/src/pgw/pgw_gtp_path.c b/src/pgw/pgw_gtp_path.c index f49ed27472..2f1a4c7ff6 100644 --- a/src/pgw/pgw_gtp_path.c +++ b/src/pgw/pgw_gtp_path.c @@ -11,39 +11,13 @@ #include "pgw_event.h" #include "pgw_gtp_path.h" -static status_t pgw_gtp_send_to_bearer(pgw_bearer_t *bearer, pkbuf_t *sendbuf) -{ - status_t rv; - gtp_header_t *gtp_h = NULL; +#define PGW_GTP_HANDLED 1 - /* Add GTP-U header */ - rv = pkbuf_header(sendbuf, GTPV1U_HEADER_LEN); - if (rv != CORE_OK) - { - d_error("pkbuf_header error"); - pkbuf_free(sendbuf); - return CORE_ERROR; - } - - gtp_h = (gtp_header_t *)sendbuf->payload; - /* Bits 8 7 6 5 4 3 2 1 - * +--+--+--+--+--+--+--+--+ - * |version |PT| 1| E| S|PN| - * +--+--+--+--+--+--+--+--+ - * 0 0 1 1 0 0 0 0 - */ - gtp_h->flags = 0x30; - gtp_h->type = GTPU_MSGTYPE_GPDU; - gtp_h->length = htons(sendbuf->len); - gtp_h->teid = htonl(bearer->sgw_s5u_teid); - - /* Send to SGW */ - d_trace(50, "Send S5U PDU (teid = 0x%x) to SGW\n", - bearer->sgw_s5u_teid); - rv = gtp_send(bearer->gnode, sendbuf); - - return rv; -} +c_uint16_t pgw_in_cksum(c_uint16_t *addr, int len); +static status_t pgw_gtp_handle_multicast(pkbuf_t *recvbuf); +static status_t pgw_gtp_handle_slacc(c_uint32_t teid, pkbuf_t *recvbuf); +static status_t pgw_gtp_send_to_bearer(pgw_bearer_t *bearer, pkbuf_t *sendbuf); +static status_t pgw_gtp_send_router_advertisement(pgw_sess_t *sess); static int _gtpv1_tun_recv_cb(sock_id sock, void *data) { @@ -73,41 +47,14 @@ static int _gtpv1_tun_recv_cb(sock_id sock, void *data) { /* Unicast */ rv = pgw_gtp_send_to_bearer(bearer, recvbuf); - d_assert(rv == CORE_OK,, "pgw_gtp_send_to_bearer failed"); + d_assert(rv == CORE_OK,, "pgw_gtp_send_to_bearer() failed"); } else { if (context_self()->parameter.multicast) { - struct ip *ip_h = NULL; - struct ip6_hdr *ip6_h = NULL; - - ip_h = (struct ip *)recvbuf->payload; - if (ip_h->ip_v == 6) - { - ip6_h = (struct ip6_hdr *)recvbuf->payload; - if (IN6_IS_ADDR_MULTICAST(&ip6_h->ip6_dst)) - { - hash_index_t *hi = NULL; - - /* IPv6 Multicast */ - for (hi = pgw_sess_first(); hi; hi = pgw_sess_next(hi)) - { - pgw_sess_t *sess = pgw_sess_this(hi); - d_assert(sess, return 0,); - if (sess->ipv6) - { - /* PDN IPv6 is avaiable */ - pgw_bearer_t *bearer = pgw_default_bearer_in_sess(sess); - d_assert(bearer, return 0,); - - rv = pgw_gtp_send_to_bearer(bearer, recvbuf); - d_assert(rv == CORE_OK,, - "pgw_gtp_send_to_bearer failed"); - } - } - } - } + rv = pgw_gtp_handle_multicast(recvbuf); + d_assert(rv != CORE_ERROR,, "pgw_gtp_handle_multicast() failed"); } } @@ -154,6 +101,7 @@ static int _gtpv1_u_recv_cb(sock_id sock, void *data) status_t rv; pkbuf_t *pkbuf = NULL; c_uint32_t size = GTPV1U_HEADER_LEN; + gtp_header_t *gtp_h = NULL; d_assert(sock, return -1, "Null param"); @@ -166,9 +114,15 @@ static int _gtpv1_u_recv_cb(sock_id sock, void *data) return -1; } + d_assert(pkbuf, return 0,); d_trace(50, "S5-U PDU received from SGW\n"); d_trace_hex(50, pkbuf->payload, pkbuf->len); + + d_assert(pkbuf->payload, return 0,); + gtp_h = pkbuf->payload; + if (gtp_h->flags & GTPU_FLAGS_S) size += 4; + /* Remove GTP header and send packets to TUN interface */ if (pkbuf_header(pkbuf, -size) != CORE_OK) { @@ -178,13 +132,24 @@ static int _gtpv1_u_recv_cb(sock_id sock, void *data) return -1; } + /* Check IPv6 */ + if (context_self()->parameter.no_slaac == 0) + { + rv = pgw_gtp_handle_slacc(ntohl(gtp_h->teid), pkbuf); + if (rv == PGW_GTP_HANDLED) + { + pkbuf_free(pkbuf); + return 0; + } + d_assert(rv == CORE_OK,, "pgw_gtp_handle_slacc() failed"); + } + if (sock_write(pgw_self()->tun_sock, pkbuf->payload, pkbuf->len) <= 0) { d_error("Can not send packets to tuntap"); } pkbuf_free(pkbuf); - return 0; } @@ -286,3 +251,222 @@ status_t pgw_gtp_close() return CORE_OK; } + +static status_t pgw_gtp_handle_multicast(pkbuf_t *recvbuf) +{ + status_t rv; + struct ip *ip_h = NULL; + struct ip6_hdr *ip6_h = NULL; + + ip_h = (struct ip *)recvbuf->payload; + if (ip_h->ip_v == 6) + { + ip6_h = (struct ip6_hdr *)recvbuf->payload; + if (IN6_IS_ADDR_MULTICAST(&ip6_h->ip6_dst)) + { + hash_index_t *hi = NULL; + + /* IPv6 Multicast */ + for (hi = pgw_sess_first(); hi; hi = pgw_sess_next(hi)) + { + pgw_sess_t *sess = pgw_sess_this(hi); + d_assert(sess, return CORE_ERROR,); + if (sess->ipv6) + { + /* PDN IPv6 is avaiable */ + pgw_bearer_t *bearer = pgw_default_bearer_in_sess(sess); + d_assert(bearer, return CORE_ERROR,); + + rv = pgw_gtp_send_to_bearer(bearer, recvbuf); + d_assert(rv == CORE_OK,, + "pgw_gtp_send_to_bearer failed"); + + return PGW_GTP_HANDLED; + } + } + } + } + + return CORE_OK; +} + +static status_t pgw_gtp_handle_slacc(c_uint32_t teid, pkbuf_t *recvbuf) +{ + status_t rv; + struct ip *ip_h = NULL; + + d_assert(recvbuf, return CORE_ERROR,); + d_assert(recvbuf->payload, return CORE_ERROR,); + ip_h = (struct ip *)recvbuf->payload; + if (ip_h->ip_v == 6) + { + struct ip6_hdr *ip6_h = (struct ip6_hdr *)recvbuf->payload; + if (ip6_h->ip6_nxt == IPPROTO_ICMPV6) + { + struct icmphdr *icmp_h = + (struct icmphdr *)(recvbuf->payload + sizeof(struct ip6_hdr)); + if (icmp_h->type == ND_ROUTER_SOLICIT) + { + pgw_bearer_t *bearer = NULL; + pgw_sess_t *sess = NULL; + + bearer = pgw_bearer_find_by_pgw_s5u_teid(teid); + d_assert(teid, return CORE_ERROR, + "cannot find teid = %d", teid); + sess = bearer->sess; + d_assert(sess, return CORE_ERROR,); + + if (sess->ipv6) + { + rv = pgw_gtp_send_router_advertisement(sess); + d_assert(rv == CORE_OK,,"send router advertisement failed"); + } + return PGW_GTP_HANDLED; + } + } + } + + return CORE_OK; +} + +static status_t pgw_gtp_send_to_bearer(pgw_bearer_t *bearer, pkbuf_t *sendbuf) +{ + status_t rv; + gtp_header_t *gtp_h = NULL; + + /* Add GTP-U header */ + rv = pkbuf_header(sendbuf, GTPV1U_HEADER_LEN); + if (rv != CORE_OK) + { + d_error("pkbuf_header error"); + pkbuf_free(sendbuf); + return CORE_ERROR; + } + + gtp_h = (gtp_header_t *)sendbuf->payload; + /* Bits 8 7 6 5 4 3 2 1 + * +--+--+--+--+--+--+--+--+ + * |version |PT| 1| E| S|PN| + * +--+--+--+--+--+--+--+--+ + * 0 0 1 1 0 0 0 0 + */ + gtp_h->flags = 0x30; + gtp_h->type = GTPU_MSGTYPE_GPDU; + gtp_h->length = htons(sendbuf->len); + gtp_h->teid = htonl(bearer->sgw_s5u_teid); + + /* Send to SGW */ + d_trace(50, "Send S5U PDU (teid = 0x%x) to SGW\n", + bearer->sgw_s5u_teid); + rv = gtp_send(bearer->gnode, sendbuf); + + return rv; +} + +static status_t pgw_gtp_send_router_advertisement(pgw_sess_t *sess) +{ + status_t rv; + pkbuf_t *pkbuf = NULL; + c_uint8_t *p = NULL; + pgw_bearer_t *bearer = NULL; + ipsubnet_t src_ipsub, dst_ipsub; + struct ip6_hdr *ip6_h = NULL; + c_uint16_t plen = 0; + c_uint8_t nxt = 0; + struct nd_router_advert *advert_h = NULL; + struct nd_opt_prefix_info *prefix = NULL; + + d_assert(sess, return CORE_ERROR,); + bearer = pgw_default_bearer_in_sess(sess); + d_assert(bearer, return CORE_ERROR,); + + pkbuf = pkbuf_alloc(GTPV1U_HEADER_LEN, 200); + d_assert(pkbuf, return CORE_ERROR,); + pkbuf->len = 88; + + rv = core_ipsubnet(&src_ipsub, "fe80::1", NULL); + d_assert(rv == CORE_OK, return CORE_ERROR,); + rv = core_ipsubnet(&dst_ipsub, "ff02::1", NULL); + d_assert(rv == CORE_OK, return CORE_ERROR,); + + advert_h = (struct nd_router_advert *)(pkbuf->payload + sizeof *ip6_h); + memset(advert_h, 0, sizeof *advert_h); + advert_h->nd_ra_type = ND_ROUTER_ADVERT; + advert_h->nd_ra_code = 0; + advert_h->nd_ra_curhoplimit = 64; + advert_h->nd_ra_flags_reserved = 0; + advert_h->nd_ra_router_lifetime = htons(64800); /* 64800s */ + advert_h->nd_ra_reachable = 0; + advert_h->nd_ra_retransmit = 0; + + prefix = (struct nd_opt_prefix_info *) + ((c_uint8_t*)advert_h + sizeof *advert_h); + memset(prefix, 0, sizeof *prefix); + prefix->nd_opt_pi_type = ND_OPT_PREFIX_INFORMATION; + prefix->nd_opt_pi_len = 4; /* 32bytes */ + prefix->nd_opt_pi_prefix_len = 64; + prefix->nd_opt_pi_flags_reserved = + ND_OPT_PI_FLAG_ONLINK|ND_OPT_PI_FLAG_AUTO; + prefix->nd_opt_pi_valid_time = htonl(0xffffffff); /* Infinite */ + prefix->nd_opt_pi_preferred_time = htonl(0xffffffff); /* Infinite */ + memcpy(prefix->nd_opt_pi_prefix.s6_addr, sess->ipv6->addr, IPV6_LEN/2); + + /* For IPv6 Pseudo-Header */ + plen = htons(48); + nxt = IPPROTO_ICMPV6; + + p = (c_uint8_t *)pkbuf->payload; + memset(p, 0, sizeof *ip6_h); + memcpy(p, src_ipsub.sub, sizeof src_ipsub.sub); + p += sizeof src_ipsub.sub; + memcpy(p, dst_ipsub.sub, sizeof dst_ipsub.sub); + p += sizeof dst_ipsub.sub; + p += 2; memcpy(p, &plen, 2); p += 2; + p += 3; *p = nxt; p += 1; + advert_h->nd_ra_cksum = pgw_in_cksum( + (c_uint16_t *)pkbuf->payload, pkbuf->len); + + ip6_h = (struct ip6_hdr *)pkbuf->payload; + memset(ip6_h, 0, sizeof *ip6_h); + ip6_h->ip6_flow = htonl(0x60000001); + ip6_h->ip6_plen = plen; + ip6_h->ip6_nxt = nxt; /* ICMPv6 */ + ip6_h->ip6_hlim = 0xff; + memcpy(ip6_h->ip6_src.s6_addr, src_ipsub.sub, sizeof src_ipsub.sub); + memcpy(ip6_h->ip6_dst.s6_addr, dst_ipsub.sub, sizeof dst_ipsub.sub); + + rv = pgw_gtp_send_to_bearer(bearer, pkbuf); + d_assert(rv == CORE_OK,, "pgw_gtp_send_to_bearer() faild"); + + pkbuf_free(pkbuf); + return rv; +} + +c_uint16_t pgw_in_cksum(c_uint16_t *addr, int len) +{ + int nleft = len; + c_uint32_t sum = 0; + c_uint16_t *w = addr; + c_uint16_t answer = 0; + + // Adding 16 bits sequentially in sum + while (nleft > 1) + { + sum += *w; + nleft -= 2; + w++; + } + + // If an odd byte is left + if (nleft == 1) + { + *(c_uint8_t *) (&answer) = *(c_uint8_t *) w; + sum += answer; + } + + sum = (sum >> 16) + (sum & 0xffff); + sum += (sum >> 16); + answer = ~sum; + + return answer; +} diff --git a/src/pgw/pgw_ipfw.c b/src/pgw/pgw_ipfw.c index b0be291e9f..dea43aa5f1 100644 --- a/src/pgw/pgw_ipfw.c +++ b/src/pgw/pgw_ipfw.c @@ -241,6 +241,7 @@ pgw_bearer_t*pgw_bearer_find_by_packet(pkbuf_t *pkt) char buf[CORE_ADDRSTRLEN]; d_assert(pkt, return NULL, "pkt is NULL"); + d_assert(pkt->payload, return NULL, "pkt is NULL"); ip_h = (struct ip *)pkt->payload; if (ip_h->ip_v == 4) diff --git a/test/attach_test.c b/test/attach_test.c index da43626400..7d3c83785c 100644 --- a/test/attach_test.c +++ b/test/attach_test.c @@ -301,6 +301,16 @@ static void attach_test1(abts_case *tc, void *data) core_sleep(time_from_msec(300)); + rv = testgtpu_build_slacc_rs(&sendbuf, 0); + ABTS_INT_EQUAL(tc, CORE_OK, rv); + rv = testgtpu_enb_send(sendbuf); + ABTS_INT_EQUAL(tc, CORE_OK, rv); + + recvbuf = pkbuf_alloc(0, MAX_SDU_LEN); + rv = testgtpu_enb_read(gtpu, recvbuf); + ABTS_INT_EQUAL(tc, CORE_OK, rv); + pkbuf_free(recvbuf); + /* Send GTP-U ICMP Packet */ rv = testgtpu_build_ping(&sendbuf, "45.45.0.2", "45.45.0.1"); ABTS_INT_EQUAL(tc, CORE_OK, rv); diff --git a/test/testpacket.c b/test/testpacket.c index 504eaf51e4..75528d705c 100644 --- a/test/testpacket.c +++ b/test/testpacket.c @@ -91,287 +91,6 @@ status_t tests1ap_enb_send(sock_id id, pkbuf_t *sendbuf) return s1ap_send(id, sendbuf, NULL); } -status_t testgtpu_enb_connect(sock_id *new) -{ - char buf[INET_ADDRSTRLEN]; - status_t rv; - c_sockaddr_t *addr = NULL; - int family = AF_UNSPEC; - - if (test_only_control_plane) return CORE_OK; - - family = AF_INET6; - if (context_self()->parameter.no_ipv6) family = AF_INET; - else if (context_self()->parameter.prefer_ipv4) family = AF_INET; - else if (test_enb_addr6 == NULL) family = AF_INET; - - rv = udp_socket(new, family); - d_assert(rv == CORE_OK, return CORE_ERROR,); - - if (family == AF_INET) addr = test_enb_addr; - else if (family == AF_INET6) addr = test_enb_addr6; - else - d_assert(0, return CORE_ERROR,); - - d_assert(addr, return CORE_ERROR,); - rv = sock_bind(*new, addr); - d_assert(rv == CORE_OK, return CORE_ERROR,); - - return CORE_OK; -} - -status_t testgtpu_enb_close(sock_id sock) -{ - if (test_only_control_plane) return CORE_OK; - - return sock_delete(sock); -} - -static uint16_t in_cksum(uint16_t *addr, int len) -{ - int nleft = len; - uint32_t sum = 0; - uint16_t *w = addr; - uint16_t answer = 0; - - // Adding 16 bits sequentially in sum - while (nleft > 1) { - sum += *w; - nleft -= 2; - w++; - } - - // If an odd byte is left - if (nleft == 1) { - *(unsigned char *) (&answer) = *(unsigned char *) w; - sum += answer; - } - - sum = (sum >> 16) + (sum & 0xffff); - sum += (sum >> 16); - answer = ~sum; - - return answer; -} - -status_t testgtpu_enb_send(pkbuf_t *sendbuf) -{ - status_t rv; - sock_id sock = 0; - hash_index_t *hi = NULL; - mme_ue_t *mme_ue = NULL; - mme_sess_t *sess = NULL; - mme_bearer_t *bearer = NULL; - - c_sockaddr_t sgw; - ssize_t sent; - - if (test_only_control_plane) return 0; - - hi = mme_ue_first(); - d_assert(hi, return -1,); - mme_ue = mme_ue_this(hi); - d_assert(mme_ue, return -1,); - sess = mme_sess_first(mme_ue); - d_assert(sess, return -1,); - bearer = mme_bearer_first(sess); - d_assert(bearer, return -1,); - - memset(&sgw, 0, sizeof(c_sockaddr_t)); - sgw.c_sa_port = htons(GTPV1_U_UDP_PORT); - if (bearer->sgw_s1u_ip.ipv6) - { - sgw.c_sa_family = AF_INET6; - if (bearer->sgw_s1u_ip.ipv4) - memcpy(sgw.sin6.sin6_addr.s6_addr, - bearer->sgw_s1u_ip.both.addr6, IPV6_LEN); - else - memcpy(sgw.sin6.sin6_addr.s6_addr, - bearer->sgw_s1u_ip.addr6, IPV6_LEN); - rv = sock_fill_scope_id_in_local(&sgw); - d_assert(rv == CORE_OK, return CORE_ERROR,); - } - else - { - sgw.c_sa_family = AF_INET; - sgw.sin.sin_addr.s_addr = bearer->sgw_s1u_ip.addr; - } - - rv = udp_client(&sock, &sgw); - d_assert(rv == CORE_OK, return CORE_ERROR,); - - sent = core_send(sock, sendbuf->payload, sendbuf->len, 0); - pkbuf_free(sendbuf); - if (sent < 0 || sent != sendbuf->len) - return CORE_ERROR; - - sock_delete(sock); - - return CORE_OK; -} - -status_t testgtpu_build_ping( - pkbuf_t **sendbuf, const char *src_ip, const char *dst_ip) -{ - status_t rv; - pkbuf_t *pkbuf = NULL; - gtp_header_t *gtp_h = NULL; - ipsubnet_t src_ipsub, dst_ipsub; - struct ip *ip_h = NULL; - struct icmphdr *icmp_h = NULL; - - struct ip6_hdr *ip6_h = NULL; - struct icmphdr *icmp6_h = NULL; - - if (test_only_control_plane) return CORE_OK; - - d_assert(src_ip, return CORE_ERROR,); - d_assert(dst_ip, return CORE_ERROR,); - rv = core_ipsubnet(&src_ipsub, src_ip, NULL); - d_assert(rv == CORE_OK, return CORE_ERROR,); - rv = core_ipsubnet(&dst_ipsub, dst_ip, NULL); - d_assert(rv == CORE_OK, return CORE_ERROR,); - - pkbuf = pkbuf_alloc(0, 200 /* enough for ICMP; use smaller buffer */); - d_assert(pkbuf, return CORE_ERROR,); - memset(pkbuf->payload, 0, pkbuf->len); - - gtp_h = (gtp_header_t *)pkbuf->payload; - gtp_h->flags = 0x30; - gtp_h->type = GTPU_MSGTYPE_GPDU; - gtp_h->teid = htonl(1); - - if (dst_ipsub.family == AF_INET) - { - gtp_h->length = htons(sizeof(struct ip) + sizeof(struct icmphdr)); - - ip_h = (struct ip *)(pkbuf->payload + GTPV1U_HEADER_LEN); - ip_h->ip_v = 4; - ip_h->ip_hl = 5; - ip_h->ip_tos = 0; - ip_h->ip_id = rand(); - ip_h->ip_off = 0; - ip_h->ip_ttl = 255; - ip_h->ip_p = IPPROTO_ICMP; - ip_h->ip_len = gtp_h->length; - ip_h->ip_src.s_addr = src_ipsub.sub[0]; - ip_h->ip_dst.s_addr = dst_ipsub.sub[0]; - ip_h->ip_sum = in_cksum( - (unsigned short *)ip_h, sizeof(struct ip)); - - icmp_h = (struct icmphdr *) - (pkbuf->payload + GTPV1U_HEADER_LEN + sizeof(struct ip)); - icmp_h->type = 8; - icmp_h->un.echo.sequence = rand(); - icmp_h->un.echo.id = rand(); - icmp_h->checksum = in_cksum( - (unsigned short *)icmp_h, sizeof(struct icmphdr)); - } - else if (dst_ipsub.family == AF_INET6) - { - char cksumbuf[200]; - char *ptr = NULL; - - int icmp6_datalen = 0; -#if 0 - int icmp6_datalen = 56; - char *icmp6_data = NULL; - char hexbuf[200]; - char *hexraw = - "9805325a 00000000 ea950900 00000000" - "10111213 14151617 18191a1b 1c1d1e1f" - "20212223 24252627 28292a2b 2c2d2e2f" - "30313233 34353637"; -#endif - - gtp_h->length = htons(sizeof(struct ip6_hdr) + - sizeof(struct icmphdr) + icmp6_datalen); - - ip6_h = (struct ip6_hdr *)(pkbuf->payload + GTPV1U_HEADER_LEN); - ip6_h->ip6_flow = htonl(0x600d5a92); - ip6_h->ip6_plen = htons(sizeof(struct icmphdr) + icmp6_datalen); - ip6_h->ip6_nxt = 58; /* ICMPv6 */ - ip6_h->ip6_hlim = 64; - memcpy(ip6_h->ip6_src.s6_addr, src_ipsub.sub, sizeof src_ipsub.sub); - memcpy(ip6_h->ip6_dst.s6_addr, dst_ipsub.sub, sizeof dst_ipsub.sub); - - icmp6_h = - (struct icmphdr *)((c_uint8_t*)ip6_h + sizeof(struct ip6_hdr)); - icmp6_h->type = 128; - icmp6_h->un.echo.sequence = rand(); - icmp6_h->un.echo.id = rand(); - -#if 0 - icmp6_data = (char *)((c_uint8_t*)icmp6_h + sizeof(struct icmphdr)); - memcpy(icmp6_data, - CORE_HEX(hexraw, strlen(hexraw), hexbuf), icmp6_datalen); -#endif - - /* create pseudo-header */ - memset(cksumbuf, 0, sizeof cksumbuf); - ptr = cksumbuf; - memcpy(ptr, src_ipsub.sub, sizeof src_ipsub.sub); - ptr += sizeof src_ipsub.sub; - memcpy(ptr, dst_ipsub.sub, sizeof dst_ipsub.sub); - ptr += sizeof dst_ipsub.sub; - - ptr += 2; - memcpy(ptr, &ip6_h->ip6_plen, 2); - ptr += 2; - - ptr += 3; - *ptr = ip6_h->ip6_nxt; - ptr += 1; - - memcpy(ptr, icmp6_h, sizeof(struct icmphdr)); -#if 0 - ptr += sizeof(struct icmphdr); - memcpy(ptr, icmp6_data, icmp6_datalen); -#endif - -#define IPV6_PSEUDO_HDR 48 - icmp6_h->checksum = in_cksum((unsigned short *)cksumbuf, - IPV6_PSEUDO_HDR + sizeof(struct icmphdr) + icmp6_datalen); - } - else - d_assert(0, return CORE_ERROR,); - - *sendbuf = pkbuf; - - return CORE_OK; -} - -status_t testgtpu_enb_read(sock_id sock, pkbuf_t *recvbuf) -{ - int rc = 0; - - if (test_only_control_plane) return 0; - - while(1) - { - rc = core_recv(sock, recvbuf->payload, recvbuf->len, 0); - if (rc == -2) - { - continue; - } - else if (rc <= 0) - { - if (errno == EAGAIN) - { - continue; - } - break; - } - else - { - break; - } - } - recvbuf->len = rc; - - return CORE_OK; -} - status_t tests1ap_build_setup_req( pkbuf_t **pkbuf, S1ap_ENB_ID_PR present, c_uint32_t enb_id) { @@ -1954,3 +1673,307 @@ status_t tests1ap_build_handover_cancel(pkbuf_t **pkbuf, int i) return CORE_OK; } + +status_t testgtpu_enb_connect(sock_id *new) +{ + char buf[INET_ADDRSTRLEN]; + status_t rv; + c_sockaddr_t *addr = NULL; + int family = AF_UNSPEC; + + if (test_only_control_plane) return CORE_OK; + + family = AF_INET6; + if (context_self()->parameter.no_ipv6) family = AF_INET; + else if (context_self()->parameter.prefer_ipv4) family = AF_INET; + else if (test_enb_addr6 == NULL) family = AF_INET; + + rv = udp_socket(new, family); + d_assert(rv == CORE_OK, return CORE_ERROR,); + + if (family == AF_INET) addr = test_enb_addr; + else if (family == AF_INET6) addr = test_enb_addr6; + else + d_assert(0, return CORE_ERROR,); + + d_assert(addr, return CORE_ERROR,); + rv = sock_bind(*new, addr); + d_assert(rv == CORE_OK, return CORE_ERROR,); + + return CORE_OK; +} + +status_t testgtpu_enb_close(sock_id sock) +{ + if (test_only_control_plane) return CORE_OK; + + return sock_delete(sock); +} + +static uint16_t in_cksum(uint16_t *addr, int len) +{ + int nleft = len; + uint32_t sum = 0; + uint16_t *w = addr; + uint16_t answer = 0; + + // Adding 16 bits sequentially in sum + while (nleft > 1) { + sum += *w; + nleft -= 2; + w++; + } + + // If an odd byte is left + if (nleft == 1) { + *(unsigned char *) (&answer) = *(unsigned char *) w; + sum += answer; + } + + sum = (sum >> 16) + (sum & 0xffff); + sum += (sum >> 16); + answer = ~sum; + + return answer; +} + +status_t testgtpu_enb_send(pkbuf_t *sendbuf) +{ + status_t rv; + sock_id sock = 0; + hash_index_t *hi = NULL; + mme_ue_t *mme_ue = NULL; + mme_sess_t *sess = NULL; + mme_bearer_t *bearer = NULL; + + c_sockaddr_t sgw; + ssize_t sent; + + if (test_only_control_plane) return 0; + + hi = mme_ue_first(); + d_assert(hi, return -1,); + mme_ue = mme_ue_this(hi); + d_assert(mme_ue, return -1,); + sess = mme_sess_first(mme_ue); + d_assert(sess, return -1,); + bearer = mme_bearer_first(sess); + d_assert(bearer, return -1,); + + memset(&sgw, 0, sizeof(c_sockaddr_t)); + sgw.c_sa_port = htons(GTPV1_U_UDP_PORT); + if (bearer->sgw_s1u_ip.ipv6) + { + sgw.c_sa_family = AF_INET6; + if (bearer->sgw_s1u_ip.ipv4) + memcpy(sgw.sin6.sin6_addr.s6_addr, + bearer->sgw_s1u_ip.both.addr6, IPV6_LEN); + else + memcpy(sgw.sin6.sin6_addr.s6_addr, + bearer->sgw_s1u_ip.addr6, IPV6_LEN); + rv = sock_fill_scope_id_in_local(&sgw); + d_assert(rv == CORE_OK, return CORE_ERROR,); + } + else + { + sgw.c_sa_family = AF_INET; + sgw.sin.sin_addr.s_addr = bearer->sgw_s1u_ip.addr; + } + + rv = udp_client(&sock, &sgw); + d_assert(rv == CORE_OK, return CORE_ERROR,); + + sent = core_send(sock, sendbuf->payload, sendbuf->len, 0); + pkbuf_free(sendbuf); + if (sent < 0 || sent != sendbuf->len) + return CORE_ERROR; + + sock_delete(sock); + + return CORE_OK; +} + +status_t testgtpu_enb_read(sock_id sock, pkbuf_t *recvbuf) +{ + int rc = 0; + + if (test_only_control_plane) return 0; + + while(1) + { + rc = core_recv(sock, recvbuf->payload, recvbuf->len, 0); + if (rc == -2) + { + continue; + } + else if (rc <= 0) + { + if (errno == EAGAIN) + { + continue; + } + break; + } + else + { + break; + } + } + recvbuf->len = rc; + + return CORE_OK; +} + +status_t testgtpu_build_ping( + pkbuf_t **sendbuf, const char *src_ip, const char *dst_ip) +{ + status_t rv; + pkbuf_t *pkbuf = NULL; + gtp_header_t *gtp_h = NULL; + ipsubnet_t src_ipsub, dst_ipsub; + struct ip *ip_h = NULL; + struct icmphdr *icmp_h = NULL; + + struct ip6_hdr *ip6_h = NULL; + struct icmphdr *icmp6_h = NULL; + + if (test_only_control_plane) return CORE_OK; + + d_assert(src_ip, return CORE_ERROR,); + d_assert(dst_ip, return CORE_ERROR,); + rv = core_ipsubnet(&src_ipsub, src_ip, NULL); + d_assert(rv == CORE_OK, return CORE_ERROR,); + rv = core_ipsubnet(&dst_ipsub, dst_ip, NULL); + d_assert(rv == CORE_OK, return CORE_ERROR,); + + pkbuf = pkbuf_alloc(0, 200 /* enough for ICMP; use smaller buffer */); + d_assert(pkbuf, return CORE_ERROR,); + memset(pkbuf->payload, 0, pkbuf->len); + + gtp_h = (gtp_header_t *)pkbuf->payload; + gtp_h->flags = 0x30; + gtp_h->type = GTPU_MSGTYPE_GPDU; + gtp_h->teid = htonl(1); + + if (dst_ipsub.family == AF_INET) + { + gtp_h->length = htons(sizeof(struct ip) + sizeof(struct icmphdr)); + + ip_h = (struct ip *)(pkbuf->payload + GTPV1U_HEADER_LEN); + ip_h->ip_v = 4; + ip_h->ip_hl = 5; + ip_h->ip_tos = 0; + ip_h->ip_id = rand(); + ip_h->ip_off = 0; + ip_h->ip_ttl = 255; + ip_h->ip_p = IPPROTO_ICMP; + ip_h->ip_len = gtp_h->length; + ip_h->ip_src.s_addr = src_ipsub.sub[0]; + ip_h->ip_dst.s_addr = dst_ipsub.sub[0]; + ip_h->ip_sum = in_cksum( + (unsigned short *)ip_h, sizeof(struct ip)); + + icmp_h = (struct icmphdr *) + (pkbuf->payload + GTPV1U_HEADER_LEN + sizeof(struct ip)); + icmp_h->type = 8; + icmp_h->un.echo.sequence = rand(); + icmp_h->un.echo.id = rand(); + icmp_h->checksum = in_cksum( + (unsigned short *)icmp_h, sizeof(struct icmphdr)); + } + else if (dst_ipsub.family == AF_INET6) + { + char cksumbuf[200]; + char *ptr = NULL; + + int icmp6_datalen = 0; +#if 0 + int icmp6_datalen = 56; + char *icmp6_data = NULL; + char hexbuf[200]; + char *hexraw = + "9805325a 00000000 ea950900 00000000" + "10111213 14151617 18191a1b 1c1d1e1f" + "20212223 24252627 28292a2b 2c2d2e2f" + "30313233 34353637"; +#endif + + gtp_h->length = htons(sizeof(struct ip6_hdr) + + sizeof(struct icmphdr) + icmp6_datalen); + + ip6_h = (struct ip6_hdr *)(pkbuf->payload + GTPV1U_HEADER_LEN); + ip6_h->ip6_flow = htonl(0x600d5a92); + ip6_h->ip6_plen = htons(sizeof(struct icmphdr) + icmp6_datalen); + ip6_h->ip6_nxt = 58; /* ICMPv6 */ + ip6_h->ip6_hlim = 64; + memcpy(ip6_h->ip6_src.s6_addr, src_ipsub.sub, sizeof src_ipsub.sub); + memcpy(ip6_h->ip6_dst.s6_addr, dst_ipsub.sub, sizeof dst_ipsub.sub); + + icmp6_h = + (struct icmphdr *)((c_uint8_t*)ip6_h + sizeof(struct ip6_hdr)); + icmp6_h->type = 128; + icmp6_h->un.echo.sequence = rand(); + icmp6_h->un.echo.id = rand(); + +#if 0 + icmp6_data = (char *)((c_uint8_t*)icmp6_h + sizeof(struct icmphdr)); + memcpy(icmp6_data, + CORE_HEX(hexraw, strlen(hexraw), hexbuf), icmp6_datalen); +#endif + + /* create pseudo-header */ + memset(cksumbuf, 0, sizeof cksumbuf); + ptr = cksumbuf; + memcpy(ptr, src_ipsub.sub, sizeof src_ipsub.sub); + ptr += sizeof src_ipsub.sub; + memcpy(ptr, dst_ipsub.sub, sizeof dst_ipsub.sub); + ptr += sizeof dst_ipsub.sub; + + ptr += 2; + memcpy(ptr, &ip6_h->ip6_plen, 2); + ptr += 2; + + ptr += 3; + *ptr = ip6_h->ip6_nxt; + ptr += 1; + + memcpy(ptr, icmp6_h, sizeof(struct icmphdr)); +#if 0 + ptr += sizeof(struct icmphdr); + memcpy(ptr, icmp6_data, icmp6_datalen); +#endif + +#define IPV6_PSEUDO_HDR 48 + icmp6_h->checksum = in_cksum((unsigned short *)cksumbuf, + IPV6_PSEUDO_HDR + sizeof(struct icmphdr) + icmp6_datalen); + } + else + d_assert(0, return CORE_ERROR,); + + *sendbuf = pkbuf; + + return CORE_OK; +} + +status_t testgtpu_build_slacc_rs(pkbuf_t **pkbuf, int i) +{ + char *payload[TESTS1AP_MAX_MESSAGE] = { + "32ff003400000001 00000000 6000000000083aff cafe000000000000 0000000000000001" + "ff02000000000000 0000000000000002 8500b0b800000000", + + }; + c_uint16_t len[TESTS1AP_MAX_MESSAGE] = { + 60, + }; + char hexbuf[MAX_SDU_LEN]; + + *pkbuf = pkbuf_alloc(0, MAX_SDU_LEN); + if (!(*pkbuf)) return CORE_ERROR; + + (*pkbuf)->len = len[i]; + memcpy((*pkbuf)->payload, CORE_HEX(payload[i], strlen(payload[i]), hexbuf), + (*pkbuf)->len); + + return CORE_OK; +} + diff --git a/test/testpacket.h b/test/testpacket.h index 669ed5b794..805fd5b006 100644 --- a/test/testpacket.h +++ b/test/testpacket.h @@ -18,13 +18,6 @@ CORE_DECLARE(status_t) tests1ap_enb_close(sock_id id); CORE_DECLARE(status_t) tests1ap_enb_send(sock_id id, pkbuf_t *sendbuf); CORE_DECLARE(status_t) tests1ap_enb_read(sock_id id, pkbuf_t *recvbuf); -CORE_DECLARE(status_t) testgtpu_enb_connect(sock_id *new); -CORE_DECLARE(status_t) testgtpu_enb_close(sock_id sock); -CORE_DECLARE(status_t) testgtpu_enb_read(sock_id sock, pkbuf_t *recvbuf); -CORE_DECLARE(status_t) testgtpu_enb_send(pkbuf_t *sendbuf); -CORE_DECLARE(status_t) testgtpu_build_ping(pkbuf_t **sendbuf, - const char *src_ip, const char *dst_ip); - CORE_DECLARE(status_t) tests1ap_build_setup_req( pkbuf_t **pkbuf, S1ap_ENB_ID_PR present, c_uint32_t enb_id); CORE_DECLARE(status_t) tests1ap_build_initial_ue_msg(pkbuf_t **pkbuf, int i); @@ -82,6 +75,15 @@ CORE_DECLARE(status_t) tests1ap_build_enb_status_transfer( CORE_DECLARE(status_t) tests1ap_build_handover_notify(pkbuf_t **pkbuf, int i); CORE_DECLARE(status_t) tests1ap_build_handover_cancel(pkbuf_t **pkbuf, int i); +CORE_DECLARE(status_t) testgtpu_enb_connect(sock_id *new); +CORE_DECLARE(status_t) testgtpu_enb_close(sock_id sock); +CORE_DECLARE(status_t) testgtpu_enb_read(sock_id sock, pkbuf_t *recvbuf); +CORE_DECLARE(status_t) testgtpu_enb_send(pkbuf_t *sendbuf); + +CORE_DECLARE(status_t) testgtpu_build_ping(pkbuf_t **sendbuf, + const char *src_ip, const char *dst_ip); +CORE_DECLARE(status_t) testgtpu_build_slacc_rs(pkbuf_t **sendbuf, int i); + #ifdef __cplusplus } #endif /* __cplusplus */