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