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
15int nsh_push(struct sk_buff *skb, const struct nshhdr *pushed_nh)
16{
17 struct nshhdr *nh;
18 size_t length = nsh_hdr_len(nsh: 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(proto: skb->protocol);
25 if (!next_proto)
26 return -EAFNOSUPPORT;
27 }
28
29 /* Add the NSH header */
30 if (skb_cow_head(skb, headroom: length) < 0)
31 return -ENOMEM;
32
33 skb_push(skb, len: length);
34 nh = (struct nshhdr *)(skb->data);
35 memcpy(nh, pushed_nh, length);
36 nh->np = next_proto;
37 skb_postpush_rcsum(skb, start: nh, len: 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}
46EXPORT_SYMBOL_GPL(nsh_push);
47
48int 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(nsh: nh);
58 if (length < NSH_BASE_HDR_LEN)
59 return -EINVAL;
60 inner_proto = tun_p_to_eth_p(proto: nh->np);
61 if (!pskb_may_pull(skb, len: length))
62 return -ENOMEM;
63
64 if (!inner_proto)
65 return -EAFNOSUPPORT;
66
67 skb_pull_rcsum(skb, len: 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}
75EXPORT_SYMBOL_GPL(nsh_pop);
76
77static struct sk_buff *nsh_gso_segment(struct sk_buff *skb,
78 netdev_features_t features)
79{
80 unsigned int outer_hlen, mac_len, nsh_len;
81 struct sk_buff *segs = ERR_PTR(error: -EINVAL);
82 u16 mac_offset = skb->mac_header;
83 __be16 outer_proto, proto;
84
85 skb_reset_network_header(skb);
86
87 outer_proto = skb->protocol;
88 outer_hlen = skb_mac_header_len(skb);
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: 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(proto: nsh_hdr(skb)->np);
100 if (!proto)
101 goto out;
102
103 __skb_pull(skb, len: 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(ptr: segs)) {
112 skb_gso_error_unwind(skb, htons(ETH_P_NSH), pulled_hlen: nsh_len,
113 mac_offset, mac_len);
114 goto out;
115 }
116
117 for (skb = segs; skb; skb = skb->next) {
118 skb->protocol = outer_proto;
119 __skb_push(skb, len: nsh_len + outer_hlen);
120 skb_reset_mac_header(skb);
121 skb_set_network_header(skb, offset: outer_hlen);
122 skb->mac_len = mac_len;
123 }
124
125out:
126 return segs;
127}
128
129static 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
137static int __init nsh_init_module(void)
138{
139 dev_add_offload(po: &nsh_packet_offload);
140 return 0;
141}
142
143static void __exit nsh_cleanup_module(void)
144{
145 dev_remove_offload(po: &nsh_packet_offload);
146}
147
148module_init(nsh_init_module);
149module_exit(nsh_cleanup_module);
150
151MODULE_AUTHOR("Jiri Benc <jbenc@redhat.com>");
152MODULE_DESCRIPTION("NSH protocol");
153MODULE_LICENSE("GPL v2");
154

source code of linux/net/nsh/nsh.c