Commit | Line | Data |
---|---|---|
d2912cb1 | 1 | // SPDX-License-Identifier: GPL-2.0-only |
c411ed85 JB |
2 | /* |
3 | * Network Service Header | |
4 | * | |
5 | * Copyright (c) 2017 Red Hat, Inc. -- Jiri Benc <jbenc@redhat.com> | |
c411ed85 JB |
6 | */ |
7 | ||
8 | #include <linux/module.h> | |
9 | #include <linux/netdevice.h> | |
10 | #include <linux/skbuff.h> | |
d457a0e3 | 11 | #include <net/gso.h> |
c411ed85 JB |
12 | #include <net/nsh.h> |
13 | #include <net/tun_proto.h> | |
14 | ||
b2d0f5d5 YY |
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(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(skb->protocol); | |
25 | if (!next_proto) | |
26 | return -EAFNOSUPPORT; | |
27 | } | |
28 | ||
29 | /* Add the NSH header */ | |
30 | if (skb_cow_head(skb, length) < 0) | |
31 | return -ENOMEM; | |
32 | ||
33 | skb_push(skb, length); | |
34 | nh = (struct nshhdr *)(skb->data); | |
35 | memcpy(nh, pushed_nh, length); | |
36 | nh->np = next_proto; | |
37 | skb_postpush_rcsum(skb, nh, 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(nh); | |
af50e4ba ED |
58 | if (length < NSH_BASE_HDR_LEN) |
59 | return -EINVAL; | |
b2d0f5d5 YY |
60 | inner_proto = tun_p_to_eth_p(nh->np); |
61 | if (!pskb_may_pull(skb, length)) | |
62 | return -ENOMEM; | |
63 | ||
64 | if (!inner_proto) | |
65 | return -EAFNOSUPPORT; | |
66 | ||
67 | skb_pull_rcsum(skb, 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 | ||
c411ed85 JB |
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(-EINVAL); | |
c83b4938 | 81 | u16 mac_offset = skb->mac_header; |
c411ed85 JB |
82 | unsigned int nsh_len, mac_len; |
83 | __be16 proto; | |
c411ed85 JB |
84 | |
85 | skb_reset_network_header(skb); | |
86 | ||
c411ed85 JB |
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)); | |
af50e4ba ED |
92 | if (nsh_len < NSH_BASE_HDR_LEN) |
93 | goto out; | |
c411ed85 JB |
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); | |
bab2c80e | 104 | skb->mac_len = proto == htons(ETH_P_TEB) ? ETH_HLEN : 0; |
c411ed85 JB |
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, | |
c83b4938 | 111 | mac_offset, mac_len); |
c411ed85 JB |
112 | goto out; |
113 | } | |
114 | ||
115 | for (skb = segs; skb; skb = skb->next) { | |
116 | skb->protocol = htons(ETH_P_NSH); | |
117 | __skb_push(skb, nsh_len); | |
c83b4938 | 118 | skb->mac_header = mac_offset; |
c411ed85 JB |
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(&nsh_packet_offload); | |
138 | return 0; | |
139 | } | |
140 | ||
141 | static void __exit nsh_cleanup_module(void) | |
142 | { | |
143 | dev_remove_offload(&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"); |