projects/linux·blob master · refs

334 lines · 7.0 KiB

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 sock *sk;
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 struct mutex lock;
33};
34
35static void
36pfcp_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
45static void
46pfcp_node_recv(struct pfcp_dev *pfcp, struct sk_buff *skb,
47 struct pfcp_metadata *md)
48{
49 md->type = PFCP_TYPE_NODE;
50}
51
52static 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;
108drop:
109 kfree_skb(skb);
110 return 0;
111}
112
113static 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
122static 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
130static 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
139static 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
145static const struct device_type pfcp_type = {
146 .name = "pfcp",
147};
148
149static 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
166static 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
192static 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
199static 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
232exit_del_pfcp_sock:
233 pfcp_del_sock(pfcp);
234 synchronize_rcu();
235exit_err:
236 pfcp->net = NULL;
237 return err;
238}
239
240static 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
249static 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
262static 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
270static 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
280static 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
294static 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
301static 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
314exit_unregister_subsys:
315 unregister_pernet_subsys(&pfcp_net_ops);
316exit_err:
317 pr_err("loading PFCP module failed: err %d\n", err);
318 return err;
319}
320late_initcall(pfcp_init);
321
322static 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}
329module_exit(pfcp_exit);
330
331MODULE_LICENSE("GPL");
332MODULE_AUTHOR("Wojciech Drewek <[email protected]>");
333MODULE_DESCRIPTION("Interface driver for PFCP encapsulated traffic");
334MODULE_ALIAS_RTNL_LINK("pfcp");