drivers/net/pfcp.c

   1 // SPDX-License-Identifier: GPL-2.0-or-later
   2 /*
   3  * PFCP according to 3GPP TS 29.244
   4  *
   5  * Copyright (C) 2022, Intel Corporation.
   6  */
   7
   8 #include <linux/module.h>
   9 #include <linux/netdevice.h>
  10 #include <linux/rculist.h>
  11 #include <linux/skbuff.h>
  12 #include <linux/types.h>
  13
  14 #include <net/udp.h>
  15 #include <net/udp_tunnel.h>
  16 #include <net/pfcp.h>
  17
  18 struct pfcp_dev {
  19         struct list_head        list;
  20
  21         struct socket           *sock;
  22         struct net_device       *dev;
  23         struct net              *net;
  24
  25         struct gro_cells        gro_cells;
  26 };
  27
  28 static unsigned int pfcp_net_id __read_mostly;
  29
  30 struct pfcp_net {
  31         struct list_head        pfcp_dev_list;
  32 };
  33
  34 static void
  35 pfcp_session_recv(struct pfcp_dev *pfcp, struct sk_buff *skb,
  36                   struct pfcp_metadata *md)
  37 {
  38         struct pfcphdr_session *unparsed = pfcp_hdr_session(skb);
  39
  40         md->seid = unparsed->seid;
  41         md->type = PFCP_TYPE_SESSION;
  42 }
  43
  44 static void
  45 pfcp_node_recv(struct pfcp_dev *pfcp, struct sk_buff *skb,
  46                struct pfcp_metadata *md)
  47 {
  48         md->type = PFCP_TYPE_NODE;
  49 }
  50
  51 static int pfcp_encap_recv(struct sock *sk, struct sk_buff *skb)
  52 {
  53         IP_TUNNEL_DECLARE_FLAGS(flags) = { };
  54         struct metadata_dst *tun_dst;
  55         struct pfcp_metadata *md;
  56         struct pfcphdr *unparsed;
  57         struct pfcp_dev *pfcp;
  58
  59         if (unlikely(!pskb_may_pull(skb, PFCP_HLEN)))
  60                 goto drop;
  61
  62         pfcp = rcu_dereference_sk_user_data(sk);
  63         if (unlikely(!pfcp))
  64                 goto drop;
  65
  66         unparsed = pfcp_hdr(skb);
  67
  68         ip_tunnel_flags_zero(flags);
  69         tun_dst = udp_tun_rx_dst(skb, sk->sk_family, flags, 0,
  70                                  sizeof(*md));
  71         if (unlikely(!tun_dst))
  72                 goto drop;
  73
  74         md = ip_tunnel_info_opts(&tun_dst->u.tun_info);
  75         if (unlikely(!md))
  76                 goto drop;
  77
  78         if (unparsed->flags & PFCP_SEID_FLAG)
  79                 pfcp_session_recv(pfcp, skb, md);
  80         else
  81                 pfcp_node_recv(pfcp, skb, md);
  82
  83         __set_bit(IP_TUNNEL_PFCP_OPT_BIT, tun_dst->u.tun_info.key.tun_flags);
  84         tun_dst->u.tun_info.options_len = sizeof(*md);
  85
  86         if (unlikely(iptunnel_pull_header(skb, PFCP_HLEN, skb->protocol,
  87                                           !net_eq(sock_net(sk),
  88                                           dev_net(pfcp->dev)))))
  89                 goto drop;
  90
  91         skb_dst_set(skb, (struct dst_entry *)tun_dst);
  92
  93         skb_reset_network_header(skb);
  94         skb_reset_mac_header(skb);
  95         skb->dev = pfcp->dev;
  96
  97         gro_cells_receive(&pfcp->gro_cells, skb);
  98
  99         return 0;
 100 drop:
 101         kfree_skb(skb);
 102         return 0;
 103 }
 104
 105 static void pfcp_del_sock(struct pfcp_dev *pfcp)
 106 {
 107         udp_tunnel_sock_release(pfcp->sock);
 108         pfcp->sock = NULL;
 109 }
 110
 111 static void pfcp_dev_uninit(struct net_device *dev)
 112 {
 113         struct pfcp_dev *pfcp = netdev_priv(dev);
 114
 115         gro_cells_destroy(&pfcp->gro_cells);
 116         pfcp_del_sock(pfcp);
 117 }
 118
 119 static int pfcp_dev_init(struct net_device *dev)
 120 {
 121         struct pfcp_dev *pfcp = netdev_priv(dev);
 122
 123         pfcp->dev = dev;
 124
 125         return gro_cells_init(&pfcp->gro_cells, dev);
 126 }
 127
 128 static const struct net_device_ops pfcp_netdev_ops = {
 129         .ndo_init               = pfcp_dev_init,
 130         .ndo_uninit             = pfcp_dev_uninit,
 131         .ndo_get_stats64        = dev_get_tstats64,
 132 };
 133
 134 static const struct device_type pfcp_type = {
 135         .name = "pfcp",
 136 };
 137
 138 static void pfcp_link_setup(struct net_device *dev)
 139 {
 140         dev->netdev_ops = &pfcp_netdev_ops;
 141         dev->needs_free_netdev = true;
 142         SET_NETDEV_DEVTYPE(dev, &pfcp_type);
 143
 144         dev->hard_header_len = 0;
 145         dev->addr_len = 0;
 146
 147         dev->type = ARPHRD_NONE;
 148         dev->flags = IFF_POINTOPOINT | IFF_NOARP | IFF_MULTICAST;
 149         dev->priv_flags |= IFF_NO_QUEUE;
 150
 151         netif_keep_dst(dev);
 152 }
 153
 154 static struct socket *pfcp_create_sock(struct pfcp_dev *pfcp)
 155 {
 156         struct udp_tunnel_sock_cfg tuncfg = {};
 157         struct udp_port_cfg udp_conf = {
 158                 .local_ip.s_addr        = htonl(INADDR_ANY),
 159                 .family                 = AF_INET,
 160         };
 161         struct net *net = pfcp->net;
 162         struct socket *sock;
 163         int err;
 164
 165         udp_conf.local_udp_port = htons(PFCP_PORT);
 166
 167         err = udp_sock_create(net, &udp_conf, &sock);
 168         if (err)
 169                 return ERR_PTR(err);
 170
 171         tuncfg.sk_user_data = pfcp;
 172         tuncfg.encap_rcv = pfcp_encap_recv;
 173         tuncfg.encap_type = 1;
 174
 175         setup_udp_tunnel_sock(net, sock, &tuncfg);
 176
 177         return sock;
 178 }
 179
 180 static int pfcp_add_sock(struct pfcp_dev *pfcp)
 181 {
 182         pfcp->sock = pfcp_create_sock(pfcp);
 183
 184         return PTR_ERR_OR_ZERO(pfcp->sock);
 185 }
 186
 187 static int pfcp_newlink(struct net *net, struct net_device *dev,
 188                         struct nlattr *tb[], struct nlattr *data[],
 189                         struct netlink_ext_ack *extack)
 190 {
 191         struct pfcp_dev *pfcp = netdev_priv(dev);
 192         struct pfcp_net *pn;
 193         int err;
 194
 195         pfcp->net = net;
 196
 197         err = pfcp_add_sock(pfcp);
 198         if (err) {
 199                 netdev_dbg(dev, "failed to add pfcp socket %d\n", err);
 200                 goto exit_err;
 201         }
 202
 203         err = register_netdevice(dev);
 204         if (err) {
 205                 netdev_dbg(dev, "failed to register pfcp netdev %d\n", err);
 206                 goto exit_del_pfcp_sock;
 207         }
 208
 209         pn = net_generic(dev_net(dev), pfcp_net_id);
 210         list_add_rcu(&pfcp->list, &pn->pfcp_dev_list);
 211
 212         netdev_dbg(dev, "registered new PFCP interface\n");
 213
 214         return 0;
 215
 216 exit_del_pfcp_sock:
 217         pfcp_del_sock(pfcp);
 218 exit_err:
 219         pfcp->net = NULL;
 220         return err;
 221 }
 222
 223 static void pfcp_dellink(struct net_device *dev, struct list_head *head)
 224 {
 225         struct pfcp_dev *pfcp = netdev_priv(dev);
 226
 227         list_del_rcu(&pfcp->list);
 228         unregister_netdevice_queue(dev, head);
 229 }
 230
 231 static struct rtnl_link_ops pfcp_link_ops __read_mostly = {
 232         .kind           = "pfcp",
 233         .priv_size      = sizeof(struct pfcp_dev),
 234         .setup          = pfcp_link_setup,
 235         .newlink        = pfcp_newlink,
 236         .dellink        = pfcp_dellink,
 237 };
 238
 239 static int __net_init pfcp_net_init(struct net *net)
 240 {
 241         struct pfcp_net *pn = net_generic(net, pfcp_net_id);
 242
 243         INIT_LIST_HEAD(&pn->pfcp_dev_list);
 244         return 0;
 245 }
 246
 247 static void __net_exit pfcp_net_exit(struct net *net)
 248 {
 249         struct pfcp_net *pn = net_generic(net, pfcp_net_id);
 250         struct pfcp_dev *pfcp;
 251         LIST_HEAD(list);
 252
 253         rtnl_lock();
 254         list_for_each_entry(pfcp, &pn->pfcp_dev_list, list)
 255                 pfcp_dellink(pfcp->dev, &list);
 256
 257         unregister_netdevice_many(&list);
 258         rtnl_unlock();
 259 }
 260
 261 static struct pernet_operations pfcp_net_ops = {
 262         .init   = pfcp_net_init,
 263         .exit   = pfcp_net_exit,
 264         .id     = &pfcp_net_id,
 265         .size   = sizeof(struct pfcp_net),
 266 };
 267
 268 static int __init pfcp_init(void)
 269 {
 270         int err;
 271
 272         err = register_pernet_subsys(&pfcp_net_ops);
 273         if (err)
 274                 goto exit_err;
 275
 276         err = rtnl_link_register(&pfcp_link_ops);
 277         if (err)
 278                 goto exit_unregister_subsys;
 279         return 0;
 280
 281 exit_unregister_subsys:
 282         unregister_pernet_subsys(&pfcp_net_ops);
 283 exit_err:
 284         pr_err("loading PFCP module failed: err %d\n", err);
 285         return err;
 286 }
 287 late_initcall(pfcp_init);
 288
 289 static void __exit pfcp_exit(void)
 290 {
 291         rtnl_link_unregister(&pfcp_link_ops);
 292         unregister_pernet_subsys(&pfcp_net_ops);
 293
 294         pr_info("PFCP module unloaded\n");
 295 }
 296 module_exit(pfcp_exit);
 297
 298 MODULE_LICENSE("GPL");
 299 MODULE_AUTHOR("Wojciech Drewek <wojciech.drewek@intel.com>");
 300 MODULE_DESCRIPTION("Interface driver for PFCP encapsulated traffic");
 301 MODULE_ALIAS_RTNL_LINK("pfcp");