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 | DECLARE_FLEX_ARRAY(struct ipv6_rpl_sr_hdr, srh); |
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, segs: srh->rpl_segaddr, |
56 | nsegs: srh->segments_left); |
57 | if (err) |
58 | return false; |
59 | |
60 | if (ipv6_addr_type(addr: &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 | policy: 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(nla: tb[RPL_IPTUNNEL_SRH]); |
90 | srh_len = nla_len(nla: 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, seglen: srh_len - sizeof(*srh))) |
97 | return -EINVAL; |
98 | |
99 | newts = lwtunnel_state_alloc(hdr_len: srh_len + sizeof(*rlwt)); |
100 | if (!newts) |
101 | return -ENOMEM; |
102 | |
103 | rlwt = rpl_lwt_lwtunnel(lwt: newts); |
104 | |
105 | err = dst_cache_init(dst_cache: &rlwt->cache, GFP_ATOMIC); |
106 | if (err) { |
107 | kfree(objp: 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(dst_cache: &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 = kcalloc(struct_size(srh, segments.addr, srh->segments_left), size: 2, GFP_ATOMIC); |
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(outhdr: csrh, inhdr: isrh, daddr: &srh->rpl_segaddr[0], |
152 | n: isrh->segments_left - 1); |
153 | |
154 | hdrlen = ((csrh->hdrlen + 1) << 3); |
155 | |
156 | err = skb_cow_head(skb, headroom: hdrlen + skb->mac_len); |
157 | if (unlikely(err)) { |
158 | kfree(objp: buf); |
159 | return err; |
160 | } |
161 | |
162 | skb_pull(skb, len: sizeof(struct ipv6hdr)); |
163 | skb_postpull_rcsum(skb, start: skb_network_header(skb), |
164 | len: sizeof(struct ipv6hdr)); |
165 | |
166 | skb_push(skb, len: 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, offset: sizeof(struct ipv6hdr)); |
181 | |
182 | skb_postpush_rcsum(skb, start: hdr, len: sizeof(struct ipv6hdr) + hdrlen); |
183 | |
184 | kfree(objp: 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; |
193 | |
194 | if (skb->protocol != htons(ETH_P_IPV6)) |
195 | return -EINVAL; |
196 | |
197 | tinfo = rpl_encap_lwtunnel(lwt: dst->lwtstate); |
198 | |
199 | return rpl_do_srh_inline(skb, rlwt, srh: tinfo->srh); |
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; |
207 | int err; |
208 | |
209 | rlwt = rpl_lwt_lwtunnel(lwt: 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(dst_cache: &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: &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(dst_cache: &rlwt->cache, dst, saddr: &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(lwt: 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(dst_cache: &rlwt->cache); |
273 | preempt_enable(); |
274 | |
275 | if (!dst) { |
276 | ip6_route_input(skb); |
277 | dst = skb_dst(skb); |
278 | if (!dst->error) { |
279 | preempt_disable(); |
280 | dst_cache_set_ip6(dst_cache: &rlwt->cache, dst, |
281 | saddr: &ipv6_hdr(skb)->saddr); |
282 | preempt_enable(); |
283 | } |
284 | } else { |
285 | skb_dst_drop(skb); |
286 | skb_dst_set(skb, dst); |
287 | } |
288 | |
289 | err = skb_cow_head(skb, LL_RESERVED_SPACE(dst->dev)); |
290 | if (unlikely(err)) |
291 | return err; |
292 | |
293 | return dst_input(skb); |
294 | } |
295 | |
296 | static int nla_put_rpl_srh(struct sk_buff *skb, int attrtype, |
297 | struct rpl_iptunnel_encap *tuninfo) |
298 | { |
299 | struct rpl_iptunnel_encap *data; |
300 | struct nlattr *nla; |
301 | int len; |
302 | |
303 | len = RPL_IPTUNNEL_SRH_SIZE(tuninfo->srh); |
304 | |
305 | nla = nla_reserve(skb, attrtype, attrlen: len); |
306 | if (!nla) |
307 | return -EMSGSIZE; |
308 | |
309 | data = nla_data(nla); |
310 | memcpy(data, tuninfo->srh, len); |
311 | |
312 | return 0; |
313 | } |
314 | |
315 | static int rpl_fill_encap_info(struct sk_buff *skb, |
316 | struct lwtunnel_state *lwtstate) |
317 | { |
318 | struct rpl_iptunnel_encap *tuninfo = rpl_encap_lwtunnel(lwt: lwtstate); |
319 | |
320 | if (nla_put_rpl_srh(skb, attrtype: RPL_IPTUNNEL_SRH, tuninfo)) |
321 | return -EMSGSIZE; |
322 | |
323 | return 0; |
324 | } |
325 | |
326 | static int rpl_encap_nlsize(struct lwtunnel_state *lwtstate) |
327 | { |
328 | struct rpl_iptunnel_encap *tuninfo = rpl_encap_lwtunnel(lwt: lwtstate); |
329 | |
330 | return nla_total_size(RPL_IPTUNNEL_SRH_SIZE(tuninfo->srh)); |
331 | } |
332 | |
333 | static int rpl_encap_cmp(struct lwtunnel_state *a, struct lwtunnel_state *b) |
334 | { |
335 | struct rpl_iptunnel_encap *a_hdr = rpl_encap_lwtunnel(lwt: a); |
336 | struct rpl_iptunnel_encap *b_hdr = rpl_encap_lwtunnel(lwt: b); |
337 | int len = RPL_IPTUNNEL_SRH_SIZE(a_hdr->srh); |
338 | |
339 | if (len != RPL_IPTUNNEL_SRH_SIZE(b_hdr->srh)) |
340 | return 1; |
341 | |
342 | return memcmp(p: a_hdr, q: b_hdr, size: len); |
343 | } |
344 | |
345 | static const struct lwtunnel_encap_ops rpl_ops = { |
346 | .build_state = rpl_build_state, |
347 | .destroy_state = rpl_destroy_state, |
348 | .output = rpl_output, |
349 | .input = rpl_input, |
350 | .fill_encap = rpl_fill_encap_info, |
351 | .get_encap_size = rpl_encap_nlsize, |
352 | .cmp_encap = rpl_encap_cmp, |
353 | .owner = THIS_MODULE, |
354 | }; |
355 | |
356 | int __init rpl_init(void) |
357 | { |
358 | int err; |
359 | |
360 | err = lwtunnel_encap_add_ops(op: &rpl_ops, num: LWTUNNEL_ENCAP_RPL); |
361 | if (err) |
362 | goto out; |
363 | |
364 | pr_info("RPL Segment Routing with IPv6\n" ); |
365 | |
366 | return 0; |
367 | |
368 | out: |
369 | return err; |
370 | } |
371 | |
372 | void rpl_exit(void) |
373 | { |
374 | lwtunnel_encap_del_ops(op: &rpl_ops, num: LWTUNNEL_ENCAP_RPL); |
375 | } |
376 | |