Commit | Line | Data |
---|---|---|
65d7ab8d TH |
1 | #include <linux/errno.h> |
2 | #include <linux/ip.h> | |
3 | #include <linux/kernel.h> | |
4 | #include <linux/module.h> | |
5 | #include <linux/skbuff.h> | |
6 | #include <linux/socket.h> | |
7 | #include <linux/types.h> | |
8 | #include <net/checksum.h> | |
9 | #include <net/ip.h> | |
10 | #include <net/ip6_fib.h> | |
11 | #include <net/lwtunnel.h> | |
12 | #include <net/protocol.h> | |
13 | #include <uapi/linux/ila.h> | |
14 | ||
15 | struct ila_params { | |
16 | __be64 locator; | |
92b78aff TH |
17 | __be64 locator_match; |
18 | __wsum csum_diff; | |
65d7ab8d TH |
19 | }; |
20 | ||
21 | static inline struct ila_params *ila_params_lwtunnel( | |
22 | struct lwtunnel_state *lwstate) | |
23 | { | |
24 | return (struct ila_params *)lwstate->data; | |
25 | } | |
26 | ||
27 | static inline __wsum compute_csum_diff8(const __be32 *from, const __be32 *to) | |
28 | { | |
29 | __be32 diff[] = { | |
30 | ~from[0], ~from[1], to[0], to[1], | |
31 | }; | |
32 | ||
33 | return csum_partial(diff, sizeof(diff), 0); | |
34 | } | |
35 | ||
36 | static inline __wsum get_csum_diff(struct ipv6hdr *ip6h, struct ila_params *p) | |
37 | { | |
92b78aff TH |
38 | if (*(__be64 *)&ip6h->daddr == p->locator_match) |
39 | return p->csum_diff; | |
40 | else | |
65d7ab8d TH |
41 | return compute_csum_diff8((__be32 *)&ip6h->daddr, |
42 | (__be32 *)&p->locator); | |
43 | } | |
44 | ||
45 | static void update_ipv6_locator(struct sk_buff *skb, struct ila_params *p) | |
46 | { | |
47 | __wsum diff; | |
48 | struct ipv6hdr *ip6h = ipv6_hdr(skb); | |
49 | size_t nhoff = sizeof(struct ipv6hdr); | |
50 | ||
51 | /* First update checksum */ | |
52 | switch (ip6h->nexthdr) { | |
53 | case NEXTHDR_TCP: | |
54 | if (likely(pskb_may_pull(skb, nhoff + sizeof(struct tcphdr)))) { | |
55 | struct tcphdr *th = (struct tcphdr *) | |
56 | (skb_network_header(skb) + nhoff); | |
57 | ||
58 | diff = get_csum_diff(ip6h, p); | |
59 | inet_proto_csum_replace_by_diff(&th->check, skb, | |
60 | diff, true); | |
61 | } | |
62 | break; | |
63 | case NEXTHDR_UDP: | |
64 | if (likely(pskb_may_pull(skb, nhoff + sizeof(struct udphdr)))) { | |
65 | struct udphdr *uh = (struct udphdr *) | |
66 | (skb_network_header(skb) + nhoff); | |
67 | ||
68 | if (uh->check || skb->ip_summed == CHECKSUM_PARTIAL) { | |
69 | diff = get_csum_diff(ip6h, p); | |
70 | inet_proto_csum_replace_by_diff(&uh->check, skb, | |
71 | diff, true); | |
72 | if (!uh->check) | |
73 | uh->check = CSUM_MANGLED_0; | |
74 | } | |
75 | } | |
76 | break; | |
77 | case NEXTHDR_ICMP: | |
78 | if (likely(pskb_may_pull(skb, | |
79 | nhoff + sizeof(struct icmp6hdr)))) { | |
80 | struct icmp6hdr *ih = (struct icmp6hdr *) | |
81 | (skb_network_header(skb) + nhoff); | |
82 | ||
83 | diff = get_csum_diff(ip6h, p); | |
84 | inet_proto_csum_replace_by_diff(&ih->icmp6_cksum, skb, | |
85 | diff, true); | |
86 | } | |
87 | break; | |
88 | } | |
89 | ||
90 | /* Now change destination address */ | |
91 | *(__be64 *)&ip6h->daddr = p->locator; | |
92 | } | |
93 | ||
94 | static int ila_output(struct sock *sk, struct sk_buff *skb) | |
95 | { | |
96 | struct dst_entry *dst = skb_dst(skb); | |
65d7ab8d TH |
97 | |
98 | if (skb->protocol != htons(ETH_P_IPV6)) | |
99 | goto drop; | |
100 | ||
61adedf3 | 101 | update_ipv6_locator(skb, ila_params_lwtunnel(dst->lwtstate)); |
65d7ab8d | 102 | |
61adedf3 | 103 | return dst->lwtstate->orig_output(sk, skb); |
65d7ab8d TH |
104 | |
105 | drop: | |
106 | kfree_skb(skb); | |
107 | return -EINVAL; | |
108 | } | |
109 | ||
110 | static int ila_input(struct sk_buff *skb) | |
111 | { | |
112 | struct dst_entry *dst = skb_dst(skb); | |
65d7ab8d TH |
113 | |
114 | if (skb->protocol != htons(ETH_P_IPV6)) | |
115 | goto drop; | |
116 | ||
61adedf3 | 117 | update_ipv6_locator(skb, ila_params_lwtunnel(dst->lwtstate)); |
65d7ab8d | 118 | |
61adedf3 | 119 | return dst->lwtstate->orig_input(skb); |
65d7ab8d TH |
120 | |
121 | drop: | |
122 | kfree_skb(skb); | |
123 | return -EINVAL; | |
124 | } | |
125 | ||
126 | static struct nla_policy ila_nl_policy[ILA_ATTR_MAX + 1] = { | |
127 | [ILA_ATTR_LOCATOR] = { .type = NLA_U64, }, | |
128 | }; | |
129 | ||
130 | static int ila_build_state(struct net_device *dev, struct nlattr *nla, | |
127eb7cd | 131 | unsigned int family, const void *cfg, |
65d7ab8d TH |
132 | struct lwtunnel_state **ts) |
133 | { | |
134 | struct ila_params *p; | |
135 | struct nlattr *tb[ILA_ATTR_MAX + 1]; | |
136 | size_t encap_len = sizeof(*p); | |
137 | struct lwtunnel_state *newts; | |
92b78aff | 138 | const struct fib6_config *cfg6 = cfg; |
65d7ab8d TH |
139 | int ret; |
140 | ||
92b78aff TH |
141 | if (family != AF_INET6) |
142 | return -EINVAL; | |
143 | ||
65d7ab8d TH |
144 | ret = nla_parse_nested(tb, ILA_ATTR_MAX, nla, |
145 | ila_nl_policy); | |
146 | if (ret < 0) | |
147 | return ret; | |
148 | ||
149 | if (!tb[ILA_ATTR_LOCATOR]) | |
150 | return -EINVAL; | |
151 | ||
152 | newts = lwtunnel_state_alloc(encap_len); | |
153 | if (!newts) | |
154 | return -ENOMEM; | |
155 | ||
156 | newts->len = encap_len; | |
157 | p = ila_params_lwtunnel(newts); | |
158 | ||
159 | p->locator = (__force __be64)nla_get_u64(tb[ILA_ATTR_LOCATOR]); | |
160 | ||
92b78aff TH |
161 | if (cfg6->fc_dst_len > sizeof(__be64)) { |
162 | /* Precompute checksum difference for translation since we | |
163 | * know both the old locator and the new one. | |
164 | */ | |
165 | p->locator_match = *(__be64 *)&cfg6->fc_dst; | |
166 | p->csum_diff = compute_csum_diff8( | |
167 | (__be32 *)&p->locator_match, (__be32 *)&p->locator); | |
168 | } | |
169 | ||
65d7ab8d TH |
170 | newts->type = LWTUNNEL_ENCAP_ILA; |
171 | newts->flags |= LWTUNNEL_STATE_OUTPUT_REDIRECT | | |
172 | LWTUNNEL_STATE_INPUT_REDIRECT; | |
173 | ||
174 | *ts = newts; | |
175 | ||
176 | return 0; | |
177 | } | |
178 | ||
179 | static int ila_fill_encap_info(struct sk_buff *skb, | |
180 | struct lwtunnel_state *lwtstate) | |
181 | { | |
182 | struct ila_params *p = ila_params_lwtunnel(lwtstate); | |
183 | ||
184 | if (nla_put_u64(skb, ILA_ATTR_LOCATOR, (__force u64)p->locator)) | |
185 | goto nla_put_failure; | |
186 | ||
187 | return 0; | |
188 | ||
189 | nla_put_failure: | |
190 | return -EMSGSIZE; | |
191 | } | |
192 | ||
193 | static int ila_encap_nlsize(struct lwtunnel_state *lwtstate) | |
194 | { | |
195 | /* No encapsulation overhead */ | |
196 | return 0; | |
197 | } | |
198 | ||
199 | static int ila_encap_cmp(struct lwtunnel_state *a, struct lwtunnel_state *b) | |
200 | { | |
201 | struct ila_params *a_p = ila_params_lwtunnel(a); | |
202 | struct ila_params *b_p = ila_params_lwtunnel(b); | |
203 | ||
204 | return (a_p->locator != b_p->locator); | |
205 | } | |
206 | ||
207 | static const struct lwtunnel_encap_ops ila_encap_ops = { | |
208 | .build_state = ila_build_state, | |
209 | .output = ila_output, | |
210 | .input = ila_input, | |
211 | .fill_encap = ila_fill_encap_info, | |
212 | .get_encap_size = ila_encap_nlsize, | |
213 | .cmp_encap = ila_encap_cmp, | |
214 | }; | |
215 | ||
216 | static int __init ila_init(void) | |
217 | { | |
218 | return lwtunnel_encap_add_ops(&ila_encap_ops, LWTUNNEL_ENCAP_ILA); | |
219 | } | |
220 | ||
221 | static void __exit ila_fini(void) | |
222 | { | |
223 | lwtunnel_encap_del_ops(&ila_encap_ops, LWTUNNEL_ENCAP_ILA); | |
224 | } | |
225 | ||
226 | module_init(ila_init); | |
227 | module_exit(ila_fini); | |
228 | MODULE_AUTHOR("Tom Herbert <tom@herbertland.com>"); | |
229 | MODULE_LICENSE("GPL"); |