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 12 static void ipv6_rpl_addr_decompress(struct in6_addr *dst, 13 const struct in6_addr *daddr, 14 const void *post, unsigned char pfx) 15 { 16 memcpy(dst, daddr, pfx); 17 memcpy(&dst->s6_addr[pfx], post, IPV6_PFXTAIL_LEN(pfx)); 18 } 19 20 static void ipv6_rpl_addr_compress(void *dst, const struct in6_addr *addr, 21 unsigned char pfx) 22 { 23 memcpy(dst, &addr->s6_addr[pfx], IPV6_PFXTAIL_LEN(pfx)); 24 } 25 26 static void *ipv6_rpl_segdata_pos(const struct ipv6_rpl_sr_hdr *hdr, int i) 27 { 28 return (void *)&hdr->rpl_segdata[i * IPV6_PFXTAIL_LEN(hdr->cmpri)]; 29 } 30 31 size_t ipv6_rpl_srh_size(unsigned char n, unsigned char cmpri, 32 unsigned char cmpre) 33 { 34 return (n * IPV6_PFXTAIL_LEN(cmpri)) + IPV6_PFXTAIL_LEN(cmpre); 35 } 36 37 void ipv6_rpl_srh_decompress(struct ipv6_rpl_sr_hdr *outhdr, 38 const struct ipv6_rpl_sr_hdr *inhdr, 39 const struct in6_addr *daddr, unsigned char n) 40 { 41 int i; 42 43 outhdr->nexthdr = inhdr->nexthdr; 44 outhdr->hdrlen = (((n + 1) * sizeof(struct in6_addr)) >> 3); 45 outhdr->pad = 0; 46 outhdr->type = inhdr->type; 47 outhdr->segments_left = inhdr->segments_left; 48 outhdr->cmpri = 0; 49 outhdr->cmpre = 0; 50 51 for (i = 0; i < n; i++) 52 ipv6_rpl_addr_decompress(&outhdr->rpl_segaddr[i], daddr, 53 ipv6_rpl_segdata_pos(inhdr, i), 54 inhdr->cmpri); 55 56 ipv6_rpl_addr_decompress(&outhdr->rpl_segaddr[n], daddr, 57 ipv6_rpl_segdata_pos(inhdr, n), 58 inhdr->cmpre); 59 } 60 61 static unsigned char ipv6_rpl_srh_calc_cmpri(const struct ipv6_rpl_sr_hdr *inhdr, 62 const struct in6_addr *daddr, 63 unsigned char n) 64 { 65 unsigned char plen; 66 int i; 67 68 for (plen = 0; plen < sizeof(*daddr); plen++) { 69 for (i = 0; i < n; i++) { 70 if (daddr->s6_addr[plen] != 71 inhdr->rpl_segaddr[i].s6_addr[plen]) 72 return plen; 73 } 74 } 75 76 return plen; 77 } 78 79 static unsigned char ipv6_rpl_srh_calc_cmpre(const struct in6_addr *daddr, 80 const struct in6_addr *last_segment) 81 { 82 unsigned int plen; 83 84 for (plen = 0; plen < sizeof(*daddr); plen++) { 85 if (daddr->s6_addr[plen] != last_segment->s6_addr[plen]) 86 break; 87 } 88 89 return plen; 90 } 91 92 void ipv6_rpl_srh_compress(struct ipv6_rpl_sr_hdr *outhdr, 93 const struct ipv6_rpl_sr_hdr *inhdr, 94 const struct in6_addr *daddr, unsigned char n) 95 { 96 unsigned char cmpri, cmpre; 97 size_t seglen; 98 int i; 99 100 cmpri = ipv6_rpl_srh_calc_cmpri(inhdr, daddr, n); 101 cmpre = ipv6_rpl_srh_calc_cmpre(daddr, &inhdr->rpl_segaddr[n]); 102 103 outhdr->nexthdr = inhdr->nexthdr; 104 seglen = (n * IPV6_PFXTAIL_LEN(cmpri)) + IPV6_PFXTAIL_LEN(cmpre); 105 outhdr->hdrlen = seglen >> 3; 106 if (seglen & 0x7) { 107 outhdr->hdrlen++; 108 outhdr->pad = 8 - (seglen & 0x7); 109 } else { 110 outhdr->pad = 0; 111 } 112 outhdr->type = inhdr->type; 113 outhdr->segments_left = inhdr->segments_left; 114 outhdr->cmpri = cmpri; 115 outhdr->cmpre = cmpre; 116 117 for (i = 0; i < n; i++) 118 ipv6_rpl_addr_compress(ipv6_rpl_segdata_pos(outhdr, i), 119 &inhdr->rpl_segaddr[i], cmpri); 120 121 ipv6_rpl_addr_compress(ipv6_rpl_segdata_pos(outhdr, n), 122 &inhdr->rpl_segaddr[n], cmpre); 123 } 124