forked from acouzens/open5gs
refine code
This commit is contained in:
Sukchan Lee
parent
16080d9aa7
commit
0c3054652d
|
|
@ -13,7 +13,7 @@
|
|||
|
||||
#define PGW_GTP_HANDLED 1
|
||||
|
||||
c_uint16_t pgw_in_cksum(c_uint16_t *addr, int len);
|
||||
c_uint16_t 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);
|
||||
|
|
@ -367,12 +367,12 @@ 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;
|
||||
c_uint8_t *p = NULL;
|
||||
struct ip6_hdr *ip6_h = NULL;
|
||||
struct nd_router_advert *advert_h = NULL;
|
||||
struct nd_opt_prefix_info *prefix = NULL;
|
||||
|
||||
|
|
@ -382,15 +382,20 @@ static status_t pgw_gtp_send_router_advertisement(pgw_sess_t *sess)
|
|||
|
||||
pkbuf = pkbuf_alloc(GTPV1U_HEADER_LEN, 200);
|
||||
d_assert(pkbuf, return CORE_ERROR,);
|
||||
pkbuf->len = 88;
|
||||
pkbuf->len = sizeof *ip6_h + sizeof *advert_h + sizeof *prefix;
|
||||
memset(pkbuf->payload, 0, pkbuf->len);
|
||||
|
||||
p = (c_uint8_t *)pkbuf->payload;
|
||||
ip6_h = (struct ip6_hdr *)p;
|
||||
advert_h = (struct nd_router_advert *)((c_uint8_t *)ip6_h + sizeof *ip6_h);
|
||||
prefix = (struct nd_opt_prefix_info *)
|
||||
((c_uint8_t*)advert_h + sizeof *advert_h);
|
||||
|
||||
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;
|
||||
|
|
@ -399,35 +404,29 @@ static status_t pgw_gtp_send_router_advertisement(pgw_sess_t *sess)
|
|||
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_prefix_len = pgw_ue_ip_prefixlen(sess->ipv6);
|
||||
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);
|
||||
memcpy(prefix->nd_opt_pi_prefix.s6_addr,
|
||||
sess->ipv6->addr, prefix->nd_opt_pi_prefix_len);
|
||||
|
||||
/* For IPv6 Pseudo-Header */
|
||||
plen = htons(48);
|
||||
plen = htons(sizeof *advert_h + sizeof *prefix);
|
||||
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(
|
||||
advert_h->nd_ra_cksum = 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 */
|
||||
|
|
@ -442,7 +441,7 @@ static status_t pgw_gtp_send_router_advertisement(pgw_sess_t *sess)
|
|||
return rv;
|
||||
}
|
||||
|
||||
c_uint16_t pgw_in_cksum(c_uint16_t *addr, int len)
|
||||
c_uint16_t in_cksum(c_uint16_t *addr, int len)
|
||||
{
|
||||
int nleft = len;
|
||||
c_uint32_t sum = 0;
|
||||
|
|
|
|||
|
|
@ -1710,33 +1710,6 @@ status_t testgtpu_enb_close(sock_id sock)
|
|||
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;
|
||||
|
|
@ -1824,6 +1797,8 @@ status_t testgtpu_enb_read(sock_id sock, pkbuf_t *recvbuf)
|
|||
return CORE_OK;
|
||||
}
|
||||
|
||||
c_uint16_t in_cksum(c_uint16_t *addr, int len); /* from pgw_gtp_path.c */
|
||||
|
||||
status_t testgtpu_build_ping(
|
||||
pkbuf_t **sendbuf, const char *src_ip, const char *dst_ip)
|
||||
{
|
||||
|
|
@ -1831,11 +1806,6 @@ status_t testgtpu_build_ping(
|
|||
pkbuf_t *pkbuf = NULL;
|
||||
gtp_header_t *gtp_h = NULL;
|
||||
ipsubnet_t src_ipsub, dst_ipsub;
|
||||
struct ip *ip_h = NULL;
|
||||
struct icmp6_hdr *icmp_h = NULL;
|
||||
|
||||
struct ip6_hdr *ip6_h = NULL;
|
||||
struct icmp6_hdr *icmp6_h = NULL;
|
||||
|
||||
if (test_only_control_plane) return CORE_OK;
|
||||
|
||||
|
|
@ -1857,7 +1827,10 @@ status_t testgtpu_build_ping(
|
|||
|
||||
if (dst_ipsub.family == AF_INET)
|
||||
{
|
||||
gtp_h->length = htons(sizeof(struct ip) + sizeof(struct icmp6_hdr));
|
||||
struct ip *ip_h = NULL;
|
||||
struct icmp6_hdr *icmp_h = NULL;
|
||||
|
||||
gtp_h->length = htons(sizeof *ip_h + sizeof *icmp_h);
|
||||
|
||||
ip_h = (struct ip *)(pkbuf->payload + GTPV1U_HEADER_LEN);
|
||||
ip_h->ip_v = 4;
|
||||
|
|
@ -1870,82 +1843,50 @@ status_t testgtpu_build_ping(
|
|||
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));
|
||||
ip_h->ip_sum = in_cksum((c_uint16_t *)ip_h, sizeof *ip_h);
|
||||
|
||||
icmp_h = (struct icmp6_hdr *)
|
||||
(pkbuf->payload + GTPV1U_HEADER_LEN + sizeof(struct ip));
|
||||
icmp_h = (struct icmp6_hdr *)((c_uint8_t *)ip_h + sizeof *ip_h);
|
||||
icmp_h->icmp6_type = 8;
|
||||
icmp_h->icmp6_seq = rand();
|
||||
icmp_h->icmp6_id = rand();
|
||||
icmp_h->icmp6_cksum = in_cksum(
|
||||
(unsigned short *)icmp_h, sizeof(struct icmp6_hdr));
|
||||
icmp_h->icmp6_cksum = in_cksum((c_uint16_t *)icmp_h, sizeof *icmp_h);
|
||||
}
|
||||
else if (dst_ipsub.family == AF_INET6)
|
||||
{
|
||||
char cksumbuf[200];
|
||||
char *ptr = NULL;
|
||||
struct ip6_hdr *ip6_h = NULL;
|
||||
struct icmp6_hdr *icmp6_h = NULL;
|
||||
c_uint16_t plen = 0;
|
||||
c_uint8_t nxt = 0;
|
||||
c_uint8_t *p = 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 *ip6_h + sizeof *icmp6_h);
|
||||
plen = htons(sizeof *icmp6_h);
|
||||
nxt = IPPROTO_ICMPV6;
|
||||
|
||||
gtp_h->length = htons(sizeof(struct ip6_hdr) +
|
||||
sizeof(struct icmp6_hdr) + icmp6_datalen);
|
||||
p = (c_uint8_t *)pkbuf->payload + GTPV1U_HEADER_LEN;
|
||||
ip6_h = (struct ip6_hdr *)p;
|
||||
icmp6_h = (struct icmp6_hdr *)((c_uint8_t *)ip6_h + sizeof *ip6_h);
|
||||
|
||||
ip6_h = (struct ip6_hdr *)(pkbuf->payload + GTPV1U_HEADER_LEN);
|
||||
ip6_h->ip6_flow = htonl(0x600d5a92);
|
||||
ip6_h->ip6_plen = htons(sizeof(struct icmp6_hdr) + 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 icmp6_hdr *)((c_uint8_t*)ip6_h + sizeof(struct ip6_hdr));
|
||||
icmp6_h->icmp6_type = 128;
|
||||
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;
|
||||
|
||||
icmp6_h->icmp6_type = ICMP6_ECHO_REQUEST;
|
||||
icmp6_h->icmp6_seq = rand();
|
||||
icmp6_h->icmp6_id = rand();
|
||||
|
||||
#if 0
|
||||
icmp6_data = (char *)((c_uint8_t*)icmp6_h + sizeof(struct icmp6_hdr));
|
||||
memcpy(icmp6_data,
|
||||
CORE_HEX(hexraw, strlen(hexraw), hexbuf), icmp6_datalen);
|
||||
#endif
|
||||
icmp6_h->icmp6_cksum = in_cksum(
|
||||
(c_uint16_t *)ip6_h, sizeof *ip6_h + sizeof *icmp6_h);
|
||||
|
||||
/* 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 icmp6_hdr));
|
||||
#if 0
|
||||
ptr += sizeof(struct icmp6_hdr);
|
||||
memcpy(ptr, icmp6_data, icmp6_datalen);
|
||||
#endif
|
||||
|
||||
#define IPV6_PSEUDO_HDR 48
|
||||
icmp6_h->icmp6_cksum = in_cksum((unsigned short *)cksumbuf,
|
||||
IPV6_PSEUDO_HDR + sizeof(struct icmp6_hdr) + icmp6_datalen);
|
||||
ip6_h->ip6_flow = htonl(0x60000001);
|
||||
ip6_h->ip6_plen = plen;
|
||||
ip6_h->ip6_nxt = nxt;;
|
||||
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);
|
||||
}
|
||||
else
|
||||
d_assert(0, return CORE_ERROR,);
|
||||
|
|
|
|||
Loading…
Reference in New Issue