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