Commit | Line | Data |
---|---|---|
a7a29f9c AA |
1 | // SPDX-License-Identifier: GPL-2.0-only |
2 | /** | |
3 | * Authors: | |
4 | * (C) 2020 Alexander Aring <alex.aring@gmail.com> | |
5 | */ | |
6 | ||
7 | #include <linux/rpl_iptunnel.h> | |
8 | ||
9 | #include <net/dst_cache.h> | |
10 | #include <net/ip6_route.h> | |
11 | #include <net/lwtunnel.h> | |
12 | #include <net/ipv6.h> | |
13 | #include <net/rpl.h> | |
14 | ||
15 | struct rpl_iptunnel_encap { | |
16 | struct ipv6_rpl_sr_hdr srh[0]; | |
17 | }; | |
18 | ||
19 | struct rpl_lwt { | |
20 | struct dst_cache cache; | |
21 | struct rpl_iptunnel_encap tuninfo; | |
22 | }; | |
23 | ||
24 | static inline struct rpl_lwt *rpl_lwt_lwtunnel(struct lwtunnel_state *lwt) | |
25 | { | |
26 | return (struct rpl_lwt *)lwt->data; | |
27 | } | |
28 | ||
29 | static inline struct rpl_iptunnel_encap * | |
30 | rpl_encap_lwtunnel(struct lwtunnel_state *lwt) | |
31 | { | |
32 | return &rpl_lwt_lwtunnel(lwt)->tuninfo; | |
33 | } | |
34 | ||
35 | static const struct nla_policy rpl_iptunnel_policy[RPL_IPTUNNEL_MAX + 1] = { | |
36 | [RPL_IPTUNNEL_SRH] = { .type = NLA_BINARY }, | |
37 | }; | |
38 | ||
39 | static bool rpl_validate_srh(struct net *net, struct ipv6_rpl_sr_hdr *srh, | |
40 | size_t seglen) | |
41 | { | |
42 | int err; | |
43 | ||
44 | if ((srh->hdrlen << 3) != seglen) | |
45 | return false; | |
46 | ||
47 | /* check at least one segment and seglen fit with segments_left */ | |
48 | if (!srh->segments_left || | |
49 | (srh->segments_left * sizeof(struct in6_addr)) != seglen) | |
50 | return false; | |
51 | ||
52 | if (srh->cmpri || srh->cmpre) | |
53 | return false; | |
54 | ||
55 | err = ipv6_chk_rpl_srh_loop(net, srh->rpl_segaddr, | |
56 | srh->segments_left); | |
57 | if (err) | |
58 | return false; | |
59 | ||
60 | if (ipv6_addr_type(&srh->rpl_segaddr[srh->segments_left - 1]) & | |
61 | IPV6_ADDR_MULTICAST) | |
62 | return false; | |
63 | ||
64 | return true; | |
65 | } | |
66 | ||
67 | static int rpl_build_state(struct net *net, struct nlattr *nla, | |
68 | unsigned int family, const void *cfg, | |
69 | struct lwtunnel_state **ts, | |
70 | struct netlink_ext_ack *extack) | |
71 | { | |
72 | struct nlattr *tb[RPL_IPTUNNEL_MAX + 1]; | |
73 | struct lwtunnel_state *newts; | |
74 | struct ipv6_rpl_sr_hdr *srh; | |
75 | struct rpl_lwt *rlwt; | |
76 | int err, srh_len; | |
77 | ||
78 | if (family != AF_INET6) | |
79 | return -EINVAL; | |
80 | ||
81 | err = nla_parse_nested(tb, RPL_IPTUNNEL_MAX, nla, | |
82 | rpl_iptunnel_policy, extack); | |
83 | if (err < 0) | |
84 | return err; | |
85 | ||
86 | if (!tb[RPL_IPTUNNEL_SRH]) | |
87 | return -EINVAL; | |
88 | ||
89 | srh = nla_data(tb[RPL_IPTUNNEL_SRH]); | |
90 | srh_len = nla_len(tb[RPL_IPTUNNEL_SRH]); | |
91 | ||
92 | if (srh_len < sizeof(*srh)) | |
93 | return -EINVAL; | |
94 | ||
95 | /* verify that SRH is consistent */ | |
96 | if (!rpl_validate_srh(net, srh, srh_len - sizeof(*srh))) | |
97 | return -EINVAL; | |
98 | ||
99 | newts = lwtunnel_state_alloc(srh_len + sizeof(*rlwt)); | |
100 | if (!newts) | |
101 | return -ENOMEM; | |
102 | ||
103 | rlwt = rpl_lwt_lwtunnel(newts); | |
104 | ||
105 | err = dst_cache_init(&rlwt->cache, GFP_ATOMIC); | |
106 | if (err) { | |
107 | kfree(newts); | |
108 | return err; | |
109 | } | |
110 | ||
111 | memcpy(&rlwt->tuninfo.srh, srh, srh_len); | |
112 | ||
113 | newts->type = LWTUNNEL_ENCAP_RPL; | |
114 | newts->flags |= LWTUNNEL_STATE_INPUT_REDIRECT; | |
115 | newts->flags |= LWTUNNEL_STATE_OUTPUT_REDIRECT; | |
116 | ||
117 | *ts = newts; | |
118 | ||
119 | return 0; | |
120 | } | |
121 | ||
122 | static void rpl_destroy_state(struct lwtunnel_state *lwt) | |
123 | { | |
124 | dst_cache_destroy(&rpl_lwt_lwtunnel(lwt)->cache); | |
125 | } | |
126 | ||
127 | static int rpl_do_srh_inline(struct sk_buff *skb, const struct rpl_lwt *rlwt, | |
128 | const struct ipv6_rpl_sr_hdr *srh) | |
129 | { | |
130 | struct ipv6_rpl_sr_hdr *isrh, *csrh; | |
131 | const struct ipv6hdr *oldhdr; | |
132 | struct ipv6hdr *hdr; | |
133 | unsigned char *buf; | |
134 | size_t hdrlen; | |
135 | int err; | |
136 | ||
137 | oldhdr = ipv6_hdr(skb); | |
138 | ||
139 | buf = kzalloc(ipv6_rpl_srh_alloc_size(srh->segments_left - 1) * 2, | |
140 | GFP_ATOMIC); | |
141 | if (!buf) | |
142 | return -ENOMEM; | |
143 | ||
144 | isrh = (struct ipv6_rpl_sr_hdr *)buf; | |
145 | csrh = (struct ipv6_rpl_sr_hdr *)(buf + ((srh->hdrlen + 1) << 3)); | |
146 | ||
147 | memcpy(isrh, srh, sizeof(*isrh)); | |
148 | memcpy(isrh->rpl_segaddr, &srh->rpl_segaddr[1], | |
149 | (srh->segments_left - 1) * 16); | |
150 | isrh->rpl_segaddr[srh->segments_left - 1] = oldhdr->daddr; | |
151 | ||
152 | ipv6_rpl_srh_compress(csrh, isrh, &srh->rpl_segaddr[0], | |
153 | isrh->segments_left - 1); | |
154 | ||
155 | hdrlen = ((csrh->hdrlen + 1) << 3); | |
156 | ||
157 | err = skb_cow_head(skb, hdrlen + skb->mac_len); | |
7f80ccfe GS |
158 | if (unlikely(err)) { |
159 | kfree(buf); | |
a7a29f9c | 160 | return err; |
7f80ccfe | 161 | } |
a7a29f9c AA |
162 | |
163 | skb_pull(skb, sizeof(struct ipv6hdr)); | |
164 | skb_postpull_rcsum(skb, skb_network_header(skb), | |
165 | sizeof(struct ipv6hdr)); | |
166 | ||
167 | skb_push(skb, sizeof(struct ipv6hdr) + hdrlen); | |
168 | skb_reset_network_header(skb); | |
169 | skb_mac_header_rebuild(skb); | |
170 | ||
171 | hdr = ipv6_hdr(skb); | |
172 | memmove(hdr, oldhdr, sizeof(*hdr)); | |
173 | isrh = (void *)hdr + sizeof(*hdr); | |
174 | memcpy(isrh, csrh, hdrlen); | |
175 | ||
176 | isrh->nexthdr = hdr->nexthdr; | |
177 | hdr->nexthdr = NEXTHDR_ROUTING; | |
178 | hdr->daddr = srh->rpl_segaddr[0]; | |
179 | ||
180 | ipv6_hdr(skb)->payload_len = htons(skb->len - sizeof(struct ipv6hdr)); | |
181 | skb_set_transport_header(skb, sizeof(struct ipv6hdr)); | |
182 | ||
183 | skb_postpush_rcsum(skb, hdr, sizeof(struct ipv6hdr) + hdrlen); | |
184 | ||
185 | kfree(buf); | |
186 | ||
187 | return 0; | |
188 | } | |
189 | ||
190 | static int rpl_do_srh(struct sk_buff *skb, const struct rpl_lwt *rlwt) | |
191 | { | |
192 | struct dst_entry *dst = skb_dst(skb); | |
193 | struct rpl_iptunnel_encap *tinfo; | |
194 | int err = 0; | |
195 | ||
196 | if (skb->protocol != htons(ETH_P_IPV6)) | |
197 | return -EINVAL; | |
198 | ||
199 | tinfo = rpl_encap_lwtunnel(dst->lwtstate); | |
200 | ||
201 | err = rpl_do_srh_inline(skb, rlwt, tinfo->srh); | |
202 | if (err) | |
203 | return err; | |
204 | ||
205 | return 0; | |
206 | } | |
207 | ||
208 | static int rpl_output(struct net *net, struct sock *sk, struct sk_buff *skb) | |
209 | { | |
210 | struct dst_entry *orig_dst = skb_dst(skb); | |
211 | struct dst_entry *dst = NULL; | |
212 | struct rpl_lwt *rlwt; | |
d16fa759 | 213 | int err; |
a7a29f9c AA |
214 | |
215 | rlwt = rpl_lwt_lwtunnel(orig_dst->lwtstate); | |
216 | ||
217 | err = rpl_do_srh(skb, rlwt); | |
218 | if (unlikely(err)) | |
219 | goto drop; | |
220 | ||
221 | preempt_disable(); | |
222 | dst = dst_cache_get(&rlwt->cache); | |
223 | preempt_enable(); | |
224 | ||
225 | if (unlikely(!dst)) { | |
226 | struct ipv6hdr *hdr = ipv6_hdr(skb); | |
227 | struct flowi6 fl6; | |
228 | ||
229 | memset(&fl6, 0, sizeof(fl6)); | |
230 | fl6.daddr = hdr->daddr; | |
231 | fl6.saddr = hdr->saddr; | |
232 | fl6.flowlabel = ip6_flowinfo(hdr); | |
233 | fl6.flowi6_mark = skb->mark; | |
234 | fl6.flowi6_proto = hdr->nexthdr; | |
235 | ||
236 | dst = ip6_route_output(net, NULL, &fl6); | |
237 | if (dst->error) { | |
238 | err = dst->error; | |
239 | dst_release(dst); | |
240 | goto drop; | |
241 | } | |
242 | ||
243 | preempt_disable(); | |
244 | dst_cache_set_ip6(&rlwt->cache, dst, &fl6.saddr); | |
245 | preempt_enable(); | |
246 | } | |
247 | ||
248 | skb_dst_drop(skb); | |
249 | skb_dst_set(skb, dst); | |
250 | ||
251 | err = skb_cow_head(skb, LL_RESERVED_SPACE(dst->dev)); | |
252 | if (unlikely(err)) | |
253 | goto drop; | |
254 | ||
255 | return dst_output(net, sk, skb); | |
256 | ||
257 | drop: | |
258 | kfree_skb(skb); | |
259 | return err; | |
260 | } | |
261 | ||
262 | static int rpl_input(struct sk_buff *skb) | |
263 | { | |
264 | struct dst_entry *orig_dst = skb_dst(skb); | |
265 | struct dst_entry *dst = NULL; | |
266 | struct rpl_lwt *rlwt; | |
267 | int err; | |
268 | ||
269 | rlwt = rpl_lwt_lwtunnel(orig_dst->lwtstate); | |
270 | ||
271 | err = rpl_do_srh(skb, rlwt); | |
272 | if (unlikely(err)) { | |
273 | kfree_skb(skb); | |
274 | return err; | |
275 | } | |
276 | ||
277 | preempt_disable(); | |
278 | dst = dst_cache_get(&rlwt->cache); | |
279 | preempt_enable(); | |
280 | ||
281 | skb_dst_drop(skb); | |
282 | ||
283 | if (!dst) { | |
284 | ip6_route_input(skb); | |
285 | dst = skb_dst(skb); | |
286 | if (!dst->error) { | |
287 | preempt_disable(); | |
288 | dst_cache_set_ip6(&rlwt->cache, dst, | |
289 | &ipv6_hdr(skb)->saddr); | |
290 | preempt_enable(); | |
291 | } | |
292 | } else { | |
293 | skb_dst_set(skb, dst); | |
294 | } | |
295 | ||
296 | err = skb_cow_head(skb, LL_RESERVED_SPACE(dst->dev)); | |
297 | if (unlikely(err)) | |
298 | return err; | |
299 | ||
300 | return dst_input(skb); | |
301 | } | |
302 | ||
303 | static int nla_put_rpl_srh(struct sk_buff *skb, int attrtype, | |
304 | struct rpl_iptunnel_encap *tuninfo) | |
305 | { | |
306 | struct rpl_iptunnel_encap *data; | |
307 | struct nlattr *nla; | |
308 | int len; | |
309 | ||
310 | len = RPL_IPTUNNEL_SRH_SIZE(tuninfo->srh); | |
311 | ||
312 | nla = nla_reserve(skb, attrtype, len); | |
313 | if (!nla) | |
314 | return -EMSGSIZE; | |
315 | ||
316 | data = nla_data(nla); | |
317 | memcpy(data, tuninfo->srh, len); | |
318 | ||
319 | return 0; | |
320 | } | |
321 | ||
322 | static int rpl_fill_encap_info(struct sk_buff *skb, | |
323 | struct lwtunnel_state *lwtstate) | |
324 | { | |
325 | struct rpl_iptunnel_encap *tuninfo = rpl_encap_lwtunnel(lwtstate); | |
326 | ||
327 | if (nla_put_rpl_srh(skb, RPL_IPTUNNEL_SRH, tuninfo)) | |
328 | return -EMSGSIZE; | |
329 | ||
330 | return 0; | |
331 | } | |
332 | ||
333 | static int rpl_encap_nlsize(struct lwtunnel_state *lwtstate) | |
334 | { | |
335 | struct rpl_iptunnel_encap *tuninfo = rpl_encap_lwtunnel(lwtstate); | |
336 | ||
337 | return nla_total_size(RPL_IPTUNNEL_SRH_SIZE(tuninfo->srh)); | |
338 | } | |
339 | ||
340 | static int rpl_encap_cmp(struct lwtunnel_state *a, struct lwtunnel_state *b) | |
341 | { | |
342 | struct rpl_iptunnel_encap *a_hdr = rpl_encap_lwtunnel(a); | |
343 | struct rpl_iptunnel_encap *b_hdr = rpl_encap_lwtunnel(b); | |
344 | int len = RPL_IPTUNNEL_SRH_SIZE(a_hdr->srh); | |
345 | ||
346 | if (len != RPL_IPTUNNEL_SRH_SIZE(b_hdr->srh)) | |
347 | return 1; | |
348 | ||
349 | return memcmp(a_hdr, b_hdr, len); | |
350 | } | |
351 | ||
352 | static const struct lwtunnel_encap_ops rpl_ops = { | |
353 | .build_state = rpl_build_state, | |
354 | .destroy_state = rpl_destroy_state, | |
355 | .output = rpl_output, | |
356 | .input = rpl_input, | |
357 | .fill_encap = rpl_fill_encap_info, | |
358 | .get_encap_size = rpl_encap_nlsize, | |
359 | .cmp_encap = rpl_encap_cmp, | |
360 | .owner = THIS_MODULE, | |
361 | }; | |
362 | ||
363 | int __init rpl_init(void) | |
364 | { | |
365 | int err; | |
366 | ||
367 | err = lwtunnel_encap_add_ops(&rpl_ops, LWTUNNEL_ENCAP_RPL); | |
368 | if (err) | |
369 | goto out; | |
370 | ||
371 | pr_info("RPL Segment Routing with IPv6\n"); | |
372 | ||
373 | return 0; | |
374 | ||
375 | out: | |
376 | return err; | |
377 | } | |
378 | ||
379 | void rpl_exit(void) | |
380 | { | |
381 | lwtunnel_encap_del_ops(&rpl_ops, LWTUNNEL_ENCAP_RPL); | |
382 | } |