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 socket *sock;
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
pfcp_session_recv(struct pfcp_dev * pfcp,struct sk_buff * skb,struct pfcp_metadata * md)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
pfcp_node_recv(struct pfcp_dev * pfcp,struct sk_buff * skb,struct pfcp_metadata * md)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
pfcp_encap_recv(struct sock * sk,struct sk_buff * skb)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
pfcp_del_sock(struct pfcp_dev * pfcp)105 static void pfcp_del_sock(struct pfcp_dev *pfcp)
106 {
107 udp_tunnel_sock_release(pfcp->sock);
108 pfcp->sock = NULL;
109 }
110
pfcp_dev_uninit(struct net_device * dev)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
pfcp_dev_init(struct net_device * dev)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
pfcp_link_setup(struct net_device * dev)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 netif_keep_dst(dev);
152 }
153
pfcp_create_sock(struct pfcp_dev * pfcp)154 static 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
pfcp_add_sock(struct pfcp_dev * pfcp)180 static 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
pfcp_newlink(struct net_device * dev,struct rtnl_newlink_params * params,struct netlink_ext_ack * extack)187 static int pfcp_newlink(struct net_device *dev,
188 struct rtnl_newlink_params *params,
189 struct netlink_ext_ack *extack)
190 {
191 struct net *link_net = rtnl_newlink_link_net(params);
192 struct pfcp_dev *pfcp = netdev_priv(dev);
193 struct pfcp_net *pn;
194 int err;
195
196 pfcp->net = link_net;
197
198 err = pfcp_add_sock(pfcp);
199 if (err) {
200 netdev_dbg(dev, "failed to add pfcp socket %d\n", err);
201 goto exit_err;
202 }
203
204 err = register_netdevice(dev);
205 if (err) {
206 netdev_dbg(dev, "failed to register pfcp netdev %d\n", err);
207 goto exit_del_pfcp_sock;
208 }
209
210 pn = net_generic(link_net, pfcp_net_id);
211 list_add(&pfcp->list, &pn->pfcp_dev_list);
212
213 netdev_dbg(dev, "registered new PFCP interface\n");
214
215 return 0;
216
217 exit_del_pfcp_sock:
218 pfcp_del_sock(pfcp);
219 exit_err:
220 pfcp->net = NULL;
221 return err;
222 }
223
pfcp_dellink(struct net_device * dev,struct list_head * head)224 static void pfcp_dellink(struct net_device *dev, struct list_head *head)
225 {
226 struct pfcp_dev *pfcp = netdev_priv(dev);
227
228 list_del(&pfcp->list);
229 unregister_netdevice_queue(dev, head);
230 }
231
232 static struct rtnl_link_ops pfcp_link_ops __read_mostly = {
233 .kind = "pfcp",
234 .priv_size = sizeof(struct pfcp_dev),
235 .setup = pfcp_link_setup,
236 .newlink = pfcp_newlink,
237 .dellink = pfcp_dellink,
238 };
239
pfcp_net_init(struct net * net)240 static int __net_init pfcp_net_init(struct net *net)
241 {
242 struct pfcp_net *pn = net_generic(net, pfcp_net_id);
243
244 INIT_LIST_HEAD(&pn->pfcp_dev_list);
245 return 0;
246 }
247
pfcp_net_exit_rtnl(struct net * net,struct list_head * dev_to_kill)248 static void __net_exit pfcp_net_exit_rtnl(struct net *net,
249 struct list_head *dev_to_kill)
250 {
251 struct pfcp_net *pn = net_generic(net, pfcp_net_id);
252 struct pfcp_dev *pfcp, *pfcp_next;
253
254 list_for_each_entry_safe(pfcp, pfcp_next, &pn->pfcp_dev_list, list)
255 pfcp_dellink(pfcp->dev, dev_to_kill);
256 }
257
258 static struct pernet_operations pfcp_net_ops = {
259 .init = pfcp_net_init,
260 .exit_rtnl = pfcp_net_exit_rtnl,
261 .id = &pfcp_net_id,
262 .size = sizeof(struct pfcp_net),
263 };
264
pfcp_init(void)265 static int __init pfcp_init(void)
266 {
267 int err;
268
269 err = register_pernet_subsys(&pfcp_net_ops);
270 if (err)
271 goto exit_err;
272
273 err = rtnl_link_register(&pfcp_link_ops);
274 if (err)
275 goto exit_unregister_subsys;
276 return 0;
277
278 exit_unregister_subsys:
279 unregister_pernet_subsys(&pfcp_net_ops);
280 exit_err:
281 pr_err("loading PFCP module failed: err %d\n", err);
282 return err;
283 }
284 late_initcall(pfcp_init);
285
pfcp_exit(void)286 static void __exit pfcp_exit(void)
287 {
288 rtnl_link_unregister(&pfcp_link_ops);
289 unregister_pernet_subsys(&pfcp_net_ops);
290
291 pr_info("PFCP module unloaded\n");
292 }
293 module_exit(pfcp_exit);
294
295 MODULE_LICENSE("GPL");
296 MODULE_AUTHOR("Wojciech Drewek <wojciech.drewek@intel.com>");
297 MODULE_DESCRIPTION("Interface driver for PFCP encapsulated traffic");
298 MODULE_ALIAS_RTNL_LINK("pfcp");
299