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(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 | } |
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(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 | } |
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(error: -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: 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(proto: nsh_hdr(skb)->np); |
98 | if (!proto) |
99 | goto out; |
100 | |
101 | __skb_pull(skb, len: 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(ptr: segs)) { |
110 | skb_gso_error_unwind(skb, htons(ETH_P_NSH), pulled_hlen: 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, len: 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(po: &nsh_packet_offload); |
138 | return 0; |
139 | } |
140 | |
141 | static void __exit nsh_cleanup_module(void) |
142 | { |
143 | dev_remove_offload(po: &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" ); |
152 | |