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); | |
158 | if (unlikely(err)) | |
159 | return err; | |
160 | ||
161 | skb_pull(skb, sizeof(struct ipv6hdr)); | |
162 | skb_postpull_rcsum(skb, skb_network_header(skb), | |
163 | sizeof(struct ipv6hdr)); | |
164 | ||
165 | skb_push(skb, sizeof(struct ipv6hdr) + hdrlen); | |
166 | skb_reset_network_header(skb); | |
167 | skb_mac_header_rebuild(skb); | |
168 | ||
169 | hdr = ipv6_hdr(skb); | |
170 | memmove(hdr, oldhdr, sizeof(*hdr)); | |
171 | isrh = (void *)hdr + sizeof(*hdr); | |
172 | memcpy(isrh, csrh, hdrlen); | |
173 | ||
174 | isrh->nexthdr = hdr->nexthdr; | |
175 | hdr->nexthdr = NEXTHDR_ROUTING; | |
176 | hdr->daddr = srh->rpl_segaddr[0]; | |
177 | ||
178 | ipv6_hdr(skb)->payload_len = htons(skb->len - sizeof(struct ipv6hdr)); | |
179 | skb_set_transport_header(skb, sizeof(struct ipv6hdr)); | |
180 | ||
181 | skb_postpush_rcsum(skb, hdr, sizeof(struct ipv6hdr) + hdrlen); | |
182 | ||
183 | kfree(buf); | |
184 | ||
185 | return 0; | |
186 | } | |
187 | ||
188 | static int rpl_do_srh(struct sk_buff *skb, const struct rpl_lwt *rlwt) | |
189 | { | |
190 | struct dst_entry *dst = skb_dst(skb); | |
191 | struct rpl_iptunnel_encap *tinfo; | |
192 | int err = 0; | |
193 | ||
194 | if (skb->protocol != htons(ETH_P_IPV6)) | |
195 | return -EINVAL; | |
196 | ||
197 | tinfo = rpl_encap_lwtunnel(dst->lwtstate); | |
198 | ||
199 | err = rpl_do_srh_inline(skb, rlwt, tinfo->srh); | |
200 | if (err) | |
201 | return err; | |
202 | ||
203 | return 0; | |
204 | } | |
205 | ||
206 | static int rpl_output(struct net *net, struct sock *sk, struct sk_buff *skb) | |
207 | { | |
208 | struct dst_entry *orig_dst = skb_dst(skb); | |
209 | struct dst_entry *dst = NULL; | |
210 | struct rpl_lwt *rlwt; | |
211 | int err = -EINVAL; | |
212 | ||
213 | rlwt = rpl_lwt_lwtunnel(orig_dst->lwtstate); | |
214 | ||
215 | err = rpl_do_srh(skb, rlwt); | |
216 | if (unlikely(err)) | |
217 | goto drop; | |
218 | ||
219 | preempt_disable(); | |
220 | dst = dst_cache_get(&rlwt->cache); | |
221 | preempt_enable(); | |
222 | ||
223 | if (unlikely(!dst)) { | |
224 | struct ipv6hdr *hdr = ipv6_hdr(skb); | |
225 | struct flowi6 fl6; | |
226 | ||
227 | memset(&fl6, 0, sizeof(fl6)); | |
228 | fl6.daddr = hdr->daddr; | |
229 | fl6.saddr = hdr->saddr; | |
230 | fl6.flowlabel = ip6_flowinfo(hdr); | |
231 | fl6.flowi6_mark = skb->mark; | |
232 | fl6.flowi6_proto = hdr->nexthdr; | |
233 | ||
234 | dst = ip6_route_output(net, NULL, &fl6); | |
235 | if (dst->error) { | |
236 | err = dst->error; | |
237 | dst_release(dst); | |
238 | goto drop; | |
239 | } | |
240 | ||
241 | preempt_disable(); | |
242 | dst_cache_set_ip6(&rlwt->cache, dst, &fl6.saddr); | |
243 | preempt_enable(); | |
244 | } | |
245 | ||
246 | skb_dst_drop(skb); | |
247 | skb_dst_set(skb, dst); | |
248 | ||
249 | err = skb_cow_head(skb, LL_RESERVED_SPACE(dst->dev)); | |
250 | if (unlikely(err)) | |
251 | goto drop; | |
252 | ||
253 | return dst_output(net, sk, skb); | |
254 | ||
255 | drop: | |
256 | kfree_skb(skb); | |
257 | return err; | |
258 | } | |
259 | ||
260 | static int rpl_input(struct sk_buff *skb) | |
261 | { | |
262 | struct dst_entry *orig_dst = skb_dst(skb); | |
263 | struct dst_entry *dst = NULL; | |
264 | struct rpl_lwt *rlwt; | |
265 | int err; | |
266 | ||
267 | rlwt = rpl_lwt_lwtunnel(orig_dst->lwtstate); | |
268 | ||
269 | err = rpl_do_srh(skb, rlwt); | |
270 | if (unlikely(err)) { | |
271 | kfree_skb(skb); | |
272 | return err; | |
273 | } | |
274 | ||
275 | preempt_disable(); | |
276 | dst = dst_cache_get(&rlwt->cache); | |
277 | preempt_enable(); | |
278 | ||
279 | skb_dst_drop(skb); | |
280 | ||
281 | if (!dst) { | |
282 | ip6_route_input(skb); | |
283 | dst = skb_dst(skb); | |
284 | if (!dst->error) { | |
285 | preempt_disable(); | |
286 | dst_cache_set_ip6(&rlwt->cache, dst, | |
287 | &ipv6_hdr(skb)->saddr); | |
288 | preempt_enable(); | |
289 | } | |
290 | } else { | |
291 | skb_dst_set(skb, dst); | |
292 | } | |
293 | ||
294 | err = skb_cow_head(skb, LL_RESERVED_SPACE(dst->dev)); | |
295 | if (unlikely(err)) | |
296 | return err; | |
297 | ||
298 | return dst_input(skb); | |
299 | } | |
300 | ||
301 | static int nla_put_rpl_srh(struct sk_buff *skb, int attrtype, | |
302 | struct rpl_iptunnel_encap *tuninfo) | |
303 | { | |
304 | struct rpl_iptunnel_encap *data; | |
305 | struct nlattr *nla; | |
306 | int len; | |
307 | ||
308 | len = RPL_IPTUNNEL_SRH_SIZE(tuninfo->srh); | |
309 | ||
310 | nla = nla_reserve(skb, attrtype, len); | |
311 | if (!nla) | |
312 | return -EMSGSIZE; | |
313 | ||
314 | data = nla_data(nla); | |
315 | memcpy(data, tuninfo->srh, len); | |
316 | ||
317 | return 0; | |
318 | } | |
319 | ||
320 | static int rpl_fill_encap_info(struct sk_buff *skb, | |
321 | struct lwtunnel_state *lwtstate) | |
322 | { | |
323 | struct rpl_iptunnel_encap *tuninfo = rpl_encap_lwtunnel(lwtstate); | |
324 | ||
325 | if (nla_put_rpl_srh(skb, RPL_IPTUNNEL_SRH, tuninfo)) | |
326 | return -EMSGSIZE; | |
327 | ||
328 | return 0; | |
329 | } | |
330 | ||
331 | static int rpl_encap_nlsize(struct lwtunnel_state *lwtstate) | |
332 | { | |
333 | struct rpl_iptunnel_encap *tuninfo = rpl_encap_lwtunnel(lwtstate); | |
334 | ||
335 | return nla_total_size(RPL_IPTUNNEL_SRH_SIZE(tuninfo->srh)); | |
336 | } | |
337 | ||
338 | static int rpl_encap_cmp(struct lwtunnel_state *a, struct lwtunnel_state *b) | |
339 | { | |
340 | struct rpl_iptunnel_encap *a_hdr = rpl_encap_lwtunnel(a); | |
341 | struct rpl_iptunnel_encap *b_hdr = rpl_encap_lwtunnel(b); | |
342 | int len = RPL_IPTUNNEL_SRH_SIZE(a_hdr->srh); | |
343 | ||
344 | if (len != RPL_IPTUNNEL_SRH_SIZE(b_hdr->srh)) | |
345 | return 1; | |
346 | ||
347 | return memcmp(a_hdr, b_hdr, len); | |
348 | } | |
349 | ||
350 | static const struct lwtunnel_encap_ops rpl_ops = { | |
351 | .build_state = rpl_build_state, | |
352 | .destroy_state = rpl_destroy_state, | |
353 | .output = rpl_output, | |
354 | .input = rpl_input, | |
355 | .fill_encap = rpl_fill_encap_info, | |
356 | .get_encap_size = rpl_encap_nlsize, | |
357 | .cmp_encap = rpl_encap_cmp, | |
358 | .owner = THIS_MODULE, | |
359 | }; | |
360 | ||
361 | int __init rpl_init(void) | |
362 | { | |
363 | int err; | |
364 | ||
365 | err = lwtunnel_encap_add_ops(&rpl_ops, LWTUNNEL_ENCAP_RPL); | |
366 | if (err) | |
367 | goto out; | |
368 | ||
369 | pr_info("RPL Segment Routing with IPv6\n"); | |
370 | ||
371 | return 0; | |
372 | ||
373 | out: | |
374 | return err; | |
375 | } | |
376 | ||
377 | void rpl_exit(void) | |
378 | { | |
379 | lwtunnel_encap_del_ops(&rpl_ops, LWTUNNEL_ENCAP_RPL); | |
380 | } |