static int mpls_xmit(struct sk_buff *skb) { struct mpls_iptunnel_encap *tun_encap_info; struct mpls_shim_hdr *hdr; struct net_device *out_dev; unsigned int hh_len; unsigned int new_header_size; unsigned int mtu; struct dst_entry *dst = skb_dst(skb); struct rtable *rt = NULL; struct rt6_info *rt6 = NULL; int err = 0; bool bos; int i; unsigned int ttl; /* Obtain the ttl */ if (dst->ops->family == AF_INET) { ttl = ip_hdr(skb)->ttl; rt = (struct rtable *)dst; } else if (dst->ops->family == AF_INET6) { ttl = ipv6_hdr(skb)->hop_limit; rt6 = (struct rt6_info *)dst; } else { goto drop; } skb_orphan(skb); /* Find the output device */ out_dev = dst->dev; if (!mpls_output_possible(out_dev) || !dst->lwtstate || skb_warn_if_lro(skb)) goto drop; skb_forward_csum(skb); tun_encap_info = mpls_lwtunnel_encap(dst->lwtstate); /* Verify the destination can hold the packet */ new_header_size = mpls_encap_size(tun_encap_info); mtu = mpls_dev_mtu(out_dev); if (mpls_pkt_too_big(skb, mtu - new_header_size)) goto drop; hh_len = LL_RESERVED_SPACE(out_dev); if (!out_dev->header_ops) hh_len = 0; /* Ensure there is enough space for the headers in the skb */ if (skb_cow(skb, hh_len + new_header_size)) goto drop; skb_set_inner_protocol(skb, skb->protocol); skb_reset_inner_network_header(skb); skb_push(skb, new_header_size); skb_reset_network_header(skb); skb->dev = out_dev; skb->protocol = htons(ETH_P_MPLS_UC); /* Push the new labels */ hdr = mpls_hdr(skb); bos = true; for (i = tun_encap_info->labels - 1; i >= 0; i--) { hdr[i] = mpls_entry_encode(tun_encap_info->label[i], ttl, 0, bos); bos = false; } if (rt) err = neigh_xmit(NEIGH_ARP_TABLE, out_dev, &rt->rt_gateway, skb); else if (rt6) err = neigh_xmit(NEIGH_ND_TABLE, out_dev, &rt6->rt6i_gateway, skb); if (err) net_dbg_ratelimited("%s: packet transmission failed: %d\n", __func__, err); return LWTUNNEL_XMIT_DONE; drop: kfree_skb(skb); return -EINVAL; }
//----------------------------------------------------------------------------- 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; } }