155 |
155 |
return NULL;
|
156 |
156 |
}
|
157 |
157 |
|
|
158 |
/* Resolve a PDP context based on IP address of MS. */
|
|
159 |
static struct pdp_ctx *ip_pdp_find(struct sk_buff *skb, struct net_device *dev)
|
|
160 |
{
|
|
161 |
unsigned int proto = ntohs(skb->protocol);
|
|
162 |
struct gtp_dev *gtp = netdev_priv(dev);
|
|
163 |
|
|
164 |
switch (proto) {
|
|
165 |
case ETH_P_IP: {
|
|
166 |
struct iphdr *iph = ip_hdr(skb);
|
|
167 |
|
|
168 |
return ipv4_pdp_find(gtp, iph->daddr);
|
|
169 |
}
|
|
170 |
}
|
|
171 |
|
|
172 |
return ERR_PTR(-EOPNOTSUPP);
|
|
173 |
}
|
|
174 |
|
158 |
175 |
static bool gtp_check_src_ms_ipv4(struct sk_buff *skb, struct pdp_ctx *pctx,
|
159 |
176 |
unsigned int hdrlen)
|
160 |
177 |
{
|
... | ... | |
434 |
451 |
*/
|
435 |
452 |
}
|
436 |
453 |
|
437 |
|
struct gtp_pktinfo {
|
438 |
|
struct sock *sk;
|
439 |
|
struct iphdr *iph;
|
440 |
|
struct flowi4 fl4;
|
441 |
|
struct rtable *rt;
|
442 |
|
struct pdp_ctx *pctx;
|
443 |
|
struct net_device *dev;
|
444 |
|
__be16 gtph_port;
|
445 |
|
};
|
446 |
|
|
447 |
|
static void gtp_push_header(struct sk_buff *skb, struct gtp_pktinfo *pktinfo)
|
|
454 |
static void gtp_push_header(struct sk_buff *skb, struct pdp_ctx *pctx)
|
448 |
455 |
{
|
449 |
|
switch (pktinfo->pctx->gtp_version) {
|
|
456 |
switch (pctx->gtp_version) {
|
450 |
457 |
case GTP_V0:
|
451 |
|
pktinfo->gtph_port = htons(GTP0_PORT);
|
452 |
|
gtp0_push_header(skb, pktinfo->pctx);
|
|
458 |
gtp0_push_header(skb, pctx);
|
453 |
459 |
break;
|
454 |
460 |
case GTP_V1:
|
455 |
|
pktinfo->gtph_port = htons(GTP1U_PORT);
|
456 |
|
gtp1_push_header(skb, pktinfo->pctx);
|
|
461 |
gtp1_push_header(skb, pctx);
|
457 |
462 |
break;
|
458 |
463 |
}
|
459 |
464 |
}
|
460 |
465 |
|
|
466 |
static int get_dst_port(struct pdp_ctx *pctx)
|
|
467 |
{
|
|
468 |
if (pctx->gtp_version == GTP_V0)
|
|
469 |
return htons(GTP0_PORT);
|
|
470 |
|
|
471 |
return htons(GTP1U_PORT);
|
|
472 |
}
|
|
473 |
|
461 |
474 |
static int gtp_update_pmtu(struct pdp_ctx *pctx, struct sk_buff *skb,
|
462 |
475 |
struct net_device *dev, struct dst_entry *ndst,
|
463 |
476 |
__be16 df, int tnl_header_len,
|
... | ... | |
490 |
503 |
}
|
491 |
504 |
|
492 |
505 |
return 0;
|
493 |
|
}
|
494 |
|
|
495 |
|
static inline void gtp_set_pktinfo_ipv4(struct gtp_pktinfo *pktinfo,
|
496 |
|
struct sock *sk, struct iphdr *iph,
|
497 |
|
struct pdp_ctx *pctx, struct rtable *rt,
|
498 |
|
struct flowi4 *fl4,
|
499 |
|
struct net_device *dev)
|
500 |
|
{
|
501 |
|
pktinfo->sk = sk;
|
502 |
|
pktinfo->iph = iph;
|
503 |
|
pktinfo->pctx = pctx;
|
504 |
|
pktinfo->rt = rt;
|
505 |
|
pktinfo->fl4 = *fl4;
|
506 |
|
pktinfo->dev = dev;
|
507 |
506 |
}
|
508 |
507 |
|
509 |
|
static int gtp_build_skb_ip4(struct sk_buff *skb, struct net_device *dev,
|
510 |
|
struct gtp_pktinfo *pktinfo)
|
|
508 |
static int gtp_dev_xmit_ip4(struct pdp_ctx *pctx, struct sk_buff *skb,
|
|
509 |
struct net_device *dev)
|
511 |
510 |
{
|
512 |
|
struct gtp_dev *gtp = netdev_priv(dev);
|
513 |
|
struct pdp_ctx *pctx;
|
|
511 |
const struct iphdr *iph = ip_hdr(skb);
|
514 |
512 |
struct rtable *rt;
|
515 |
513 |
struct flowi4 fl4;
|
516 |
|
struct iphdr *iph;
|
517 |
|
|
518 |
|
/* Read the IP destination address and resolve the PDP context.
|
519 |
|
* Prepend PDP header with TEI/TID from PDP ctx.
|
520 |
|
*/
|
521 |
|
iph = ip_hdr(skb);
|
522 |
|
pctx = ipv4_pdp_find(gtp, iph->daddr);
|
523 |
|
if (!pctx) {
|
524 |
|
netdev_dbg(dev, "no PDP ctx found for %pI4, skip\n",
|
525 |
|
&iph->daddr);
|
526 |
|
return -ENOENT;
|
527 |
|
}
|
528 |
|
netdev_dbg(dev, "found PDP context %p\n", pctx);
|
|
514 |
__be16 gtph_port;
|
|
515 |
__u8 tos;
|
529 |
516 |
|
|
517 |
/* Prepend PDP header with TEI/TID from PDP ctx. */
|
530 |
518 |
rt = dst_cache_get_ip4(&pctx->dst_cache, &pctx->sgsn_addr_ip4.s_addr);
|
531 |
519 |
if (!rt) {
|
532 |
520 |
rt = ip4_route_output_gtp(&fl4, pctx->sk, pctx->sgsn_addr_ip4.s_addr);
|
... | ... | |
553 |
541 |
goto err_rt;
|
554 |
542 |
|
555 |
543 |
skb_dst_drop(skb);
|
|
544 |
gtp_push_header(skb, pctx);
|
556 |
545 |
|
557 |
|
gtp_set_pktinfo_ipv4(pktinfo, pctx->sk, iph, pctx, rt, &fl4, dev);
|
558 |
|
gtp_push_header(skb, pktinfo);
|
|
546 |
tos = ip_tunnel_ecn_encap(fl4.flowi4_tos, ip_hdr(skb), skb);
|
|
547 |
gtph_port = get_dst_port(pctx);
|
|
548 |
|
|
549 |
udp_tunnel_xmit_skb(rt, pctx->sk, skb, fl4.saddr, fl4.daddr, tos,
|
|
550 |
ip4_dst_hoplimit(&rt->dst), 0,
|
|
551 |
gtph_port, gtph_port, true, false);
|
559 |
552 |
|
560 |
553 |
return 0;
|
561 |
554 |
err_rt:
|
... | ... | |
566 |
559 |
|
567 |
560 |
static netdev_tx_t gtp_dev_xmit(struct sk_buff *skb, struct net_device *dev)
|
568 |
561 |
{
|
569 |
|
unsigned int proto = ntohs(skb->protocol);
|
570 |
|
struct gtp_pktinfo pktinfo;
|
|
562 |
struct pdp_ctx *pctx;
|
571 |
563 |
int err;
|
572 |
564 |
|
573 |
565 |
/* Ensure there is sufficient headroom. */
|
... | ... | |
576 |
568 |
|
577 |
569 |
skb_reset_inner_headers(skb);
|
578 |
570 |
|
579 |
|
/* PDP context lookups in gtp_build_skb_*() need rcu read-side lock. */
|
580 |
571 |
rcu_read_lock();
|
581 |
|
switch (proto) {
|
582 |
|
case ETH_P_IP:
|
583 |
|
err = gtp_build_skb_ip4(skb, dev, &pktinfo);
|
|
572 |
pctx = ip_pdp_find(skb, dev);
|
|
573 |
if (IS_ERR(pctx))
|
|
574 |
return PTR_ERR(pctx);
|
|
575 |
netdev_dbg(dev, "found PDP context %p\n", pctx);
|
|
576 |
|
|
577 |
switch (pctx->sk->sk_family) {
|
|
578 |
case AF_INET:
|
|
579 |
err = gtp_dev_xmit_ip4(pctx, skb, dev);
|
584 |
580 |
break;
|
585 |
|
default:
|
586 |
|
err = -EOPNOTSUPP;
|
|
581 |
default:
|
|
582 |
err = -EOPNOTSUPP;
|
587 |
583 |
break;
|
588 |
584 |
}
|
589 |
585 |
rcu_read_unlock();
|
590 |
586 |
|
591 |
|
if (err < 0)
|
592 |
|
goto tx_err;
|
|
587 |
if (!err)
|
|
588 |
return NETDEV_TX_OK;
|
593 |
589 |
|
594 |
|
switch (proto) {
|
595 |
|
case ETH_P_IP:
|
596 |
|
netdev_dbg(pktinfo.dev, "gtp -> IP src: %pI4 dst: %pI4\n",
|
597 |
|
&pktinfo.iph->saddr, &pktinfo.iph->daddr);
|
598 |
|
udp_tunnel_xmit_skb(pktinfo.rt, pktinfo.sk, skb,
|
599 |
|
pktinfo.fl4.saddr, pktinfo.fl4.daddr,
|
600 |
|
pktinfo.iph->tos,
|
601 |
|
ip4_dst_hoplimit(&pktinfo.rt->dst),
|
602 |
|
0,
|
603 |
|
pktinfo.gtph_port, pktinfo.gtph_port,
|
604 |
|
true, false);
|
605 |
|
break;
|
606 |
|
}
|
607 |
|
|
608 |
|
return NETDEV_TX_OK;
|
609 |
590 |
tx_err:
|
610 |
591 |
dev->stats.tx_errors++;
|
611 |
592 |
dev_kfree_skb(skb);
|