/* Send RST reply */
static void send_reset(struct sk_buff *oldskb, int hook)
{
struct sk_buff *nskb;
const struct iphdr *oiph;
struct iphdr *niph;
const struct tcphdr *oth;
struct tcphdr _otcph, *tcph;
/* IP header checks: fragment. */
if (ip_hdr(oldskb)->frag_off & htons(IP_OFFSET))
return;
oth = skb_header_pointer(oldskb, ip_hdrlen(oldskb),
sizeof(_otcph), &_otcph);
if (oth == NULL)
return;
/* No RST for RST. */
if (oth->rst)
return;
if (skb_rtable(oldskb)->rt_flags & (RTCF_BROADCAST | RTCF_MULTICAST))
return;
/* Check checksum */
if (nf_ip_checksum(oldskb, hook, ip_hdrlen(oldskb), IPPROTO_TCP))
return;
oiph = ip_hdr(oldskb);
nskb = alloc_skb(sizeof(struct iphdr) + sizeof(struct tcphdr) +
LL_MAX_HEADER, GFP_ATOMIC);
if (!nskb)
return;
skb_reserve(nskb, LL_MAX_HEADER);
skb_reset_network_header(nskb);
niph = (struct iphdr *)skb_put(nskb, sizeof(struct iphdr));
niph->version = 4;
niph->ihl = sizeof(struct iphdr) / 4;
niph->tos = 0;
niph->id = 0;
niph->frag_off = htons(IP_DF);
niph->protocol = IPPROTO_TCP;
niph->check = 0;
niph->saddr = oiph->daddr;
niph->daddr = oiph->saddr;
skb_reset_transport_header(nskb);
tcph = (struct tcphdr *)skb_put(nskb, sizeof(struct tcphdr));
memset(tcph, 0, sizeof(*tcph));
tcph->source = oth->dest;
tcph->dest = oth->source;
tcph->doff = sizeof(struct tcphdr) / 4;
if (oth->ack)
tcph->seq = oth->ack_seq;
else {
tcph->ack_seq = htonl(ntohl(oth->seq) + oth->syn + oth->fin +
oldskb->len - ip_hdrlen(oldskb) -
(oth->doff << 2));
tcph->ack = 1;
}
tcph->rst = 1;
tcph->check = ~tcp_v4_check(sizeof(struct tcphdr), niph->saddr,
niph->daddr, 0);
nskb->ip_summed = CHECKSUM_PARTIAL;
nskb->csum_start = (unsigned char *)tcph - nskb->head;
nskb->csum_offset = offsetof(struct tcphdr, check);
/* ip_route_me_harder expects skb->dst to be set */
skb_dst_set_noref(nskb, skb_dst(oldskb));
nskb->protocol = htons(ETH_P_IP);
if (ip_route_me_harder(nskb, RTN_UNSPEC))
goto free_nskb;
niph->ttl = ip4_dst_hoplimit(skb_dst(nskb));
/* "Never happens" */
if (nskb->len > dst_mtu(skb_dst(nskb)))
goto free_nskb;
nf_ct_attach(nskb, oldskb);
ip_local_out(nskb);
return;
free_nskb:
kfree_skb(nskb);
}
void ip_tunnel_xmit(struct sk_buff *skb, struct net_device *dev,
const struct iphdr *tnl_params, u8 protocol)
{
struct ip_tunnel *tunnel = netdev_priv(dev);
const struct iphdr *inner_iph;
struct flowi4 fl4;
u8 tos, ttl;
__be16 df;
struct rtable *rt; /* Route to the other host */
unsigned int max_headroom; /* The extra header space needed */
__be32 dst;
int err;
bool connected;
inner_iph = (const struct iphdr *)skb_inner_network_header(skb);
connected = (tunnel->parms.iph.daddr != 0);
dst = tnl_params->daddr;
if (dst == 0) {
/* NBMA tunnel */
if (!skb_dst(skb)) {
dev->stats.tx_fifo_errors++;
goto tx_error;
}
if (skb->protocol == htons(ETH_P_IP)) {
rt = skb_rtable(skb);
dst = rt_nexthop(rt, inner_iph->daddr);
}
#if IS_ENABLED(CONFIG_IPV6)
else if (skb->protocol == htons(ETH_P_IPV6)) {
const struct in6_addr *addr6;
struct neighbour *neigh;
bool do_tx_error_icmp;
int addr_type;
neigh = dst_neigh_lookup(skb_dst(skb),
&ipv6_hdr(skb)->daddr);
if (!neigh)
goto tx_error;
addr6 = (const struct in6_addr *)&neigh->primary_key;
addr_type = ipv6_addr_type(addr6);
if (addr_type == IPV6_ADDR_ANY) {
addr6 = &ipv6_hdr(skb)->daddr;
addr_type = ipv6_addr_type(addr6);
}
if ((addr_type & IPV6_ADDR_COMPATv4) == 0)
do_tx_error_icmp = true;
else {
do_tx_error_icmp = false;
dst = addr6->s6_addr32[3];
}
neigh_release(neigh);
if (do_tx_error_icmp)
goto tx_error_icmp;
}
#endif
else
goto tx_error;
connected = false;
}
tos = tnl_params->tos;
if (tos & 0x1) {
tos &= ~0x1;
if (skb->protocol == htons(ETH_P_IP)) {
tos = inner_iph->tos;
connected = false;
} else if (skb->protocol == htons(ETH_P_IPV6)) {
tos = ipv6_get_dsfield((const struct ipv6hdr *)inner_iph);
connected = false;
}
}
init_tunnel_flow(&fl4, protocol, dst, tnl_params->saddr,
tunnel->parms.o_key, RT_TOS(tos), tunnel->parms.link);
if (ip_tunnel_encap(skb, tunnel, &protocol, &fl4) < 0)
goto tx_error;
rt = connected ? tunnel_rtable_get(tunnel, 0, &fl4.saddr) : NULL;
if (!rt) {
rt = ip_route_output_key(tunnel->net, &fl4);
if (IS_ERR(rt)) {
dev->stats.tx_carrier_errors++;
goto tx_error;
}
if (connected)
tunnel_dst_set(tunnel, &rt->dst, fl4.saddr);
}
if (rt->dst.dev == dev) {
ip_rt_put(rt);
dev->stats.collisions++;
goto tx_error;
}
if (tnl_update_pmtu(dev, skb, rt, tnl_params->frag_off, inner_iph)) {
ip_rt_put(rt);
goto tx_error;
}
if (tunnel->err_count > 0) {
if (time_before(jiffies,
tunnel->err_time + IPTUNNEL_ERR_TIMEO)) {
tunnel->err_count--;
memset(IPCB(skb), 0, sizeof(*IPCB(skb)));
dst_link_failure(skb);
} else
tunnel->err_count = 0;
}
tos = ip_tunnel_ecn_encap(tos, inner_iph, skb);
ttl = tnl_params->ttl;
if (ttl == 0) {
if (skb->protocol == htons(ETH_P_IP))
ttl = inner_iph->ttl;
#if IS_ENABLED(CONFIG_IPV6)
else if (skb->protocol == htons(ETH_P_IPV6))
ttl = ((const struct ipv6hdr *)inner_iph)->hop_limit;
#endif
else
ttl = ip4_dst_hoplimit(&rt->dst);
}
df = tnl_params->frag_off;
if (skb->protocol == htons(ETH_P_IP))
df |= (inner_iph->frag_off&htons(IP_DF));
max_headroom = LL_RESERVED_SPACE(rt->dst.dev) + sizeof(struct iphdr)
+ rt->dst.header_len + ip_encap_hlen(&tunnel->encap);
if (max_headroom > dev->needed_headroom)
dev->needed_headroom = max_headroom;
if (skb_cow_head(skb, dev->needed_headroom)) {
ip_rt_put(rt);
dev->stats.tx_dropped++;
kfree_skb(skb);
return;
}
err = iptunnel_xmit(NULL, rt, skb, fl4.saddr, fl4.daddr, protocol,
tos, ttl, df, !net_eq(tunnel->net, dev_net(dev)));
iptunnel_xmit_stats(err, &dev->stats, dev->tstats);
return;
#if IS_ENABLED(CONFIG_IPV6)
tx_error_icmp:
dst_link_failure(skb);
#endif
tx_error:
dev->stats.tx_errors++;
kfree_skb(skb);
}
static unsigned int
echo_tg4(struct sk_buff *oldskb, const struct xt_action_param *par)
{
const struct udphdr *oldudp;
const struct iphdr *oldip;
struct udphdr *newudp, oldudp_buf;
struct iphdr *newip;
struct sk_buff *newskb;
unsigned int data_len;
void *payload;
/* This allows us to do the copy operation in fewer lines of code. */
if (skb_linearize(oldskb) < 0)
return NF_DROP;
oldip = ip_hdr(oldskb);
oldudp = skb_header_pointer(oldskb, par->thoff,
sizeof(*oldudp), &oldudp_buf);
if (oldudp == NULL)
return NF_DROP;
if (ntohs(oldudp->len) <= sizeof(*oldudp))
return NF_DROP;
newskb = alloc_skb(LL_MAX_HEADER + sizeof(*newip) +
ntohs(oldudp->len), GFP_ATOMIC);
if (newskb == NULL)
return NF_DROP;
skb_reserve(newskb, LL_MAX_HEADER);
newskb->protocol = oldskb->protocol;
skb_reset_network_header(newskb);
newip = (void *)skb_put(newskb, sizeof(*newip));
newip->version = oldip->version;
newip->ihl = sizeof(*newip) / 4;
newip->tos = oldip->tos;
newip->id = 0;
newip->frag_off = htons(IP_DF);
newip->protocol = oldip->protocol;
newip->check = 0;
newip->saddr = oldip->daddr;
newip->daddr = oldip->saddr;
skb_reset_transport_header(newskb);
newudp = (void *)skb_put(newskb, sizeof(*newudp));
newudp->source = oldudp->dest;
newudp->dest = oldudp->source;
newudp->len = oldudp->len;
data_len = htons(oldudp->len) - sizeof(*oldudp);
payload = skb_header_pointer(oldskb, par->thoff +
sizeof(*oldudp), data_len, NULL);
memcpy(skb_put(newskb, data_len), payload, data_len);
#if 0
/*
* Since no fields are modified (we just swapped things around),
* this works too in our specific echo case.
*/
newudp->check = oldudp->check;
#else
newudp->check = 0;
newudp->check = csum_tcpudp_magic(newip->saddr, newip->daddr,
ntohs(newudp->len), IPPROTO_UDP,
csum_partial(newudp, ntohs(newudp->len), 0));
#endif
/* ip_route_me_harder expects the skb's dst to be set */
skb_dst_set(newskb, dst_clone(skb_dst(oldskb)));
if (ip_route_me_harder(newskb, RTN_UNSPEC) != 0)
goto free_nskb;
newip->ttl = ip4_dst_hoplimit(skb_dst(newskb));
newskb->ip_summed = CHECKSUM_NONE;
/* "Never happens" (?) */
if (newskb->len > dst_mtu(skb_dst(newskb)))
goto free_nskb;
nf_ct_attach(newskb, oldskb);
ip_local_out(newskb);
return NF_DROP;
free_nskb:
kfree_skb(newskb);
return NF_DROP;
}
static void tarpit_tcp4(struct net *net, struct sk_buff *oldskb,
unsigned int hook, unsigned int mode)
{
struct tcphdr _otcph, *tcph;
const struct tcphdr *oth;
unsigned int addr_type = RTN_UNSPEC;
struct sk_buff *nskb;
const struct iphdr *oldhdr;
struct iphdr *niph;
uint16_t tmp, payload;
/* A truncated TCP header is not going to be useful */
if (oldskb->len < ip_hdrlen(oldskb) + sizeof(struct tcphdr))
return;
oth = skb_header_pointer(oldskb, ip_hdrlen(oldskb),
sizeof(_otcph), &_otcph);
if (oth == NULL)
return;
/* Check checksum. */
if (nf_ip_checksum(oldskb, hook, ip_hdrlen(oldskb), IPPROTO_TCP))
return;
/*
* Copy skb (even if skb is about to be dropped, we cannot just
* clone it because there may be other things, such as tcpdump,
* interested in it)
*/
nskb = skb_copy_expand(oldskb, LL_MAX_HEADER,
skb_tailroom(oldskb), GFP_ATOMIC);
if (nskb == NULL)
return;
/* This packet will not be the same as the other: clear nf fields */
nf_reset(nskb);
skb_nfmark(nskb) = 0;
skb_init_secmark(nskb);
skb_shinfo(nskb)->gso_size = 0;
skb_shinfo(nskb)->gso_segs = 0;
skb_shinfo(nskb)->gso_type = 0;
oldhdr = ip_hdr(oldskb);
tcph = (struct tcphdr *)(skb_network_header(nskb) + ip_hdrlen(nskb));
/* Swap source and dest */
niph = ip_hdr(nskb);
niph->daddr = xchg(&niph->saddr, niph->daddr);
tmp = tcph->source;
tcph->source = tcph->dest;
tcph->dest = tmp;
/* Calculate payload size?? */
payload = nskb->len - ip_hdrlen(nskb) - sizeof(struct tcphdr);
/* Truncate to length (no data) */
tcph->doff = sizeof(struct tcphdr) / 4;
skb_trim(nskb, ip_hdrlen(nskb) + sizeof(struct tcphdr));
niph->tot_len = htons(nskb->len);
tcph->urg_ptr = 0;
/* Reset flags */
((u_int8_t *)tcph)[13] = 0;
if (!tarpit_generic(tcph, oth, payload, mode))
goto free_nskb;
/* Adjust TCP checksum */
tcph->check = 0;
tcph->check = tcp_v4_check(sizeof(struct tcphdr), niph->saddr,
niph->daddr, csum_partial((char *)tcph,
sizeof(struct tcphdr), 0));
/* Set DF, id = 0 */
niph->frag_off = htons(IP_DF);
if (mode == XTTARPIT_TARPIT || mode == XTTARPIT_RESET)
niph->id = 0;
else if (mode == XTTARPIT_HONEYPOT)
niph->id = ~oldhdr->id + 1;
#ifdef CONFIG_BRIDGE_NETFILTER
if (hook != NF_INET_FORWARD || (nskb->nf_bridge != NULL &&
nskb->nf_bridge->physoutdev != NULL))
#else
if (hook != NF_INET_FORWARD)
#endif
addr_type = RTN_LOCAL;
if (ip_route_me_harder(net, nskb, addr_type))
goto free_nskb;
else
niph = ip_hdr(nskb);
nskb->ip_summed = CHECKSUM_NONE;
/* Adjust IP TTL */
if (mode == XTTARPIT_HONEYPOT)
niph->ttl = 128;
else
niph->ttl = ip4_dst_hoplimit(skb_dst(nskb));
/* Adjust IP checksum */
niph->check = 0;
niph->check = ip_fast_csum(skb_network_header(nskb), niph->ihl);
/* "Never happens" */
if (nskb->len > dst_mtu(skb_dst(nskb)))
goto free_nskb;
nf_ct_attach(nskb, oldskb);
NF_HOOK(NFPROTO_IPV4, NF_INET_LOCAL_OUT, net, nskb->sk, nskb, NULL,
skb_dst(nskb)->dev, dst_output);
return;
free_nskb:
kfree_skb(nskb);
}
/* Send RST reply */
void nf_send_reset(struct net *net, struct sk_buff *oldskb, int hook)
{
struct sk_buff *nskb;
const struct iphdr *oiph;
struct iphdr *niph;
const struct tcphdr *oth;
struct tcphdr _oth;
oth = nf_reject_ip_tcphdr_get(oldskb, &_oth, hook);
if (!oth)
return;
if (skb_rtable(oldskb)->rt_flags & (RTCF_BROADCAST | RTCF_MULTICAST))
return;
oiph = ip_hdr(oldskb);
nskb = alloc_skb(sizeof(struct iphdr) + sizeof(struct tcphdr) +
LL_MAX_HEADER, GFP_ATOMIC);
if (!nskb)
return;
/* ip_route_me_harder expects skb->dst to be set */
skb_dst_set_noref(nskb, skb_dst(oldskb));
skb_reserve(nskb, LL_MAX_HEADER);
niph = nf_reject_iphdr_put(nskb, oldskb, IPPROTO_TCP,
ip4_dst_hoplimit(skb_dst(nskb)));
nf_reject_ip_tcphdr_put(nskb, oldskb, oth);
if (ip_route_me_harder(net, nskb, RTN_UNSPEC))
goto free_nskb;
/* "Never happens" */
if (nskb->len > dst_mtu(skb_dst(nskb)))
goto free_nskb;
nf_ct_attach(nskb, oldskb);
#if IS_ENABLED(CONFIG_BRIDGE_NETFILTER)
/* If we use ip_local_out for bridged traffic, the MAC source on
* the RST will be ours, instead of the destination's. This confuses
* some routers/firewalls, and they drop the packet. So we need to
* build the eth header using the original destination's MAC as the
* source, and send the RST packet directly.
*/
if (oldskb->nf_bridge) {
struct ethhdr *oeth = eth_hdr(oldskb);
nskb->dev = nf_bridge_get_physindev(oldskb);
niph->tot_len = htons(nskb->len);
ip_send_check(niph);
if (dev_hard_header(nskb, nskb->dev, ntohs(nskb->protocol),
oeth->h_source, oeth->h_dest, nskb->len) < 0)
goto free_nskb;
dev_queue_xmit(nskb);
} else
#endif
ip_local_out(net, nskb->sk, nskb);
return;
free_nskb:
kfree_skb(nskb);
}
/* tipc_send_msg - enqueue a send request */
static int tipc_udp_send_msg(struct net *net, struct sk_buff *skb,
struct tipc_bearer *b,
struct tipc_media_addr *dest)
{
int ttl, err = 0;
struct udp_bearer *ub;
struct udp_media_addr *dst = (struct udp_media_addr *)&dest->value;
struct udp_media_addr *src = (struct udp_media_addr *)&b->addr.value;
struct rtable *rt;
if (skb_headroom(skb) < UDP_MIN_HEADROOM) {
err = pskb_expand_head(skb, UDP_MIN_HEADROOM, 0, GFP_ATOMIC);
if (err)
goto tx_error;
}
skb_set_inner_protocol(skb, htons(ETH_P_TIPC));
ub = rcu_dereference_rtnl(b->media_ptr);
if (!ub) {
err = -ENODEV;
goto tx_error;
}
if (dst->proto == htons(ETH_P_IP)) {
struct flowi4 fl = {
.daddr = dst->ipv4.s_addr,
.saddr = src->ipv4.s_addr,
.flowi4_mark = skb->mark,
.flowi4_proto = IPPROTO_UDP
};
rt = ip_route_output_key(net, &fl);
if (IS_ERR(rt)) {
err = PTR_ERR(rt);
goto tx_error;
}
ttl = ip4_dst_hoplimit(&rt->dst);
err = udp_tunnel_xmit_skb(rt, ub->ubsock->sk, skb,
src->ipv4.s_addr,
dst->ipv4.s_addr, 0, ttl, 0,
src->udp_port, dst->udp_port,
false, true);
if (err < 0) {
ip_rt_put(rt);
goto tx_error;
}
#if IS_ENABLED(CONFIG_IPV6)
} else {
struct dst_entry *ndst;
struct flowi6 fl6 = {
.flowi6_oif = ub->ifindex,
.daddr = dst->ipv6,
.saddr = src->ipv6,
.flowi6_proto = IPPROTO_UDP
};
err = ipv6_stub->ipv6_dst_lookup(net, ub->ubsock->sk, &ndst,
&fl6);
if (err)
goto tx_error;
ttl = ip6_dst_hoplimit(ndst);
err = udp_tunnel6_xmit_skb(ndst, ub->ubsock->sk, skb,
ndst->dev, &src->ipv6,
&dst->ipv6, 0, ttl, src->udp_port,
dst->udp_port, false);
#endif
}
return err;
tx_error:
kfree_skb(skb);
return err;
}
/* tipc_udp_recv - read data from bearer socket */
static int tipc_udp_recv(struct sock *sk, struct sk_buff *skb)
{
struct udp_bearer *ub;
struct tipc_bearer *b;
ub = rcu_dereference_sk_user_data(sk);
if (!ub) {
pr_err_ratelimited("Failed to get UDP bearer reference");
kfree_skb(skb);
return 0;
}
skb_pull(skb, sizeof(struct udphdr));
rcu_read_lock();
b = rcu_dereference_rtnl(ub->bearer);
if (b) {
tipc_rcv(sock_net(sk), skb, b);
rcu_read_unlock();
return 0;
}
rcu_read_unlock();
kfree_skb(skb);
return 0;
}
static int enable_mcast(struct udp_bearer *ub, struct udp_media_addr *remote)
{
int err = 0;
struct ip_mreqn mreqn;
struct sock *sk = ub->ubsock->sk;
if (ntohs(remote->proto) == ETH_P_IP) {
if (!ipv4_is_multicast(remote->ipv4.s_addr))
return 0;
mreqn.imr_multiaddr = remote->ipv4;
mreqn.imr_ifindex = ub->ifindex;
err = ip_mc_join_group(sk, &mreqn);
#if IS_ENABLED(CONFIG_IPV6)
} else {
if (!ipv6_addr_is_multicast(&remote->ipv6))
return 0;
err = ipv6_stub->ipv6_sock_mc_join(sk, ub->ifindex,
&remote->ipv6);
#endif
}
return err;
}
void ip_md_tunnel_xmit(struct sk_buff *skb, struct net_device *dev, u8 proto)
{
struct ip_tunnel *tunnel = netdev_priv(dev);
u32 headroom = sizeof(struct iphdr);
struct ip_tunnel_info *tun_info;
const struct ip_tunnel_key *key;
const struct iphdr *inner_iph;
struct rtable *rt;
struct flowi4 fl4;
__be16 df = 0;
u8 tos, ttl;
tun_info = skb_tunnel_info(skb);
if (unlikely(!tun_info || !(tun_info->mode & IP_TUNNEL_INFO_TX) ||
ip_tunnel_info_af(tun_info) != AF_INET))
goto tx_error;
key = &tun_info->key;
memset(&(IPCB(skb)->opt), 0, sizeof(IPCB(skb)->opt));
inner_iph = (const struct iphdr *)skb_inner_network_header(skb);
tos = key->tos;
if (tos == 1) {
if (skb->protocol == htons(ETH_P_IP))
tos = inner_iph->tos;
else if (skb->protocol == htons(ETH_P_IPV6))
tos = ipv6_get_dsfield((const struct ipv6hdr *)inner_iph);
}
init_tunnel_flow(&fl4, proto, key->u.ipv4.dst, key->u.ipv4.src, 0,
RT_TOS(tos), tunnel->parms.link);
if (tunnel->encap.type != TUNNEL_ENCAP_NONE)
goto tx_error;
rt = ip_route_output_key(tunnel->net, &fl4);
if (IS_ERR(rt)) {
dev->stats.tx_carrier_errors++;
goto tx_error;
}
if (rt->dst.dev == dev) {
ip_rt_put(rt);
dev->stats.collisions++;
goto tx_error;
}
tos = ip_tunnel_ecn_encap(tos, inner_iph, skb);
ttl = key->ttl;
if (ttl == 0) {
if (skb->protocol == htons(ETH_P_IP))
ttl = inner_iph->ttl;
else if (skb->protocol == htons(ETH_P_IPV6))
ttl = ((const struct ipv6hdr *)inner_iph)->hop_limit;
else
ttl = ip4_dst_hoplimit(&rt->dst);
}
if (key->tun_flags & TUNNEL_DONT_FRAGMENT)
df = htons(IP_DF);
else if (skb->protocol == htons(ETH_P_IP))
df = inner_iph->frag_off & htons(IP_DF);
headroom += LL_RESERVED_SPACE(rt->dst.dev) + rt->dst.header_len;
if (headroom > dev->needed_headroom)
dev->needed_headroom = headroom;
if (skb_cow_head(skb, dev->needed_headroom)) {
ip_rt_put(rt);
goto tx_dropped;
}
iptunnel_xmit(NULL, rt, skb, fl4.saddr, fl4.daddr, proto, key->tos,
key->ttl, df, !net_eq(tunnel->net, dev_net(dev)));
return;
tx_error:
dev->stats.tx_errors++;
goto kfree;
tx_dropped:
dev->stats.tx_dropped++;
kfree:
kfree_skb(skb);
}
//-----------------------------------------------------------------------------
static int _gtpusp_ksocket_process_gtp(const unsigned char * const rx_buf_pP, const int lenP, unsigned char* tx_buf_pP)
//-----------------------------------------------------------------------------
{
gtpv1u_msg_t gtpv1u_msg;
uint8_t msg_type;
struct iphdr *iph_p = NULL;
struct iphdr *new_iph_p= NULL;
struct sk_buff *skb_p = NULL;
const unsigned char * rx_buf_p = rx_buf_pP;
int err = 0;
struct rtable *rt = NULL;
struct flowi fl = {
.u = {
.ip4 = {
.daddr = 0,
.flowi4_tos = 0,
.flowi4_scope = RT_SCOPE_UNIVERSE,
}
}
};
msg_type = rx_buf_pP[1];
switch(msg_type) {
case GTP_ECHO_REQ:
PR_INFO(MODULE_NAME": TODO GTP ECHO_REQ, SEND TO GTPV1U TASK USER SPACE\n");
//TODO;
return 0;
break;
case GTP_ERROR_INDICATION:
PR_INFO(MODULE_NAME":TODO GTP ERROR INDICATION, SEND TO GTPV1U TASK USER SPACE\n");
//TODO;
return 0;
break;
case GTP_ECHO_RSP:
PR_INFO(MODULE_NAME":GTP ECHO_RSP, SEND TO GTPV1U TASK USER SPACE\n");
return 0;
break;
case GTP_GPDU: {
gtpv1u_msg.version = ((*rx_buf_p) & 0xE0) >> 5;
gtpv1u_msg.protocol_type = ((*rx_buf_p) & 0x10) >> 4;
gtpv1u_msg.ext_hdr_flag = ((*rx_buf_p) & 0x04) >> 2;
gtpv1u_msg.seq_num_flag = ((*rx_buf_p) & 0x02) >> 1;
gtpv1u_msg.npdu_num_flag = ((*rx_buf_p) & 0x01);
rx_buf_p++;
gtpv1u_msg.msg_type = *(rx_buf_p);
rx_buf_p++;
rx_buf_p += 2;
gtpv1u_msg.teid = ntohl(*((u_int32_t *)rx_buf_p));
rx_buf_p += 4;
if(gtpv1u_msg.ext_hdr_flag || gtpv1u_msg.seq_num_flag || gtpv1u_msg.npdu_num_flag) {
gtpv1u_msg.seq_num = ntohs(*(((u_int16_t *)rx_buf_p)));
rx_buf_p += 2;
gtpv1u_msg.npdu_num = *(rx_buf_p++);
gtpv1u_msg.next_ext_hdr_type = *(rx_buf_p++);
}
gtpv1u_msg.msg_buf_offset = (u_int32_t)(rx_buf_p - rx_buf_pP);
gtpv1u_msg.msg_buf_len = lenP - gtpv1u_msg.msg_buf_offset;
gtpv1u_msg.msg_len = lenP;
iph_p = (struct iphdr*)(&rx_buf_pP[gtpv1u_msg.msg_buf_offset]);
fl.u.ip4.daddr = iph_p->daddr;
fl.u.ip4.flowi4_tos = RT_TOS(iph_p->tos);
rt = ip_route_output_key(&init_net, &fl.u.ip4);
if (rt == NULL) {
PR_INFO("GTPURH: Failed to route packet to dst 0x%x. Error: (%d)\n", fl.u.ip4.daddr, err);
return NF_DROP;
}
if (rt->dst.dev == NULL) {
pr_info("GTPURH: dst dev NULL\n");
return 0;
}
skb_p = alloc_skb(LL_MAX_HEADER + ntohs(iph_p->tot_len), GFP_ATOMIC);
if (skb_p == NULL) {
return 0;
}
skb_p->priority = rt_tos2priority(iph_p->tos);
skb_p->pkt_type = PACKET_OTHERHOST;
skb_dst_set(skb_p, dst_clone(&rt->dst));
skb_p->dev = skb_dst(skb_p)->dev;
skb_reserve(skb_p, LL_MAX_HEADER + ntohs(iph_p->tot_len));
skb_p->protocol = htons(ETH_P_IP);
new_iph_p = (void *)skb_push(skb_p, ntohs(iph_p->tot_len) - (iph_p->ihl << 2));
skb_reset_transport_header(skb_p);
new_iph_p = (void *)skb_push(skb_p, iph_p->ihl << 2);
memcpy(new_iph_p, iph_p, ntohs(iph_p->tot_len));
skb_reset_network_header(skb_p);
skb_reset_inner_network_header(skb_p);
skb_reset_inner_transport_header(skb_p);
skb_p->mark = gtpv1u_msg.teid;
new_iph_p->ttl = ip4_dst_hoplimit(skb_dst(skb_p));
skb_p->ip_summed = CHECKSUM_NONE;
if (skb_p->len > dst_mtu(skb_dst(skb_p))) {
PR_INFO("GTPURH: bad length\n");
goto free_skb;
}
ip_local_out(skb_p);
return 0;
free_skb:
pr_info("GTPURH: Dropped skb\n");
kfree_skb(skb_p);
return 0;
}
break;
default:
PR_INFO(MODULE_NAME":ERROR GTPU msg type %u\n", msg_type);
return 0;
}
}
void ip_tunnel_xmit(struct sk_buff *skb, struct net_device *dev,
const struct iphdr *tnl_params)
{
struct ip_tunnel *tunnel = netdev_priv(dev);
const struct iphdr *inner_iph;
struct iphdr *iph;
struct flowi4 fl4;
u8 tos, ttl;
__be16 df;
struct rtable *rt; /* Route to the other host */
struct net_device *tdev; /* Device to other host */
unsigned int max_headroom; /* The extra header space needed */
__be32 dst;
inner_iph = (const struct iphdr *)skb_inner_network_header(skb);
memset(IPCB(skb), 0, sizeof(*IPCB(skb)));
dst = tnl_params->daddr;
if (dst == 0) {
/* NBMA tunnel */
if (skb_dst(skb) == NULL) {
dev->stats.tx_fifo_errors++;
goto tx_error;
}
if (skb->protocol == htons(ETH_P_IP)) {
rt = skb_rtable(skb);
dst = rt_nexthop(rt, inner_iph->daddr);
}
#if IS_ENABLED(CONFIG_IPV6)
else if (skb->protocol == htons(ETH_P_IPV6)) {
const struct in6_addr *addr6;
struct neighbour *neigh;
bool do_tx_error_icmp;
int addr_type;
neigh = dst_neigh_lookup(skb_dst(skb),
&ipv6_hdr(skb)->daddr);
if (neigh == NULL)
goto tx_error;
addr6 = (const struct in6_addr *)&neigh->primary_key;
addr_type = ipv6_addr_type(addr6);
if (addr_type == IPV6_ADDR_ANY) {
addr6 = &ipv6_hdr(skb)->daddr;
addr_type = ipv6_addr_type(addr6);
}
if ((addr_type & IPV6_ADDR_COMPATv4) == 0)
do_tx_error_icmp = true;
else {
do_tx_error_icmp = false;
dst = addr6->s6_addr32[3];
}
neigh_release(neigh);
if (do_tx_error_icmp)
goto tx_error_icmp;
}
#endif
else
goto tx_error;
}
tos = tnl_params->tos;
if (tos & 0x1) {
tos &= ~0x1;
if (skb->protocol == htons(ETH_P_IP))
tos = inner_iph->tos;
else if (skb->protocol == htons(ETH_P_IPV6))
tos = ipv6_get_dsfield((const struct ipv6hdr *)inner_iph);
}
rt = ip_route_output_tunnel(dev_net(dev), &fl4,
tunnel->parms.iph.protocol,
dst, tnl_params->saddr,
tunnel->parms.o_key,
RT_TOS(tos),
tunnel->parms.link);
if (IS_ERR(rt)) {
dev->stats.tx_carrier_errors++;
goto tx_error;
}
tdev = rt->dst.dev;
if (tdev == dev) {
ip_rt_put(rt);
dev->stats.collisions++;
goto tx_error;
}
if (tnl_update_pmtu(dev, skb, rt, tnl_params->frag_off)) {
ip_rt_put(rt);
goto tx_error;
}
if (tunnel->err_count > 0) {
if (time_before(jiffies,
tunnel->err_time + IPTUNNEL_ERR_TIMEO)) {
tunnel->err_count--;
memset(IPCB(skb), 0, sizeof(*IPCB(skb)));
dst_link_failure(skb);
} else
tunnel->err_count = 0;
}
ttl = tnl_params->ttl;
if (ttl == 0) {
if (skb->protocol == htons(ETH_P_IP))
ttl = inner_iph->ttl;
#if IS_ENABLED(CONFIG_IPV6)
else if (skb->protocol == htons(ETH_P_IPV6))
ttl = ((const struct ipv6hdr *)inner_iph)->hop_limit;
#endif
else
ttl = ip4_dst_hoplimit(&rt->dst);
}
df = tnl_params->frag_off;
if (skb->protocol == htons(ETH_P_IP))
df |= (inner_iph->frag_off&htons(IP_DF));
max_headroom = LL_RESERVED_SPACE(tdev) + sizeof(struct iphdr)
+ rt->dst.header_len;
if (max_headroom > dev->needed_headroom)
dev->needed_headroom = max_headroom;
if (skb_cow_head(skb, dev->needed_headroom)) {
dev->stats.tx_dropped++;
dev_kfree_skb(skb);
return;
}
skb_dst_drop(skb);
skb_dst_set(skb, &rt->dst);
/* Push down and install the IP header. */
skb_push(skb, sizeof(struct iphdr));
skb_reset_network_header(skb);
iph = ip_hdr(skb);
inner_iph = (const struct iphdr *)skb_inner_network_header(skb);
iph->version = 4;
iph->ihl = sizeof(struct iphdr) >> 2;
iph->frag_off = df;
iph->protocol = tnl_params->protocol;
iph->tos = ip_tunnel_ecn_encap(tos, inner_iph, skb);
iph->daddr = fl4.daddr;
iph->saddr = fl4.saddr;
iph->ttl = ttl;
__ip_select_ident(iph, &rt->dst, (skb_shinfo(skb)->gso_segs ?: 1) - 1);
iptunnel_xmit(skb, dev);
return;
#if IS_ENABLED(CONFIG_IPV6)
tx_error_icmp:
dst_link_failure(skb);
#endif
tx_error:
dev->stats.tx_errors++;
dev_kfree_skb(skb);
}
