302 lines · c
1// SPDX-License-Identifier: GPL-2.0-or-later2/*3 * PFCP according to 3GPP TS 29.2444 *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 18struct 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 28static unsigned int pfcp_net_id __read_mostly;29 30struct pfcp_net {31 struct list_head pfcp_dev_list;32};33 34static void35pfcp_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 44static void45pfcp_node_recv(struct pfcp_dev *pfcp, struct sk_buff *skb,46 struct pfcp_metadata *md)47{48 md->type = PFCP_TYPE_NODE;49}50 51static 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 else81 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;100drop:101 kfree_skb(skb);102 return 0;103}104 105static void pfcp_del_sock(struct pfcp_dev *pfcp)106{107 udp_tunnel_sock_release(pfcp->sock);108 pfcp->sock = NULL;109}110 111static 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 119static 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 128static 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 134static const struct device_type pfcp_type = {135 .name = "pfcp",136};137 138static 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 154static 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 180static 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 187static 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 216exit_del_pfcp_sock:217 pfcp_del_sock(pfcp);218exit_err:219 pfcp->net = NULL;220 return err;221}222 223static 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 231static 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 239static 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 247static 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 261static 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 268static 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 281exit_unregister_subsys:282 unregister_pernet_subsys(&pfcp_net_ops);283exit_err:284 pr_err("loading PFCP module failed: err %d\n", err);285 return err;286}287late_initcall(pfcp_init);288 289static 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}296module_exit(pfcp_exit);297 298MODULE_LICENSE("GPL");299MODULE_AUTHOR("Wojciech Drewek <wojciech.drewek@intel.com>");300MODULE_DESCRIPTION("Interface driver for PFCP encapsulated traffic");301MODULE_ALIAS_RTNL_LINK("pfcp");302