1 // SPDX-License-Identifier: GPL-2.0-only 2 /* 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)) 11 #define IPV6_RPL_BEST_ADDR_COMPRESSION 15 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 32 size_t ipv6_rpl_srh_size(unsigned char n, unsigned char cmpri, 33 unsigned char cmpre) 34 { 35 return (n * IPV6_PFXTAIL_LEN(cmpri)) + IPV6_PFXTAIL_LEN(cmpre); 36 } 37 38 void ipv6_rpl_srh_decompress(struct ipv6_rpl_sr_hdr *outhdr, 39 const struct ipv6_rpl_sr_hdr *inhdr, 40 const struct in6_addr *daddr, unsigned char n) 41 { 42 int i; 43 44 outhdr->nexthdr = inhdr->nexthdr; 45 outhdr->hdrlen = (((n + 1) * sizeof(struct in6_addr)) >> 3); 46 outhdr->pad = 0; 47 outhdr->type = inhdr->type; 48 outhdr->segments_left = inhdr->segments_left; 49 outhdr->cmpri = 0; 50 outhdr->cmpre = 0; 51 52 for (i = 0; i < n; i++) 53 ipv6_rpl_addr_decompress(&outhdr->rpl_segaddr[i], daddr, 54 ipv6_rpl_segdata_pos(inhdr, i), 55 inhdr->cmpri); 56 57 ipv6_rpl_addr_decompress(&outhdr->rpl_segaddr[n], daddr, 58 ipv6_rpl_segdata_pos(inhdr, n), 59 inhdr->cmpre); 60 } 61 62 static unsigned char ipv6_rpl_srh_calc_cmpri(const struct ipv6_rpl_sr_hdr *inhdr, 63 const struct in6_addr *daddr, 64 unsigned char n) 65 { 66 unsigned char plen; 67 int i; 68 69 for (plen = 0; plen < sizeof(*daddr); plen++) { 70 for (i = 0; i < n; i++) { 71 if (daddr->s6_addr[plen] != 72 inhdr->rpl_segaddr[i].s6_addr[plen]) 73 return plen; 74 } 75 } 76 77 return IPV6_RPL_BEST_ADDR_COMPRESSION; 78 } 79 80 static unsigned char ipv6_rpl_srh_calc_cmpre(const struct in6_addr *daddr, 81 const struct in6_addr *last_segment) 82 { 83 unsigned int plen; 84 85 for (plen = 0; plen < sizeof(*daddr); plen++) { 86 if (daddr->s6_addr[plen] != last_segment->s6_addr[plen]) 87 return plen; 88 } 89 90 return IPV6_RPL_BEST_ADDR_COMPRESSION; 91 } 92 93 void ipv6_rpl_srh_compress(struct ipv6_rpl_sr_hdr *outhdr, 94 const struct ipv6_rpl_sr_hdr *inhdr, 95 const struct in6_addr *daddr, unsigned char n) 96 { 97 unsigned char cmpri, cmpre; 98 size_t seglen; 99 int i; 100 101 cmpri = ipv6_rpl_srh_calc_cmpri(inhdr, daddr, n); 102 cmpre = ipv6_rpl_srh_calc_cmpre(daddr, &inhdr->rpl_segaddr[n]); 103 104 outhdr->nexthdr = inhdr->nexthdr; 105 seglen = (n * IPV6_PFXTAIL_LEN(cmpri)) + IPV6_PFXTAIL_LEN(cmpre); 106 outhdr->hdrlen = seglen >> 3; 107 if (seglen & 0x7) { 108 outhdr->hdrlen++; 109 outhdr->pad = 8 - (seglen & 0x7); 110 } else { 111 outhdr->pad = 0; 112 } 113 outhdr->type = inhdr->type; 114 outhdr->segments_left = inhdr->segments_left; 115 outhdr->cmpri = cmpri; 116 outhdr->cmpre = cmpre; 117 118 for (i = 0; i < n; i++) 119 ipv6_rpl_addr_compress(ipv6_rpl_segdata_pos(outhdr, i), 120 &inhdr->rpl_segaddr[i], cmpri); 121 122 ipv6_rpl_addr_compress(ipv6_rpl_segdata_pos(outhdr, n), 123 &inhdr->rpl_segaddr[n], cmpre); 124 } 125