Loading...
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
14int 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}
45EXPORT_SYMBOL_GPL(nsh_push);
46
47int 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}
74EXPORT_SYMBOL_GPL(nsh_pop);
75
76static 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 unsigned int nsh_len, mac_len;
81 __be16 proto;
82 int nhoff;
83
84 skb_reset_network_header(skb);
85
86 nhoff = skb->network_header - skb->mac_header;
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 skb->network_header - nhoff,
112 mac_len);
113 goto out;
114 }
115
116 for (skb = segs; skb; skb = skb->next) {
117 skb->protocol = htons(ETH_P_NSH);
118 __skb_push(skb, nsh_len);
119 skb_set_mac_header(skb, -nhoff);
120 skb->network_header = skb->mac_header + mac_len;
121 skb->mac_len = mac_len;
122 }
123
124out:
125 return segs;
126}
127
128static struct packet_offload nsh_packet_offload __read_mostly = {
129 .type = htons(ETH_P_NSH),
130 .priority = 15,
131 .callbacks = {
132 .gso_segment = nsh_gso_segment,
133 },
134};
135
136static int __init nsh_init_module(void)
137{
138 dev_add_offload(&nsh_packet_offload);
139 return 0;
140}
141
142static void __exit nsh_cleanup_module(void)
143{
144 dev_remove_offload(&nsh_packet_offload);
145}
146
147module_init(nsh_init_module);
148module_exit(nsh_cleanup_module);
149
150MODULE_AUTHOR("Jiri Benc <jbenc@redhat.com>");
151MODULE_DESCRIPTION("NSH protocol");
152MODULE_LICENSE("GPL v2");
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
17int 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}
48EXPORT_SYMBOL_GPL(nsh_push);
49
50int 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}
77EXPORT_SYMBOL_GPL(nsh_pop);
78
79static 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 unsigned int nsh_len, mac_len;
84 __be16 proto;
85 int nhoff;
86
87 skb_reset_network_header(skb);
88
89 nhoff = skb->network_header - skb->mac_header;
90 mac_len = skb->mac_len;
91
92 if (unlikely(!pskb_may_pull(skb, NSH_BASE_HDR_LEN)))
93 goto out;
94 nsh_len = nsh_hdr_len(nsh_hdr(skb));
95 if (nsh_len < NSH_BASE_HDR_LEN)
96 goto out;
97 if (unlikely(!pskb_may_pull(skb, nsh_len)))
98 goto out;
99
100 proto = tun_p_to_eth_p(nsh_hdr(skb)->np);
101 if (!proto)
102 goto out;
103
104 __skb_pull(skb, nsh_len);
105
106 skb_reset_mac_header(skb);
107 skb_reset_mac_len(skb);
108 skb->protocol = proto;
109
110 features &= NETIF_F_SG;
111 segs = skb_mac_gso_segment(skb, features);
112 if (IS_ERR_OR_NULL(segs)) {
113 skb_gso_error_unwind(skb, htons(ETH_P_NSH), nsh_len,
114 skb->network_header - nhoff,
115 mac_len);
116 goto out;
117 }
118
119 for (skb = segs; skb; skb = skb->next) {
120 skb->protocol = htons(ETH_P_NSH);
121 __skb_push(skb, nsh_len);
122 skb_set_mac_header(skb, -nhoff);
123 skb->network_header = skb->mac_header + mac_len;
124 skb->mac_len = mac_len;
125 }
126
127out:
128 return segs;
129}
130
131static struct packet_offload nsh_packet_offload __read_mostly = {
132 .type = htons(ETH_P_NSH),
133 .priority = 15,
134 .callbacks = {
135 .gso_segment = nsh_gso_segment,
136 },
137};
138
139static int __init nsh_init_module(void)
140{
141 dev_add_offload(&nsh_packet_offload);
142 return 0;
143}
144
145static void __exit nsh_cleanup_module(void)
146{
147 dev_remove_offload(&nsh_packet_offload);
148}
149
150module_init(nsh_init_module);
151module_exit(nsh_cleanup_module);
152
153MODULE_AUTHOR("Jiri Benc <jbenc@redhat.com>");
154MODULE_DESCRIPTION("NSH protocol");
155MODULE_LICENSE("GPL v2");