| 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);
|
