// SPDX-License-Identifier: GPL-2.0-only
/*
 * Authors:
 * (C) 2020 Alexander Aring <alex.aring@gmail.com>
 */

#include <net/ipv6.h>
#include <net/rpl.h>

#define IPV6_PFXTAIL_LEN(x) (sizeof(struct in6_addr) - (x))
#define IPV6_RPL_BEST_ADDR_COMPRESSION 15

static void ipv6_rpl_addr_decompress(struct in6_addr *dst,
         const struct in6_addr *daddr,
         const void *post, unsigned char pfx)
{
 memcpy(dst, daddr, pfx);
 memcpy(&dst->s6_addr[pfx], post, IPV6_PFXTAIL_LEN(pfx));
}

static void ipv6_rpl_addr_compress(void *dst, const struct in6_addr *addr,
       unsigned char pfx)
{
 memcpy(dst, &addr->s6_addr[pfx], IPV6_PFXTAIL_LEN(pfx));
}

static void *ipv6_rpl_segdata_pos(const struct ipv6_rpl_sr_hdr *hdr, int i)
{
 return (void *)&hdr->rpl_segdata[i * IPV6_PFXTAIL_LEN(hdr->cmpri)];
}

void ipv6_rpl_srh_decompress(struct ipv6_rpl_sr_hdr *outhdr,
        const struct ipv6_rpl_sr_hdr *inhdr,
        const struct in6_addr *daddr, unsigned char n)
{
 int i;

 outhdr->nexthdr = inhdr->nexthdr;
 outhdr->hdrlen = (((n + 1) * sizeof(struct in6_addr)) >> 3);
 outhdr->pad = 0;
 outhdr->type = inhdr->type;
 outhdr->segments_left = inhdr->segments_left;
 outhdr->cmpri = 0;
 outhdr->cmpre = 0;

 for (i = 0; i < n; i++)
  ipv6_rpl_addr_decompress(&outhdr->rpl_segaddr[i], daddr,
      ipv6_rpl_segdata_pos(inhdr, i),
      inhdr->cmpri);

 ipv6_rpl_addr_decompress(&outhdr->rpl_segaddr[n], daddr,
     ipv6_rpl_segdata_pos(inhdr, n),
     inhdr->cmpre);
}

static unsigned char ipv6_rpl_srh_calc_cmpri(const struct ipv6_rpl_sr_hdr *inhdr,
          const struct in6_addr *daddr,
          unsigned char n)
{
 unsigned char plen;
 int i;

 for (plen = 0; plen < sizeof(*daddr); plen++) {
  for (i = 0; i < n; i++) {
   if (daddr->s6_addr[plen] !=
       inhdr->rpl_segaddr[i].s6_addr[plen])
    return plen;
  }
 }

 return IPV6_RPL_BEST_ADDR_COMPRESSION;
}

static unsigned char ipv6_rpl_srh_calc_cmpre(const struct in6_addr *daddr,
          const struct in6_addr *last_segment)
{
 unsigned int plen;

 for (plen = 0; plen < sizeof(*daddr); plen++) {
  if (daddr->s6_addr[plen] != last_segment->s6_addr[plen])
   return plen;
 }

 return IPV6_RPL_BEST_ADDR_COMPRESSION;
}

void ipv6_rpl_srh_compress(struct ipv6_rpl_sr_hdr *outhdr,
      const struct ipv6_rpl_sr_hdr *inhdr,
      const struct in6_addr *daddr, unsigned char n)
{
 unsigned char cmpri, cmpre;
 size_t seglen;
 int i;

 cmpri = ipv6_rpl_srh_calc_cmpri(inhdr, daddr, n);
 cmpre = ipv6_rpl_srh_calc_cmpre(daddr, &inhdr->rpl_segaddr[n]);

 outhdr->nexthdr = inhdr->nexthdr;
 seglen = (n * IPV6_PFXTAIL_LEN(cmpri)) + IPV6_PFXTAIL_LEN(cmpre);
 outhdr->hdrlen = seglen >> 3;
 if (seglen & 0x7) {
  outhdr->hdrlen++;
  outhdr->pad = 8 - (seglen & 0x7);
 } else {
  outhdr->pad = 0;
 }
 outhdr->type = inhdr->type;
 outhdr->segments_left = inhdr->segments_left;
 outhdr->cmpri = cmpri;
 outhdr->cmpre = cmpre;

 for (i = 0; i < n; i++)
  ipv6_rpl_addr_compress(ipv6_rpl_segdata_pos(outhdr, i),
           &inhdr->rpl_segaddr[i], cmpri);

 ipv6_rpl_addr_compress(ipv6_rpl_segdata_pos(outhdr, n),
          &inhdr->rpl_segaddr[n], cmpre);
}