Merge tag 'pwm/for-6.5-rc1' of git://git.kernel.org/pub/scm/linux/kernel/git/thierry...
[linux-2.6-microblaze.git] / net / nsh / nsh.c
1 // SPDX-License-Identifier: GPL-2.0-only
2 /*
3  * Network Service Header
4  *
5  * Copyright (c) 2017 Red Hat, Inc. -- Jiri Benc <jbenc@redhat.com>
6  */
7
8 #include <linux/module.h>
9 #include <linux/netdevice.h>
10 #include <linux/skbuff.h>
11 #include <net/gso.h>
12 #include <net/nsh.h>
13 #include <net/tun_proto.h>
14
15 int nsh_push(struct sk_buff *skb, const struct nshhdr *pushed_nh)
16 {
17         struct nshhdr *nh;
18         size_t length = nsh_hdr_len(pushed_nh);
19         u8 next_proto;
20
21         if (skb->mac_len) {
22                 next_proto = TUN_P_ETHERNET;
23         } else {
24                 next_proto = tun_p_from_eth_p(skb->protocol);
25                 if (!next_proto)
26                         return -EAFNOSUPPORT;
27         }
28
29         /* Add the NSH header */
30         if (skb_cow_head(skb, length) < 0)
31                 return -ENOMEM;
32
33         skb_push(skb, length);
34         nh = (struct nshhdr *)(skb->data);
35         memcpy(nh, pushed_nh, length);
36         nh->np = next_proto;
37         skb_postpush_rcsum(skb, nh, length);
38
39         skb->protocol = htons(ETH_P_NSH);
40         skb_reset_mac_header(skb);
41         skb_reset_network_header(skb);
42         skb_reset_mac_len(skb);
43
44         return 0;
45 }
46 EXPORT_SYMBOL_GPL(nsh_push);
47
48 int nsh_pop(struct sk_buff *skb)
49 {
50         struct nshhdr *nh;
51         size_t length;
52         __be16 inner_proto;
53
54         if (!pskb_may_pull(skb, NSH_BASE_HDR_LEN))
55                 return -ENOMEM;
56         nh = (struct nshhdr *)(skb->data);
57         length = nsh_hdr_len(nh);
58         if (length < NSH_BASE_HDR_LEN)
59                 return -EINVAL;
60         inner_proto = tun_p_to_eth_p(nh->np);
61         if (!pskb_may_pull(skb, length))
62                 return -ENOMEM;
63
64         if (!inner_proto)
65                 return -EAFNOSUPPORT;
66
67         skb_pull_rcsum(skb, length);
68         skb_reset_mac_header(skb);
69         skb_reset_network_header(skb);
70         skb_reset_mac_len(skb);
71         skb->protocol = inner_proto;
72
73         return 0;
74 }
75 EXPORT_SYMBOL_GPL(nsh_pop);
76
77 static struct sk_buff *nsh_gso_segment(struct sk_buff *skb,
78                                        netdev_features_t features)
79 {
80         struct sk_buff *segs = ERR_PTR(-EINVAL);
81         u16 mac_offset = skb->mac_header;
82         unsigned int nsh_len, mac_len;
83         __be16 proto;
84
85         skb_reset_network_header(skb);
86
87         mac_len = skb->mac_len;
88
89         if (unlikely(!pskb_may_pull(skb, NSH_BASE_HDR_LEN)))
90                 goto out;
91         nsh_len = nsh_hdr_len(nsh_hdr(skb));
92         if (nsh_len < NSH_BASE_HDR_LEN)
93                 goto out;
94         if (unlikely(!pskb_may_pull(skb, nsh_len)))
95                 goto out;
96
97         proto = tun_p_to_eth_p(nsh_hdr(skb)->np);
98         if (!proto)
99                 goto out;
100
101         __skb_pull(skb, nsh_len);
102
103         skb_reset_mac_header(skb);
104         skb->mac_len = proto == htons(ETH_P_TEB) ? ETH_HLEN : 0;
105         skb->protocol = proto;
106
107         features &= NETIF_F_SG;
108         segs = skb_mac_gso_segment(skb, features);
109         if (IS_ERR_OR_NULL(segs)) {
110                 skb_gso_error_unwind(skb, htons(ETH_P_NSH), nsh_len,
111                                      mac_offset, mac_len);
112                 goto out;
113         }
114
115         for (skb = segs; skb; skb = skb->next) {
116                 skb->protocol = htons(ETH_P_NSH);
117                 __skb_push(skb, nsh_len);
118                 skb->mac_header = mac_offset;
119                 skb->network_header = skb->mac_header + mac_len;
120                 skb->mac_len = mac_len;
121         }
122
123 out:
124         return segs;
125 }
126
127 static struct packet_offload nsh_packet_offload __read_mostly = {
128         .type = htons(ETH_P_NSH),
129         .priority = 15,
130         .callbacks = {
131                 .gso_segment = nsh_gso_segment,
132         },
133 };
134
135 static int __init nsh_init_module(void)
136 {
137         dev_add_offload(&nsh_packet_offload);
138         return 0;
139 }
140
141 static void __exit nsh_cleanup_module(void)
142 {
143         dev_remove_offload(&nsh_packet_offload);
144 }
145
146 module_init(nsh_init_module);
147 module_exit(nsh_cleanup_module);
148
149 MODULE_AUTHOR("Jiri Benc <jbenc@redhat.com>");
150 MODULE_DESCRIPTION("NSH protocol");
151 MODULE_LICENSE("GPL v2");