/* 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); }