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 struct mutex lock; 33 }; 34 35 static void 36 pfcp_session_recv(struct pfcp_dev *pfcp, struct sk_buff *skb, 37 struct pfcp_metadata *md) 38 { 39 struct pfcphdr_session *unparsed = pfcp_hdr_session(skb); 40 41 md->seid = unparsed->seid; 42 md->type = PFCP_TYPE_SESSION; 43 } 44 45 static void 46 pfcp_node_recv(struct pfcp_dev *pfcp, struct sk_buff *skb, 47 struct pfcp_metadata *md) 48 { 49 md->type = PFCP_TYPE_NODE; 50 } 51 52 static int pfcp_encap_recv(struct sock *sk, struct sk_buff *skb) 53 { 54 IP_TUNNEL_DECLARE_FLAGS(flags) = { }; 55 struct metadata_dst *tun_dst; 56 struct pfcp_metadata *md; 57 struct pfcphdr *unparsed; 58 struct pfcp_dev *pfcp; 59 60 if (unlikely(!pskb_may_pull(skb, PFCP_HLEN))) 61 goto drop; 62 63 pfcp = rcu_dereference_sk_user_data(sk); 64 if (unlikely(!pfcp)) 65 goto drop; 66 67 unparsed = pfcp_hdr(skb); 68 if (unparsed->flags & PFCP_SEID_FLAG) { 69 if (unlikely(!pskb_may_pull(skb, PFCP_HLEN + 70 offsetofend(struct pfcphdr_session, 71 seid)))) 72 goto drop; 73 unparsed = pfcp_hdr(skb); 74 } 75 76 ip_tunnel_flags_zero(flags); 77 tun_dst = udp_tun_rx_dst(skb, sk->sk_family, flags, 0, 78 sizeof(*md)); 79 if (unlikely(!tun_dst)) 80 goto drop; 81 82 md = ip_tunnel_info_opts(&tun_dst->u.tun_info); 83 if (unlikely(!md)) 84 goto drop; 85 86 if (unparsed->flags & PFCP_SEID_FLAG) 87 pfcp_session_recv(pfcp, skb, md); 88 else 89 pfcp_node_recv(pfcp, skb, md); 90 91 __set_bit(IP_TUNNEL_PFCP_OPT_BIT, tun_dst->u.tun_info.key.tun_flags); 92 tun_dst->u.tun_info.options_len = sizeof(*md); 93 94 if (unlikely(iptunnel_pull_header(skb, PFCP_HLEN, skb->protocol, 95 !net_eq(sock_net(sk), 96 dev_net(pfcp->dev))))) 97 goto drop; 98 99 skb_dst_set(skb, (struct dst_entry *)tun_dst); 100 101 skb_reset_network_header(skb); 102 skb_reset_mac_header(skb); 103 skb->dev = pfcp->dev; 104 105 gro_cells_receive(&pfcp->gro_cells, skb); 106 107 return 0; 108 drop: 109 kfree_skb(skb); 110 return 0; 111 } 112 113 static void pfcp_del_sock(struct pfcp_dev *pfcp) 114 { 115 if (!pfcp->sk) 116 return; 117 118 udp_tunnel_sock_release(pfcp->sk); 119 pfcp->sk = NULL; 120 } 121 122 static void pfcp_dev_uninit(struct net_device *dev) 123 { 124 struct pfcp_dev *pfcp = netdev_priv(dev); 125 126 gro_cells_destroy(&pfcp->gro_cells); 127 pfcp_del_sock(pfcp); 128 } 129 130 static int pfcp_dev_init(struct net_device *dev) 131 { 132 struct pfcp_dev *pfcp = netdev_priv(dev); 133 134 pfcp->dev = dev; 135 136 return gro_cells_init(&pfcp->gro_cells, dev); 137 } 138 139 static const struct net_device_ops pfcp_netdev_ops = { 140 .ndo_init = pfcp_dev_init, 141 .ndo_uninit = pfcp_dev_uninit, 142 .ndo_get_stats64 = dev_get_tstats64, 143 }; 144 145 static const struct device_type pfcp_type = { 146 .name = "pfcp", 147 }; 148 149 static void pfcp_link_setup(struct net_device *dev) 150 { 151 dev->netdev_ops = &pfcp_netdev_ops; 152 dev->needs_free_netdev = true; 153 SET_NETDEV_DEVTYPE(dev, &pfcp_type); 154 155 dev->hard_header_len = 0; 156 dev->addr_len = 0; 157 158 dev->type = ARPHRD_NONE; 159 dev->flags = IFF_POINTOPOINT | IFF_NOARP | IFF_MULTICAST; 160 dev->priv_flags |= IFF_NO_QUEUE; 161 162 dev->pcpu_stat_type = NETDEV_PCPU_STAT_TSTATS; 163 netif_keep_dst(dev); 164 } 165 166 static struct sock *pfcp_create_sock(struct pfcp_dev *pfcp) 167 { 168 struct udp_tunnel_sock_cfg tuncfg = {}; 169 struct udp_port_cfg udp_conf = { 170 .local_ip.s_addr = htonl(INADDR_ANY), 171 .family = AF_INET, 172 }; 173 struct net *net = pfcp->net; 174 struct socket *sock; 175 int err; 176 177 udp_conf.local_udp_port = htons(PFCP_PORT); 178 179 err = udp_sock_create(net, &udp_conf, &sock); 180 if (err) 181 return ERR_PTR(err); 182 183 tuncfg.sk_user_data = pfcp; 184 tuncfg.encap_rcv = pfcp_encap_recv; 185 tuncfg.encap_type = 1; 186 187 setup_udp_tunnel_sock(net, sock->sk, &tuncfg); 188 189 return sock->sk; 190 } 191 192 static int pfcp_add_sock(struct pfcp_dev *pfcp) 193 { 194 pfcp->sk = pfcp_create_sock(pfcp); 195 196 return PTR_ERR_OR_ZERO(pfcp->sk); 197 } 198 199 static int pfcp_newlink(struct net_device *dev, 200 struct rtnl_newlink_params *params, 201 struct netlink_ext_ack *extack) 202 { 203 struct net *link_net = rtnl_newlink_link_net(params); 204 struct pfcp_dev *pfcp = netdev_priv(dev); 205 struct pfcp_net *pn; 206 int err; 207 208 pfcp->net = link_net; 209 210 err = pfcp_add_sock(pfcp); 211 if (err) { 212 netdev_dbg(dev, "failed to add pfcp socket %d\n", err); 213 goto exit_err; 214 } 215 216 err = register_netdevice(dev); 217 if (err) { 218 netdev_dbg(dev, "failed to register pfcp netdev %d\n", err); 219 goto exit_del_pfcp_sock; 220 } 221 222 pn = net_generic(link_net, pfcp_net_id); 223 224 mutex_lock(&pn->lock); 225 list_add(&pfcp->list, &pn->pfcp_dev_list); 226 mutex_unlock(&pn->lock); 227 228 netdev_dbg(dev, "registered new PFCP interface\n"); 229 230 return 0; 231 232 exit_del_pfcp_sock: 233 pfcp_del_sock(pfcp); 234 synchronize_rcu(); 235 exit_err: 236 pfcp->net = NULL; 237 return err; 238 } 239 240 static void __pfcp_dellink(struct net *net, struct net_device *dev, 241 struct list_head *head) 242 { 243 struct pfcp_dev *pfcp = netdev_priv(dev); 244 245 list_del_init(&pfcp->list); 246 unregister_netdevice_queue_net(net, dev, head); 247 } 248 249 static void pfcp_dellink(struct net_device *dev, struct list_head *head) 250 { 251 struct pfcp_dev *pfcp = netdev_priv(dev); 252 struct pfcp_net *pn; 253 254 pn = net_generic(pfcp->net, pfcp_net_id); 255 256 mutex_lock(&pn->lock); 257 if (!list_empty(&pfcp->list)) 258 __pfcp_dellink(dev_net(dev), dev, head); 259 mutex_unlock(&pn->lock); 260 } 261 262 static struct rtnl_link_ops pfcp_link_ops __read_mostly = { 263 .kind = "pfcp", 264 .priv_size = sizeof(struct pfcp_dev), 265 .setup = pfcp_link_setup, 266 .newlink = pfcp_newlink, 267 .dellink = pfcp_dellink, 268 }; 269 270 static int __net_init pfcp_net_init(struct net *net) 271 { 272 struct pfcp_net *pn = net_generic(net, pfcp_net_id); 273 274 INIT_LIST_HEAD(&pn->pfcp_dev_list); 275 mutex_init(&pn->lock); 276 277 return 0; 278 } 279 280 static void __net_exit pfcp_net_exit_rtnl(struct net *net, 281 struct list_head *dev_to_kill) 282 { 283 struct pfcp_net *pn = net_generic(net, pfcp_net_id); 284 struct pfcp_dev *pfcp, *pfcp_next; 285 286 mutex_lock(&pn->lock); 287 288 list_for_each_entry_safe(pfcp, pfcp_next, &pn->pfcp_dev_list, list) 289 __pfcp_dellink(net, pfcp->dev, dev_to_kill); 290 291 mutex_unlock(&pn->lock); 292 } 293 294 static struct pernet_operations pfcp_net_ops = { 295 .init = pfcp_net_init, 296 .exit_rtnl = pfcp_net_exit_rtnl, 297 .id = &pfcp_net_id, 298 .size = sizeof(struct pfcp_net), 299 }; 300 301 static int __init pfcp_init(void) 302 { 303 int err; 304 305 err = register_pernet_subsys(&pfcp_net_ops); 306 if (err) 307 goto exit_err; 308 309 err = rtnl_link_register(&pfcp_link_ops); 310 if (err) 311 goto exit_unregister_subsys; 312 return 0; 313 314 exit_unregister_subsys: 315 unregister_pernet_subsys(&pfcp_net_ops); 316 exit_err: 317 pr_err("loading PFCP module failed: err %d\n", err); 318 return err; 319 } 320 late_initcall(pfcp_init); 321 322 static void __exit pfcp_exit(void) 323 { 324 rtnl_link_unregister(&pfcp_link_ops); 325 unregister_pernet_subsys(&pfcp_net_ops); 326 327 pr_info("PFCP module unloaded\n"); 328 } 329 module_exit(pfcp_exit); 330 331 MODULE_LICENSE("GPL"); 332 MODULE_AUTHOR("Wojciech Drewek <wojciech.drewek@intel.com>"); 333 MODULE_DESCRIPTION("Interface driver for PFCP encapsulated traffic"); 334 MODULE_ALIAS_RTNL_LINK("pfcp"); 335