lib6lowpanip.c

tinyos-2.x.rar

    } else {
      // otherwise who knows what's here... it's an error because the
      // NH bit said we were inline but when we got here, we didn't
      // recognize the NH encoding.
      return NULL;
    }
  } else {
    // there was no IPNH field, but there might be uncompressed fields
    // we want to copy out for consistency (since we always unpack all
    // headers not part of the payload).
    uint8_t nhdr = *nxt_hdr;
    uint8_t nhdr_len = 0;
    struct ip6_ext *hdr;
    u_info->nxt_hdr = nhdr;

    // copy any IPv6 extension headers out of the packet.
    // the rule is that the extension headers must fit in the first
    // fragment so we can route on them after only one fragment.
    while (EXTENSION_HEADER(nhdr)) {
      hdr = (struct ip6_ext *)buf;

      switch (nhdr) {
      case IPV6_HOP:
        u_info->hdr_hop = (struct ip6_ext *)buf;
        break;
      case IPV6_ROUTING:
        u_info->hdr_route = (struct ip6_route *)buf;
        break;
      case IPV6_DEST:
        u_info->hdr_dest = (struct ip6_ext *)buf;
        break;
      }
      nhdr = hdr->nxt_hdr;
      nhdr_len = hdr->len;
      u_info->nxt_hdr = nhdr;

      if (len < nhdr_len) return NULL;
      ip_memcpy(dest, buf, nhdr_len);
      dest += nhdr_len;
      buf += nhdr_len;

      u_info->payload_offset += nhdr_len;
      extra_header_length += nhdr_len;
    }

    u_info->transport_ptr = dest;
  }

  u_info->payload_start = buf;
  u_info->header_end = dest;
  if (u_info->transport_ptr == NULL)
    u_info->transport_ptr = dest;

  // we can go back and figure out the payload length now that we know
  // how long the compressed headers were
  if (hasFrag1Header(pkt) || hasFragNHeader(pkt)) {
    getFragDgramSize(pkt, &size);
    size -= sizeof(struct ip6_hdr);
  } else {
    // it's a one fragment packet
    size = pkt->len - (buf - pkt->data);
    size += extra_header_length;
    size -= getLowpanPayload(pkt) - pkt->data;
  }

  *plen = size >> 8;
  *(plen + 1) = size & 0xff;

  // finally fill in the udp length field that we finally can recompute
  switch (*nxt_hdr) {
  case IANA_UDP:
    *prot_len = size >> 8;
    *(prot_len + 1) = size & 0xff;
  }
  

  return buf;
}

/* packs addr into *buf, and updates the pointer relative to the length
 * that was needed.
 * @returns the bit flags indicating which length was used
 */
static uint8_t packAddress(uint8_t dispatch, uint8_t **buf, ip6_addr_t addr) {

  if ((dispatch == LOWPAN_HC_CRP_PATTERN && globalPrefix &&
      cmpPfx(addr, __my_address.s6_addr)) ||
      (dispatch == LOWPAN_HC_LOCAL_PATTERN &&
       cmpPfx(addr, linklocal_prefix))) {
    // only choice here are 64-bit and 16-bit addresses
    if (ipv6_addr_suffix_is_long(addr)) {
      // use the 64-bit compression
      ip_memcpy(*buf, &addr[8], 8);
      *buf += 8;
      return LOWPAN_IPHC_ADDR_64;
    } else {
      // down to 16 bits: we never use the 0-bit compression
      // (althought we could for link local).
      ip_memcpy(*buf, &addr[14], 2);
      *buf += 2;
      return LOWPAN_IPHC_ADDR_16;
    }
  } else if (addr[0] == 0xff && // is multicast
             addr[1] < 0x0f) { // the scope is small enough
    // XXX : SDH : Need to check that the group is small enough
    **buf = LOWPAN_IPHC_HC_MCAST; // set the encoding bits
    **buf |= (addr[1] << LOWPAN_HC_MCAST_SCOPE_OFFSET); // scope

    // direct mapped group id
    **buf |= addr[14] & 0x1;
    *(*buf + 1) = addr[15];
    *buf += 2;
    return LOWPAN_IPHC_ADDR_16;
  } else {
    // fuck it, send the whole thing
    ip_memcpy(*buf, addr, 16);
    *buf += 16;
    return LOWPAN_IPHC_ADDR_128;
  }
}

/*
 * pack the headers of msg into the buffer pointed to by buf.
 * 
 * @returns a pointer to where we stopped writing
 */
uint8_t packHeaders(struct split_ip_msg *msg,
                    uint8_t *buf, uint8_t len) {
  uint8_t *dispatch, *encoding, addr_enc, nxt_hdr;
  struct ip6_hdr *hdr = &msg->hdr;
  dispatch = buf;
  buf += 1;
  encoding = buf;
  buf += 1;
  *encoding = 0;

  if (!(hdr->vlfc[0] == (IPV6_VERSION << 4) && 
        hdr->vlfc[1] == 0 &&
        hdr->vlfc[2] == 0 &&
        hdr->vlfc[3] == 0)) {
    ip_memcpy(buf, &hdr->vlfc, 4);
    buf += 4;
  } else {
    *encoding |= LOWPAN_IPHC_VTF_MASK;
  }

  nxt_hdr = hdr->nxt_hdr;
  if (hdr->nxt_hdr == IANA_UDP && /* or other compressed values... */
      msg->headers != NULL) {
    // we will add the HCNH encoding at the end of the header
    *encoding |= LOWPAN_IPHC_NH_MASK;
  } else {
    *buf = hdr->nxt_hdr;
    buf += 1;
  }

  // always carry hop limit
  *buf = hdr->hlim;
  buf += 1;

  if (globalPrefix && cmpPfx(hdr->ip6_src.s6_addr, __my_address.s6_addr)) {
    *dispatch = LOWPAN_HC_CRP_PATTERN;
  } else if (globalPrefix && cmpPfx(hdr->ip6_dst.s6_addr, __my_address.s6_addr)) {
    *dispatch = LOWPAN_HC_CRP_PATTERN;
  } else if (cmpPfx(hdr->ip6_src.s6_addr, linklocal_prefix)) {
    *dispatch = LOWPAN_HC_LOCAL_PATTERN;
  } else {
    *dispatch = LOWPAN_HC_LOCAL_PATTERN;
  }

  addr_enc = packAddress(*dispatch, &buf, hdr->ip6_src.s6_addr);
  *encoding |= addr_enc << LOWPAN_IPHC_SC_OFFSET;

  addr_enc = packAddress(*dispatch, &buf, hdr->ip6_dst.s6_addr);
  *encoding |= addr_enc << LOWPAN_IPHC_DST_OFFSET;

  len -= (buf - dispatch);
  // now come the compressions for special next header values.
  // we pack all the headers in the split message into this fragment, and fail if we cannot;
  {
    int i = 0;
    struct generic_header *cur = msg->headers;
    while (cur != NULL) {
      if (nxt_hdr == IANA_UDP && i == 0) {
        struct udp_hdr *udp = cur->hdr.udp;
  
        uint8_t *udp_enc = buf;
        uint8_t *cmpr_port = NULL;
        // do the LOWPAN_UDP coding
        
        if (len < sizeof(struct udp_hdr)) return (buf - dispatch);
        
        *udp_enc = LOWPAN_UDP_DISPATCH;;
        buf += 1;


        if ((ntoh16(udp->srcport) & LOWPAN_UDP_PORT_BASE_MASK) == 
            LOWPAN_UDP_PORT_BASE) {
          //printf("compr to 4b\n");
          cmpr_port = buf;
          *cmpr_port = (ntoh16(udp->srcport) & ~LOWPAN_UDP_PORT_BASE_MASK) << 4;
          *udp_enc |= LOWPAN_UDP_S_MASK;
          buf += 1;
        } else {
          ip_memcpy(buf, (uint8_t *)&udp->srcport, 2);
          buf += 2;
        }
        
        if ((ntoh16(udp->dstport) & LOWPAN_UDP_PORT_BASE_MASK) == 
            LOWPAN_UDP_PORT_BASE) {
          if (cmpr_port == NULL) {
            // the source port must not have been compressed, so 
            // allocate a new byte for this guy
            *buf = ((ntoh16(udp->dstport) & ~LOWPAN_UDP_PORT_BASE_MASK) << 4);
            buf += 1;
          } else {
            // already in the middle of a byte for the port compression,
            // so fill in the rest of the byte
            *cmpr_port = *cmpr_port | ((ntoh16(udp->dstport) & ~LOWPAN_UDP_PORT_BASE_MASK));
          }
          *udp_enc |= LOWPAN_UDP_D_MASK;
        } else {
          ip_memcpy(buf, (uint8_t *)&udp->dstport, 2);
          buf += 2;
        }
        
        // we never elide the checksum
        ip_memcpy(buf, (uint8_t *)&udp->chksum, 2);
        buf += 2;
      } else {
        // otherwise we just need to copy the extension header
        if (len < cur->len) return 0;
        ip_memcpy(buf, (uint8_t *)cur->hdr.ext, cur->len);
        len -= cur->len;
        buf += cur->len;
      }
      cur = cur->next;
      i++;
    }
  }
  // I think we're done here...
  return buf - dispatch;
}

/*
 * indicates how much of the packet after the IP header we will pack
 *
 */
/* int packs_header(struct split_ip_msg *msg) { */
/*   switch (hdr->nxt_hdr) { */
/*   case IANA_UDP: */
/*     return sizeof(struct udp_hdr); */
/*   default: */
/*     return 0; */
/*   } */
/* } */



#ifndef NO_LIB6LOWPAN_ASCII

#define TO_CHAR(X) (((X) < 10) ? ('0' + (X)) : ('a' + ((X) - 10)))
#define CHAR_VAL(X)  (((X) >= '0' && (X) <= '9') ? ((X) - '0') : \
                      (((X) >= 'A' && (X) <= 'F') ? ((X) - 'A' + 10) : ((X) - 'a' + 10)))


void inet_pton6(char *addr, struct in6_addr *dest) {
  uint16_t cur = 0;
  char *p = addr;
  uint8_t block = 0, shift = 0;
  if (addr == NULL || dest == NULL) return;
  ip_memclr(dest->s6_addr, 16);

  // first fill in from the front
  while (*p != '\0') {
    if (*p != ':') {
      cur <<= 4;
      cur |= CHAR_VAL(*p);
    } else {
      dest->s6_addr16[block++] = hton16(cur);
      cur = 0;
    }
    p++;
    if (*p == '\0')
      return;
    if (*(p - 1) == ':' && *p == ':') {
      break;
    }
  }
  // we must have hit a "::" which means we need to start filling in from the end.
  block = 7;
  cur = 0;
  while (*p != '\0') p++;
  p--;
  // now pointing at the end of the address string
  while (p > addr) {
    if (*p != ':') {
      cur |= (CHAR_VAL(*p) << shift);
      shift += 4;
    } else {
      dest->s6_addr16[block--] = hton16(cur);
      cur = 0; shift = 0;
    }
    p --;
    if (*(p + 1) == ':' && *p == ':') break;
  }
}



int inet_ntop6(struct in6_addr *addr, char *buf, int cnt) {
  uint16_t block;
  char *end = buf + cnt;
  int i, j, compressed = 0;

  for (j = 0; j < 8; j++) {
    if (buf > end - 7) return -1;

    block = ntohs(addr->s6_addr16[j]);
    for (i = 4; i <= 16; i+=4) {
      if (block > (0xffff >> i) || (compressed == 2 && i == 16)) {
        *buf++ = TO_CHAR((block >> (16 - i)) & 0xf);
      }
    }
    if (addr->s6_addr16[j] == 0 && compressed == 0) {
      *buf++ = ':';
      compressed++;
    }
    if (addr->s6_addr16[j] != 0 && compressed == 1) compressed++;

    if (j < 7 && compressed != 1) *buf++ = ':';
  }
  *buf++ = '\0';
  return buf - (end - cnt);
}

#endif