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 sock *sk; 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->sk); 108 pfcp->sk = 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 dev->pcpu_stat_type = NETDEV_PCPU_STAT_TSTATS; 152 netif_keep_dst(dev); 153 } 154 155 static struct sock *pfcp_create_sock(struct pfcp_dev *pfcp) 156 { 157 struct udp_tunnel_sock_cfg tuncfg = {}; 158 struct udp_port_cfg udp_conf = { 159 .local_ip.s_addr = htonl(INADDR_ANY), 160 .family = AF_INET, 161 }; 162 struct net *net = pfcp->net; 163 struct socket *sock; 164 int err; 165 166 udp_conf.local_udp_port = htons(PFCP_PORT); 167 168 err = udp_sock_create(net, &udp_conf, &sock); 169 if (err) 170 return ERR_PTR(err); 171 172 tuncfg.sk_user_data = pfcp; 173 tuncfg.encap_rcv = pfcp_encap_recv; 174 tuncfg.encap_type = 1; 175 176 setup_udp_tunnel_sock(net, sock->sk, &tuncfg); 177 178 return sock->sk; 179 } 180 181 static int pfcp_add_sock(struct pfcp_dev *pfcp) 182 { 183 pfcp->sk = pfcp_create_sock(pfcp); 184 185 return PTR_ERR_OR_ZERO(pfcp->sk); 186 } 187 188 static int pfcp_newlink(struct net_device *dev, 189 struct rtnl_newlink_params *params, 190 struct netlink_ext_ack *extack) 191 { 192 struct net *link_net = rtnl_newlink_link_net(params); 193 struct pfcp_dev *pfcp = netdev_priv(dev); 194 struct pfcp_net *pn; 195 int err; 196 197 pfcp->net = link_net; 198 199 err = pfcp_add_sock(pfcp); 200 if (err) { 201 netdev_dbg(dev, "failed to add pfcp socket %d\n", err); 202 goto exit_err; 203 } 204 205 err = register_netdevice(dev); 206 if (err) { 207 netdev_dbg(dev, "failed to register pfcp netdev %d\n", err); 208 goto exit_del_pfcp_sock; 209 } 210 211 pn = net_generic(link_net, pfcp_net_id); 212 list_add(&pfcp->list, &pn->pfcp_dev_list); 213 214 netdev_dbg(dev, "registered new PFCP interface\n"); 215 216 return 0; 217 218 exit_del_pfcp_sock: 219 pfcp_del_sock(pfcp); 220 synchronize_rcu(); 221 exit_err: 222 pfcp->net = NULL; 223 return err; 224 } 225 226 static void pfcp_dellink(struct net_device *dev, struct list_head *head) 227 { 228 struct pfcp_dev *pfcp = netdev_priv(dev); 229 230 list_del(&pfcp->list); 231 unregister_netdevice_queue(dev, head); 232 } 233 234 static struct rtnl_link_ops pfcp_link_ops __read_mostly = { 235 .kind = "pfcp", 236 .priv_size = sizeof(struct pfcp_dev), 237 .setup = pfcp_link_setup, 238 .newlink = pfcp_newlink, 239 .dellink = pfcp_dellink, 240 }; 241 242 static int __net_init pfcp_net_init(struct net *net) 243 { 244 struct pfcp_net *pn = net_generic(net, pfcp_net_id); 245 246 INIT_LIST_HEAD(&pn->pfcp_dev_list); 247 return 0; 248 } 249 250 static void __net_exit pfcp_net_exit_rtnl(struct net *net, 251 struct list_head *dev_to_kill) 252 { 253 struct pfcp_net *pn = net_generic(net, pfcp_net_id); 254 struct pfcp_dev *pfcp, *pfcp_next; 255 256 list_for_each_entry_safe(pfcp, pfcp_next, &pn->pfcp_dev_list, list) 257 pfcp_dellink(pfcp->dev, dev_to_kill); 258 } 259 260 static struct pernet_operations pfcp_net_ops = { 261 .init = pfcp_net_init, 262 .exit_rtnl = pfcp_net_exit_rtnl, 263 .id = &pfcp_net_id, 264 .size = sizeof(struct pfcp_net), 265 }; 266 267 static int __init pfcp_init(void) 268 { 269 int err; 270 271 err = register_pernet_subsys(&pfcp_net_ops); 272 if (err) 273 goto exit_err; 274 275 err = rtnl_link_register(&pfcp_link_ops); 276 if (err) 277 goto exit_unregister_subsys; 278 return 0; 279 280 exit_unregister_subsys: 281 unregister_pernet_subsys(&pfcp_net_ops); 282 exit_err: 283 pr_err("loading PFCP module failed: err %d\n", err); 284 return err; 285 } 286 late_initcall(pfcp_init); 287 288 static void __exit pfcp_exit(void) 289 { 290 rtnl_link_unregister(&pfcp_link_ops); 291 unregister_pernet_subsys(&pfcp_net_ops); 292 293 pr_info("PFCP module unloaded\n"); 294 } 295 module_exit(pfcp_exit); 296 297 MODULE_LICENSE("GPL"); 298 MODULE_AUTHOR("Wojciech Drewek <wojciech.drewek@intel.com>"); 299 MODULE_DESCRIPTION("Interface driver for PFCP encapsulated traffic"); 300 MODULE_ALIAS_RTNL_LINK("pfcp"); 301