Commit | Line | Data |
---|---|---|
8610c7c6 | 1 | // SPDX-License-Identifier: GPL-2.0-only |
f1f09df1 | 2 | /* |
8610c7c6 AA |
3 | * Authors: |
4 | * (C) 2020 Alexander Aring <alex.aring@gmail.com> | |
5 | */ | |
6 | ||
7 | #include <net/ipv6.h> | |
8 | #include <net/rpl.h> | |
9 | ||
10 | #define IPV6_PFXTAIL_LEN(x) (sizeof(struct in6_addr) - (x)) | |
62e69776 | 11 | #define IPV6_RPL_BEST_ADDR_COMPRESSION 15 |
8610c7c6 AA |
12 | |
13 | static void ipv6_rpl_addr_decompress(struct in6_addr *dst, | |
14 | const struct in6_addr *daddr, | |
15 | const void *post, unsigned char pfx) | |
16 | { | |
17 | memcpy(dst, daddr, pfx); | |
18 | memcpy(&dst->s6_addr[pfx], post, IPV6_PFXTAIL_LEN(pfx)); | |
19 | } | |
20 | ||
21 | static void ipv6_rpl_addr_compress(void *dst, const struct in6_addr *addr, | |
22 | unsigned char pfx) | |
23 | { | |
24 | memcpy(dst, &addr->s6_addr[pfx], IPV6_PFXTAIL_LEN(pfx)); | |
25 | } | |
26 | ||
27 | static void *ipv6_rpl_segdata_pos(const struct ipv6_rpl_sr_hdr *hdr, int i) | |
28 | { | |
29 | return (void *)&hdr->rpl_segdata[i * IPV6_PFXTAIL_LEN(hdr->cmpri)]; | |
30 | } | |
31 | ||
8610c7c6 AA |
32 | void ipv6_rpl_srh_decompress(struct ipv6_rpl_sr_hdr *outhdr, |
33 | const struct ipv6_rpl_sr_hdr *inhdr, | |
34 | const struct in6_addr *daddr, unsigned char n) | |
35 | { | |
36 | int i; | |
37 | ||
38 | outhdr->nexthdr = inhdr->nexthdr; | |
39 | outhdr->hdrlen = (((n + 1) * sizeof(struct in6_addr)) >> 3); | |
40 | outhdr->pad = 0; | |
41 | outhdr->type = inhdr->type; | |
42 | outhdr->segments_left = inhdr->segments_left; | |
43 | outhdr->cmpri = 0; | |
44 | outhdr->cmpre = 0; | |
45 | ||
a7f9a6f4 | 46 | for (i = 0; i < n; i++) |
8610c7c6 AA |
47 | ipv6_rpl_addr_decompress(&outhdr->rpl_segaddr[i], daddr, |
48 | ipv6_rpl_segdata_pos(inhdr, i), | |
49 | inhdr->cmpri); | |
50 | ||
51 | ipv6_rpl_addr_decompress(&outhdr->rpl_segaddr[n], daddr, | |
52 | ipv6_rpl_segdata_pos(inhdr, n), | |
53 | inhdr->cmpre); | |
54 | } | |
55 | ||
56 | static unsigned char ipv6_rpl_srh_calc_cmpri(const struct ipv6_rpl_sr_hdr *inhdr, | |
57 | const struct in6_addr *daddr, | |
58 | unsigned char n) | |
59 | { | |
60 | unsigned char plen; | |
61 | int i; | |
62 | ||
63 | for (plen = 0; plen < sizeof(*daddr); plen++) { | |
a7f9a6f4 | 64 | for (i = 0; i < n; i++) { |
8610c7c6 AA |
65 | if (daddr->s6_addr[plen] != |
66 | inhdr->rpl_segaddr[i].s6_addr[plen]) | |
67 | return plen; | |
68 | } | |
69 | } | |
70 | ||
62e69776 | 71 | return IPV6_RPL_BEST_ADDR_COMPRESSION; |
8610c7c6 AA |
72 | } |
73 | ||
74 | static unsigned char ipv6_rpl_srh_calc_cmpre(const struct in6_addr *daddr, | |
75 | const struct in6_addr *last_segment) | |
76 | { | |
77 | unsigned int plen; | |
78 | ||
79 | for (plen = 0; plen < sizeof(*daddr); plen++) { | |
80 | if (daddr->s6_addr[plen] != last_segment->s6_addr[plen]) | |
62e69776 | 81 | return plen; |
8610c7c6 AA |
82 | } |
83 | ||
62e69776 | 84 | return IPV6_RPL_BEST_ADDR_COMPRESSION; |
8610c7c6 AA |
85 | } |
86 | ||
87 | void ipv6_rpl_srh_compress(struct ipv6_rpl_sr_hdr *outhdr, | |
88 | const struct ipv6_rpl_sr_hdr *inhdr, | |
89 | const struct in6_addr *daddr, unsigned char n) | |
90 | { | |
91 | unsigned char cmpri, cmpre; | |
92 | size_t seglen; | |
93 | int i; | |
94 | ||
95 | cmpri = ipv6_rpl_srh_calc_cmpri(inhdr, daddr, n); | |
96 | cmpre = ipv6_rpl_srh_calc_cmpre(daddr, &inhdr->rpl_segaddr[n]); | |
97 | ||
98 | outhdr->nexthdr = inhdr->nexthdr; | |
99 | seglen = (n * IPV6_PFXTAIL_LEN(cmpri)) + IPV6_PFXTAIL_LEN(cmpre); | |
100 | outhdr->hdrlen = seglen >> 3; | |
101 | if (seglen & 0x7) { | |
102 | outhdr->hdrlen++; | |
103 | outhdr->pad = 8 - (seglen & 0x7); | |
104 | } else { | |
105 | outhdr->pad = 0; | |
106 | } | |
107 | outhdr->type = inhdr->type; | |
108 | outhdr->segments_left = inhdr->segments_left; | |
109 | outhdr->cmpri = cmpri; | |
110 | outhdr->cmpre = cmpre; | |
111 | ||
a7f9a6f4 | 112 | for (i = 0; i < n; i++) |
8610c7c6 AA |
113 | ipv6_rpl_addr_compress(ipv6_rpl_segdata_pos(outhdr, i), |
114 | &inhdr->rpl_segaddr[i], cmpri); | |
115 | ||
116 | ipv6_rpl_addr_compress(ipv6_rpl_segdata_pos(outhdr, n), | |
117 | &inhdr->rpl_segaddr[n], cmpre); | |
118 | } |