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