text: /****************************************************************************
   * net/sixlowpan/sixlowpan_hc1.c
   *
   *   Copyright (C) 2017, Gregory Nutt, all rights reserved
   *   Author: Gregory Nutt <gnutt@nuttx.org>
   *
   * Derives from Contiki:
   *
   *   Copyright (c) 2008, Swedish Institute of Computer Science.
   *   All rights reserved.
   *   Authors: Adam Dunkels <adam@sics.se>
   *            Nicolas Tsiftes <nvt@sics.se>
   *            Niclas Finne <nfi@sics.se>
   *            Mathilde Durvy <mdurvy@cisco.com>
   *            Julien Abeille <jabeille@cisco.com>
   *            Joakim Eriksson <joakime@sics.se>
   *            Joel Hoglund <joel@sics.se>
   *
   * Redistribution and use in source and binary forms, with or without
   * modification, are permitted provided that the following conditions
   * are met:
   * 1. Redistributions of source code must retain the above copyright
   *    notice, this list of conditions and the following disclaimer.
   * 2. Redistributions in binary form must reproduce the above copyright
   *    notice, this list of conditions and the following disclaimer in the
   *    documentation and/or other materials provided with the distribution.
   * 3. Neither the name of the Institute nor the names of its contributors
   *    may be used to endorse or promote products derived from this software
   *    without specific prior written permission.
   *
   * THIS SOFTWARE IS PROVIDED BY THE INSTITUTE AND CONTRIBUTORS ``AS IS'' AND
   * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
   * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
   * ARE DISCLAIMED.  IN NO EVENT SHALL THE INSTITUTE OR CONTRIBUTORS BE LIABLE
   * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
   * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
   * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
   * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
   * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
   * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
   * SUCH DAMAGE.
   *
   ****************************************************************************/
  
  /****************************************************************************
   * Included Files
   ****************************************************************************/
  
  #include <nuttx/config.h>
  
  #include <string.h>
  #include <errno.h>
  #include <assert.h>
  #include <debug.h>
  
  #include <nuttx/mm/iob.h>
  #include <nuttx/net/netdev.h>
  #include <nuttx/net/radiodev.h>
  #include "sixlowpan/sixlowpan_internal.h"
  
  #ifdef CONFIG_NET_6LOWPAN_COMPRESSION_HC1
  
  /****************************************************************************
   * Public Functions
   ****************************************************************************/
  
  /****************************************************************************
   * Name: sixlowpan_compresshdr_hc1
   *
   * Description:
   *   Compress IP/UDP header using HC1 and HC_UDP
   *
   *   This function is called by the 6lowpan code to create a compressed
   *   6lowpan packet in the packetbuf buffer from a full IPv6 packet in the
   *   uip_buf buffer.
   *
   *   If we can compress everything, we use HC1 dispatch, if not we use
   *   IPv6 dispatch.  We can compress everything if:
   *
   *     - IP version is
   *     - Flow label and traffic class are 0
   *     - Both src and dest ip addresses are link local
   *     - Both src and dest interface ID are recoverable from lower layer
   *       header
   *     - Next header is either ICMP, UDP or TCP
   *
   *   Moreover, if next header is UDP, we try to compress it using HC_UDP.
   *   This is feasible is both ports are between F0B0 and F0B0 + 15
   *
   *   Resulting header structure:
   *   - For ICMP, TCP, non compressed UDP\n
   *     HC1 encoding = 11111010 (UDP) 11111110 (TCP) 11111100 (ICMP)
   *                      1                   2                   3
   *   0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1
   *   +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
   *   | LoWPAN HC1 Dsp | HC1 encoding  | IPv6 Hop limit| L4 hdr + data|
   *   +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
   *   | ...
   *   +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
   *
   *   - For compressed UDP
   *     HC1 encoding = 11111011, HC_UDP encoding = 11100000
   *                      1                   2                   3
   *   0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1
   *   +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
   *   | LoWPAN HC1 Dsp| HC1 encoding  |  HC_UDP encod.| IPv6 Hop limit|
   *   +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
   *   | src p.| dst p.| UDP checksum                  | L4 data...
   *   +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
   *
   * Input Parameters:
   *   radio   - A reference to a radio network device instance
   *   ipv6    - The IPv6 header followed by TCP, UDP, or ICMPv6 header to be
   *             compressed
   *   destmac - L2 destination address, needed to compress the IP
   *             destination field
   *   fptr    - Pointer to frame to be compressed.
   *
   * Returned Value:
   *   On success the indications of the defines COMPRESS_HDR_* are returned.
   *   A negated errno value is returned on failure.
   *
   ****************************************************************************/
  
  int sixlowpan_compresshdr_hc1(FAR struct radio_driver_s *radio,
                                FAR const struct ipv6_hdr_s *ipv6,
                                FAR const struct netdev_varaddr_s *destmac,
                                FAR uint8_t *fptr)
  {
    FAR uint8_t *hc1 = fptr + g_frame_hdrlen;
    int ret = COMPRESS_HDR_INLINE;
  
    /* Check if all the assumptions for full compression are valid */
  
    if (ipv6->vtc != 0x60 || ipv6->tcf != 0 || ipv6->flow != 0 ||
        !sixlowpan_islinklocal(ipv6->srcipaddr) ||
        !sixlowpan_ismacbased(ipv6->srcipaddr, &radio->r_dev.d_mac.radio) ||
        !sixlowpan_islinklocal(ipv6->destipaddr) ||
        !sixlowpan_ismacbased(ipv6->destipaddr, destmac) ||
        (1
  #ifdef CONFIG_NET_TCP
          && ipv6->proto != IP_PROTO_TCP
  #endif
  #ifdef CONFIG_NET_UDP
          && ipv6->proto != IP_PROTO_UDP
  #endif
  #ifdef CONFIG_NET_ICMPv6
          && ipv6->proto != IP_PROTO_ICMP6
  #endif
        ))
      {
        /* IPV6 DISPATCH
         * Something cannot be compressed, use IPV6 DISPATCH, compress
         * nothing, copy IPv6 header into the frame buffer
         */
  
        nwarn("WARNING: Fall back to IPv6 dispatch\n");
  
        /* IPv6 dispatch header (1 byte) */
  
        hc1[SIXLOWPAN_HC1_DISPATCH] = SIXLOWPAN_DISPATCH_IPV6;
        g_frame_hdrlen        += SIXLOWPAN_IPV6_HDR_LEN;
  
        memcpy(fptr + g_frame_hdrlen, ipv6, IPv6_HDRLEN);
        g_frame_hdrlen        += IPv6_HDRLEN;
        g_uncomp_hdrlen       += IPv6_HDRLEN;
      }
    else
      {
        /* HC1 DISPATCH  maximum compression:
         * All fields in the IP header but Hop Limit are elided.  If next
         * header is UDP, we compress UDP header using HC2
         */
  
        hc1[SIXLOWPAN_HC1_DISPATCH] = SIXLOWPAN_DISPATCH_HC1;
        g_uncomp_hdrlen += IPv6_HDRLEN;
        switch (ipv6->proto)
          {
  #ifdef CONFIG_NET_ICMPv6
          case IP_PROTO_ICMP6:
            {
              /* HC1 encoding and ttl */
  
              hc1[SIXLOWPAN_HC1_ENCODING] = 0xfc;
              hc1[SIXLOWPAN_HC1_TTL]      = ipv6->ttl;
              g_frame_hdrlen             += SIXLOWPAN_HC1_HDR_LEN;
            }
            break;
  #endif
  #ifdef CONFIG_NET_TCP
          case IP_PROTO_TCP:
            {
              /* HC1 encoding and ttl */
  
              hc1[SIXLOWPAN_HC1_ENCODING] = 0xfe;
              hc1[SIXLOWPAN_HC1_TTL]      = ipv6->ttl;
              g_frame_hdrlen             += SIXLOWPAN_HC1_HDR_LEN;
            }
            break;
  #endif
  #ifdef CONFIG_NET_UDP
          case IP_PROTO_UDP:
            {
              FAR struct udp_hdr_s *udp =
                &(((FAR struct ipv6udp_hdr_s *)ipv6)->udp);
  
              /* Try to compress UDP header (we do only full compression).
               * This is feasible if both src and dest ports are between
               * CONFIG_NET_6LOWPAN_MINPORT and CONFIG_NET_6LOWPAN_MINPORT +
               * 15
               */
  
              ninfo("local/remote port %04x/%04x\n",
                     udp->srcport, udp->destport);
  
              if (NTOHS(udp->srcport)  >=  CONFIG_NET_6LOWPAN_MINPORT &&
                  NTOHS(udp->srcport)  <  (CONFIG_NET_6LOWPAN_MINPORT + 16) &&
                  NTOHS(udp->destport) >=  CONFIG_NET_6LOWPAN_MINPORT &&
                  NTOHS(udp->destport) <  (CONFIG_NET_6LOWPAN_MINPORT + 16))
                {
                  FAR uint8_t *hcudp = fptr + g_frame_hdrlen;
  
                  /* HC1 encoding */
  
                  hcudp[SIXLOWPAN_HC1_HC_UDP_HC1_ENCODING] = 0xfb;
  
                  /* HC_UDP encoding, ttl, src and dest ports, checksum */
  
                  hcudp[SIXLOWPAN_HC1_HC_UDP_UDP_ENCODING] = 0xe0;
                  hcudp[SIXLOWPAN_HC1_HC_UDP_TTL]          = ipv6->ttl;
                  hcudp[SIXLOWPAN_HC1_HC_UDP_PORTS]        =
                    (uint8_t)((NTOHS(udp->srcport) -
                              CONFIG_NET_6LOWPAN_MINPORT) << 4) +
                    (uint8_t)((NTOHS(udp->destport) -
                              CONFIG_NET_6LOWPAN_MINPORT));
  
                  memcpy(&hcudp[SIXLOWPAN_HC1_HC_UDP_CHKSUM],
                         &udp->udpchksum, 2);
  
                  g_frame_hdrlen  += SIXLOWPAN_HC1_HC_UDP_HDR_LEN;
                  g_uncomp_hdrlen += UDP_HDRLEN;
                }
              else
                {
                  /* HC1 encoding and ttl */
  
                  hc1[SIXLOWPAN_HC1_ENCODING] = 0xfa;
                  hc1[SIXLOWPAN_HC1_TTL]      = ipv6->ttl;
                  g_frame_hdrlen             += SIXLOWPAN_HC1_HDR_LEN;
                }
  
              ret = COMPRESS_HDR_ELIDED;
            }
            break;
  #endif /* CONFIG_NET_UDP */
  
          default:
            {
              /* Test above assures that this will never happen */
  
              nerr("ERROR: Unhandled protocol\n");
              DEBUGPANIC();
            }
            break;
          }
      }
  
    return ret;
  }
  
  /****************************************************************************
   * Name: sixlowpan_uncompresshdr_hc1
   *
   * Description:
   *   Uncompress HC1 (and HC_UDP) headers and put them in sixlowpan_buf
   *
   *   This function is called by the input function when the dispatch is
   *   HC1.  It processes the frame in the IOB buffer, uncompresses the
   *   header fields, and copies the result in the packet buffer.  At the
   *   end of the decompression, g_frame_hdrlen and uncompressed_hdr_len
   *   are set to the appropriate values
   *
   * Input Parameters:
   *   metadata - Obfuscated MAC metadata including node addressing
   *              information.
   *   iplen    - Equal to 0 if the packet is not a fragment (IP length is
   *              then inferred from the L2 length), non 0 if the packet is
   *              a 1st fragment.
   *   iob      - Pointer to the IOB containing the received frame.
   *   fptr     - Pointer to frame to be uncompressed.
   *   bptr     - Output goes here.  Normally this is a known offset into
   *              d_buf, may be redirected to a "bitbucket" on the case of
   *              FRAGN frames.
   *
   * Returned Value:
   *   Zero (OK) is returned on success, on failure a negated errno value is
   *   returned.
   *
   ****************************************************************************/
  
  int sixlowpan_uncompresshdr_hc1(FAR struct radio_driver_s *radio,
                                  FAR const void *metadata, uint16_t iplen,
                                  FAR struct iob_s *iob, FAR uint8_t *fptr,
                                  FAR uint8_t *bptr)
  {
    FAR struct ipv6_hdr_s *ipv6 = (FAR struct ipv6_hdr_s *)bptr;
    FAR uint8_t *hc1 = fptr + g_frame_hdrlen;
    struct netdev_varaddr_s addr;
    int ret;
  
    ninfo("fptr=%p g_frame_hdrlen=%u\n", fptr, g_frame_hdrlen);
  
    /* Format the IPv6 header in the device d_buf.
     *
     * Set version, traffic clase, and flow label.  This assumes that Bit 4 is
     * set in HC1.
     */
  
    ipv6->vtc  = 0x60;  /* Bits 0-3: version, bits 4-7: traffic class (MS) */
    ipv6->tcf  = 0;     /* Bits 0-3: traffic class (LS), 4-bits: flow label (MS) */
    ipv6->flow = 0;     /* 16-bit flow label (LS) */
  
    g_uncomp_hdrlen += IPv6_HDRLEN;
  
    /* len[], proto, and ttl depend on the encoding */
  
    switch (hc1[SIXLOWPAN_HC1_ENCODING] & 0x06)
      {
  #ifdef CONFIG_NET_ICMPv6
      case SIXLOWPAN_HC1_NH_ICMPv6:
        ipv6->proto     = IP_PROTO_ICMP6;
        ipv6->ttl       = hc1[SIXLOWPAN_HC1_TTL];
        g_frame_hdrlen += SIXLOWPAN_HC1_HDR_LEN;
        break;
  #endif
  #ifdef CONFIG_NET_TCP
      case SIXLOWPAN_HC1_NH_TCP:
        ipv6->proto     = IP_PROTO_TCP;
        ipv6->ttl       = hc1[SIXLOWPAN_HC1_TTL];
        g_frame_hdrlen += SIXLOWPAN_HC1_HDR_LEN;
        break;
  #endif
  #ifdef CONFIG_NET_UDP
      case SIXLOWPAN_HC1_NH_UDP:
        {
          FAR struct udp_hdr_s *udp =
                           (FAR struct udp_hdr_s *)(bptr + IPv6_HDRLEN);
          FAR uint8_t *hcudp = fptr + g_frame_hdrlen;
  
          ipv6->proto = IP_PROTO_UDP;
  
          /* Check for HC_UDP encoding */
  
          if ((hcudp[SIXLOWPAN_HC1_HC_UDP_HC1_ENCODING] & 0x01) != 0)
            {
              /* UDP header is compressed with HC_UDP */
  
              if (hcudp[SIXLOWPAN_HC1_HC_UDP_UDP_ENCODING] !=
                  SIXLOWPAN_HC_UDP_ALL_C)
                {
                  nwarn("WARNING: "
                        "sixlowpan (uncompress_hdr), packet not supported");
                  return -EOPNOTSUPP;
                }
  
              /* IP TTL */
  
              ipv6->ttl = hcudp[SIXLOWPAN_HC1_HC_UDP_TTL];
  
              /* UDP ports, len, checksum */
  
              udp->srcport =
                HTONS(CONFIG_NET_6LOWPAN_MINPORT +
                      (hcudp[SIXLOWPAN_HC1_HC_UDP_PORTS] >> 4));
              udp->destport =
                HTONS(CONFIG_NET_6LOWPAN_MINPORT +
                      (hcudp[SIXLOWPAN_HC1_HC_UDP_PORTS] & 0x0f));
  
              ninfo("UDP srcport=%04x destport=%04x\n",
                     udp->srcport, udp->destport);
  
              memcpy(&udp->udpchksum,
                     &hcudp[SIXLOWPAN_HC1_HC_UDP_CHKSUM], 2);
  
              g_uncomp_hdrlen += UDP_HDRLEN;
              g_frame_hdrlen  += SIXLOWPAN_HC1_HC_UDP_HDR_LEN;
            }
          else
            {
              g_frame_hdrlen  += SIXLOWPAN_HC1_HDR_LEN;
            }
        }
        break;
  #endif /* CONFIG_NET_UDP */
  
      default:
        return -EPROTONOSUPPORT;
      }
  
    /* Re-create the link-local, mac-based IP address from src/dest node
     * addresses.
     *
     *   PC:  Prefix compressed (link-local prefix assumed)
     *   IC:  Interface identifier elided (derivable from the corresponding
     *        link-layer address).
     */
  
    if ((hc1[SIXLOWPAN_HC1_ENCODING] & SIXLOWPAN_HC1_SRCADDR_MASK) ==
        SIXLOWPAN_HC1_SRCADDR_PCIC)
      {
        ret = sixlowpan_extract_srcaddr(radio, metadata, &addr);
        if (ret < 0)
          {
            nerr("ERROR: sixlowpan_extract_srcaddr failed: %d\n", ret);
          }
        else
          {
            sixlowpan_ipfromaddr(&addr, ipv6->srcipaddr);
          }
      }
    else
      {
        nwarn("HC1 srcipaddr encoding not supported: %02x\n",
              hc1[SIXLOWPAN_HC1_ENCODING]);
      }
  
    ninfo("srcipaddr=%04x:%04x:%04x:%04x:%04x:%04x:%04x:%04x\n",
          NTOHS(ipv6->srcipaddr[0]), NTOHS(ipv6->srcipaddr[1]),
          NTOHS(ipv6->srcipaddr[2]), NTOHS(ipv6->srcipaddr[3]),
          NTOHS(ipv6->srcipaddr[4]), NTOHS(ipv6->srcipaddr[5]),
          NTOHS(ipv6->srcipaddr[6]), NTOHS(ipv6->srcipaddr[7]));
  
    if ((hc1[SIXLOWPAN_HC1_ENCODING] & SIXLOWPAN_HC1_DESTADDR_MASK) ==
        SIXLOWPAN_HC1_DESTADDR_PCIC)
      {
        ret = sixlowpan_extract_srcaddr(radio, metadata, &addr);
        if (ret < 0)
          {
            nerr("ERROR: sixlowpan_extract_srcaddr failed: %d\n", ret);
          }
        else
          {
            sixlowpan_ipfromaddr(&addr, ipv6->destipaddr);
          }
      }
    else
      {
        nwarn("HC1 destipaddr encoding not supported: %02x\n",
              hc1[SIXLOWPAN_HC1_ENCODING]);
      }
  
    ninfo("destipaddr=%04x:%04x:%04x:%04x:%04x:%04x:%04x:%04x\n",
          NTOHS(ipv6->destipaddr[0]), NTOHS(ipv6->destipaddr[1]),
          NTOHS(ipv6->destipaddr[2]), NTOHS(ipv6->destipaddr[3]),
          NTOHS(ipv6->destipaddr[4]), NTOHS(ipv6->destipaddr[5]),
          NTOHS(ipv6->destipaddr[6]), NTOHS(ipv6->destipaddr[7]));
  
    /* IP length field. */
  
    if (iplen == 0)
      {
        /* This is not a fragmented packet */
  
        ipv6->len[0] = 0;
        ipv6->len[1] = iob->io_len - g_frame_hdrlen + g_uncomp_hdrlen -
                       IPv6_HDRLEN;
      }
    else
      {
        /* This is a 1st fragment */
  
        ipv6->len[0] = (iplen - IPv6_HDRLEN) >> 8;
        ipv6->len[1] = (iplen - IPv6_HDRLEN) & 0x00ff;
      }
  
    ninfo("IPv6 len=%02x:%02x\n", ipv6->len[0], ipv6->len[1]);
  
  #ifdef CONFIG_NET_UDP
    /* Length field in UDP header */
  
    if (ipv6->proto == IP_PROTO_UDP)
      {
        FAR struct udp_hdr_s *udp =
                             (FAR struct udp_hdr_s *)(bptr + IPv6_HDRLEN);
        memcpy(&udp->udplen, &ipv6->len[0], 2);
  
        ninfo("IPv6 len=%04x\n", udp->udplen);
      }
  #endif
  
    return OK;
  }
  
  #endif /* CONFIG_NET_6LOWPAN_COMPRESSION_HC1 */
  