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
 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 void
 35pfcp_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 void
 45pfcp_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	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;
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(net, pfcp_net_id);
210	list_add(&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(&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, *pfcp_next;
251	struct net_device *dev;
252	LIST_HEAD(list);
253
254	rtnl_lock();
255	for_each_netdev(net, dev)
256		if (dev->rtnl_link_ops == &pfcp_link_ops)
257			pfcp_dellink(dev, &list);
258
259	list_for_each_entry_safe(pfcp, pfcp_next, &pn->pfcp_dev_list, list)
260		pfcp_dellink(pfcp->dev, &list);
261
262	unregister_netdevice_many(&list);
263	rtnl_unlock();
264}
265
266static struct pernet_operations pfcp_net_ops = {
267	.init	= pfcp_net_init,
268	.exit	= pfcp_net_exit,
269	.id	= &pfcp_net_id,
270	.size	= sizeof(struct pfcp_net),
271};
272
273static int __init pfcp_init(void)
274{
275	int err;
276
277	err = register_pernet_subsys(&pfcp_net_ops);
278	if (err)
279		goto exit_err;
280
281	err = rtnl_link_register(&pfcp_link_ops);
282	if (err)
283		goto exit_unregister_subsys;
284	return 0;
285
286exit_unregister_subsys:
287	unregister_pernet_subsys(&pfcp_net_ops);
288exit_err:
289	pr_err("loading PFCP module failed: err %d\n", err);
290	return err;
291}
292late_initcall(pfcp_init);
293
294static void __exit pfcp_exit(void)
295{
296	rtnl_link_unregister(&pfcp_link_ops);
297	unregister_pernet_subsys(&pfcp_net_ops);
298
299	pr_info("PFCP module unloaded\n");
300}
301module_exit(pfcp_exit);
302
303MODULE_LICENSE("GPL");
304MODULE_AUTHOR("Wojciech Drewek <wojciech.drewek@intel.com>");
305MODULE_DESCRIPTION("Interface driver for PFCP encapsulated traffic");
306MODULE_ALIAS_RTNL_LINK("pfcp");