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