ipp.nc

tinyos-2.x.rar

	return;
    }

    /* UDP Destination Port */
    if ((hc2_enc & HC2_UDP_DST_PORT_MASK) == HC2_UDP_DST_PORT_INLINE) {
	dst_port = get_16t(buf);
	buf += sizeof(dst_port);
	len -= sizeof(dst_port);
    } else {
	//TODO
	return;
    }

    /* UDP Length */
    if ((hc2_enc & HC2_UDP_LEN_MASK) == HC2_UDP_LEN_INLINE) {
	/* check the length */
	if (ntohs(get_16t(buf)) != len + sizeof(uint16_t)*2) {
#ifdef ENABLE_PRINTF_DEBUG
	    printf("length check failed\n");
	    printf("reported length: %d\n", len + sizeof(uint16_t)*2);
	    printf("UDP header len: %d\n", ntohs(get_16t(buf)));
	    call PrintfFlush.flush();
#endif /* ENABLE_PRINTF_DEBUG */
	    return;
	}
	buf += sizeof(uint16_t);
	len -= sizeof(uint16_t);
    }
	
    /* Checksum */
    chksum = get_16t(buf);
    buf += sizeof(chksum);
    len -= sizeof(chksum);

    /* --- end of decompression --- */

    /* Compute and check the IP header checksum. */
    tmp_chksum = 0;
    /* IPv6 pseaudo header */
    tmp_chksum = ipv6_chksum(&rx_pkt.ip_src_addr, &rx_pkt.ip_dst_addr,
			     NEXT_HEADER_UDP,
			     /* len is only app data, so add UDP header
			      * length to get the length for chksum */
			     len + sizeof(struct udp_hdr),
			     tmp_chksum);
    /* UDP header */
    tmp_len = htons(len + sizeof(struct udp_hdr));
    tmp_chksum = ip_chksum((void*) &src_port, sizeof(src_port), tmp_chksum);
    tmp_chksum = ip_chksum((void*) &dst_port, sizeof(src_port), tmp_chksum);
    tmp_chksum = ip_chksum((void*) &chksum, sizeof(chksum), tmp_chksum);
    tmp_chksum = ip_chksum((void*) &tmp_len, sizeof(len), tmp_chksum);
    /* UDP payload - application data */
    tmp_chksum = ip_chksum(buf, len, tmp_chksum);

    if (tmp_chksum != 0xffff) {
#ifdef ENABLE_PRINTF_DEBUG
	printf("udp_input_compressed(): checksum failed\n");
	call PrintfFlush.flush();
#endif /* ENABLE_PRINTF_DEBUG */
	call Leds.led0Toggle();
	return;
    }

//     printf("udp_input_compressed()\n");
//     printf("src_port: 0x%X\n", src_port);
//     printf("dst_port: 0x%X\n", dst_port);
//     printf("len (app_data): %d\n", len);
//     call PrintfFlush.flush();

    /* Scan the list of UDP sockets and look for one that is accepting
       this port */
    for (c = 0, conn = udp_conns; c < COUNT_UDP_CONNS; c++, conn++) {
	/*
	printf("lport: 0x%X\n", conn->lport);
	printf("rport: 0x%X\n", conn->rport);
	printf("conn->ripaddr: ");
	dump_serial_packet(&(conn->ripaddr), sizeof(ip6_addr_t));
	printf("src_addr: ");
	dump_serial_packet(&rx_pkt.ip_src_addr, sizeof(ip6_addr_t));
	*/
	if ( (conn->lport != 0 && conn->lport == dst_port) &&
	     (conn->rport == 0 || conn->rport == src_port) &&
	     (ipv6_addr_is_zero(&(conn->ripaddr)) ||
	      (cmp_ipv6_addr(&conn->ripaddr, &rx_pkt.ip_src_addr) == 0))
	     )
	    goto udp_match_found;
    }
#ifdef ENABLE_PRINTF_DEBUG
    printf("udp_input_compressed(): "\
	   "no connection matched - dropping UDP packet\n");
    call PrintfFlush.flush();
#endif /* ENABLE_PRINTF_DEBUG */
    return;
    
 udp_match_found:
    if (len > 0) {
	signal UDPClient.receive[c](&rx_pkt.ip_src_addr, ntohs(src_port),
				    buf, len);
    }
}

/* processed the IPv6 header (uncompressed) */
void ipv6_input_uncompressed(uint8_t* buf, uint16_t len)
{
    struct ip6_hdr *hdr = (struct ip6_hdr *) buf;

    /* check the version */
    if ((hdr->vtc & IPV6_VERSION_MASK) != 0x60) {
#ifdef ENABLE_PRINTF_DEBUG
	printf("IP version check failed (%X)\n",
	       hdr->vtc & IPV6_VERSION_MASK);
	call PrintfFlush.flush();
#endif /* ENABLE_PRINTF_DEBUG */
	return;
    }
    
    /* Hop Limit */
    if (! hdr->hlim) {
	/* Hop Limit reached zero */
#ifdef ENABLE_PRINTF_DEBUG
	printf("Hop Limit reached zero\n");
	call PrintfFlush.flush();
#endif /* ENABLE_PRINTF_DEBUG */
	return;
    }

    /* check dst IP address */
    if (! ipv6_addr_is_for_me(&hdr->dst_addr)) {
	return;
    }

    /* Check the size of the packet. If the size reported to us in
     * uip_len doesn't match the size reported in the IP header, there
     * has been a transmission error and we drop the packet.
     */
    if ( hdr->plen != htons(len - sizeof(struct ip6_hdr))) {
#ifdef ENABLE_PRINTF_DEBUG
	printf("length check failed\n");
	printf("l2 reported length: %d\n", len - sizeof(struct ip6_hdr));
	printf("IPv6 header plen: %d (network byte order: 0x%X\n",
	       ntohs(hdr->plen), hdr->plen);
	//((hdr->plen & 0xff00) >> 8) & ((hdr->plen & 0xff) << 8));
	call PrintfFlush.flush();
#endif /* ENABLE_PRINTF_DEBUG */
	return;
    }
    
    /* copy IP addresses to rx_pkt */
    memcpy(&rx_pkt.ip_src_addr, &(hdr->src_addr), sizeof(rx_pkt.ip_src_addr));
    memcpy(&rx_pkt.ip_dst_addr, &(hdr->dst_addr), sizeof(rx_pkt.ip_dst_addr));

    /* multipex on next header */
    switch (hdr->nxt_hdr) {
    case NEXT_HEADER_ICMP6:
	icmpv6_input(buf + sizeof(struct ip6_hdr),
		     len - sizeof(struct ip6_hdr));
	break;
    case NEXT_HEADER_UDP:
	udp_input(buf + sizeof(struct ip6_hdr),
		  len - sizeof(struct ip6_hdr));
	break;
	/*
    case NEXT_HEADER_TCP:
	break;
	*/
    default:
#ifdef ENABLE_PRINTF_DEBUG
	printf("unknown IPv6 next header: 0x%X\n", hdr->nxt_hdr);
	call PrintfFlush.flush();
#endif /* ENABLE_PRINTF_DEBUG */
	break;
    }
}

/* processed the IPv6 header (uncompressed) */
void ipv6_input_compressed(uint8_t* buf, uint16_t len)
{
    struct ip6_hdr *ip_hdr = (struct ip6_hdr *) buf;

    uint8_t hc1_enc;
    uint8_t hc2_enc = 0;
    uint8_t next_header;

    /*
    printf("nxt_hdr: 0x%X\n", hdr->nxt_hdr);
    dump_serial_packet(buf, len);
    call PrintfFlush.flush();
    */

    hc1_enc = *buf;
    buf += sizeof(hc1_enc);
    len -= sizeof(hc1_enc);

    /* HC2 encoding follows HC1 encoding */
    if ((hc1_enc & HC1_HC2_MASK) == HC1_HC2_PRESENT) {
	hc2_enc = *buf;
	buf += sizeof(hc2_enc);
	len -= sizeof(hc2_enc);
    }

    /* Hop Limit */
    if (*buf) {
	buf += sizeof(ip_hdr->hlim);
	len -= sizeof(ip_hdr->hlim);
    } else {
	/* Hop Limit reached zero */
	return;
    }

    /* source IP address */
    if ((hc1_enc & HC1_SRC_PREFIX_MASK) == HC1_SRC_PREFIX_INLINE) {
	memcpy(&rx_pkt.ip_src_addr, buf, sizeof(rx_pkt.ip_src_addr)/2);
	buf += sizeof(rx_pkt.ip_src_addr)/2;
	len -= sizeof(rx_pkt.ip_src_addr)/2;
    } else {
	/* linl-local prefix */
	memset(&rx_pkt.ip_src_addr, 0, sizeof(rx_pkt.ip_src_addr)/2);
	rx_pkt.ip_src_addr.addr[0] = 0xFE;
	rx_pkt.ip_src_addr.addr[1] = 0x80;
    }
     
    if ((hc1_enc & HC1_SRC_IFACEID_MASK) == HC1_SRC_IFACEID_INLINE) {
	memcpy(((void*)&rx_pkt.ip_src_addr) + sizeof(rx_pkt.ip_src_addr)/2,
	       buf, sizeof(rx_pkt.ip_src_addr)/2);
	buf += sizeof(rx_pkt.ip_src_addr)/2;
	len -= sizeof(rx_pkt.ip_src_addr)/2;
    }

    /* destination IP address */
    if ((hc1_enc & HC1_DST_PREFIX_MASK) == HC1_DST_PREFIX_INLINE) {
	memcpy(&rx_pkt.ip_dst_addr, buf, sizeof(rx_pkt.ip_dst_addr)/2);
	buf += sizeof(rx_pkt.ip_dst_addr)/2;
	len -= sizeof(rx_pkt.ip_dst_addr)/2;
    } else {
	/* linl-local prefix */
	memset(&rx_pkt.ip_dst_addr, 0, sizeof(rx_pkt.ip_dst_addr)/2);
	rx_pkt.ip_dst_addr.addr[0] = 0xFE;
	rx_pkt.ip_dst_addr.addr[1] = 0x80;
    }
     
    if ((hc1_enc & HC1_DST_IFACEID_MASK) == HC1_DST_IFACEID_INLINE) {
	memcpy(((void*)&rx_pkt.ip_dst_addr) + sizeof(rx_pkt.ip_dst_addr)/2,
	       buf, sizeof(rx_pkt.ip_dst_addr)/2);
	buf += sizeof(rx_pkt.ip_dst_addr)/2;
	len -= sizeof(rx_pkt.ip_dst_addr)/2;
    }

    /* check dst IP address */
    if (! ipv6_addr_is_for_me(&rx_pkt.ip_dst_addr)) {
	/*
	printf("IP address check failed\n");
	dump_serial_packet(hdr->dst_addr.addr, sizeof(hdr->dst_addr.addr));
	call PrintfFlush.flush();
	*/
	return;
    }

    /* Traffic Class and Flow Label */
    if ((hc1_enc & HC1_TCFL_MASK) == HC1_TCFL_INLINE) {
	//TODO
	return;
    }

    /* Next Header */
    switch (hc1_enc & HC1_NEXTHDR_MASK) {
    case HC1_NEXTHDR_INLINE:
	next_header = *buf;
	buf += sizeof(uint8_t);
	len -= sizeof(uint8_t);
	break;
    case HC1_NEXTHDR_UDP:
	next_header = NEXT_HEADER_UDP;
	break;
    case HC1_NEXTHDR_ICMP:
	next_header = NEXT_HEADER_ICMP6;
	break;
    case HC1_NEXTHDR_TCP:
	next_header = NEXT_HEADER_TCP;
	break;
    default:
#ifdef ENABLE_PRINTF_DEBUG
	printf("unknown next header HC1 encoding\n");
	call PrintfFlush.flush();
#endif /* ENABLE_PRINTF_DEBUG */
	return;
    }
    
    /* multipex on the next header */
    switch (next_header) {
    case NEXT_HEADER_ICMP6:
	icmpv6_input(buf, len);
	break;
    case NEXT_HEADER_UDP:
	/* HC_UDP compression */
	if ((hc1_enc & HC1_HC2_MASK) == HC1_HC2_PRESENT
	    && (hc1_enc & HC1_NEXTHDR_MASK) == HC1_NEXTHDR_UDP) {
	    udp_input_compressed(buf, len, hc2_enc);
	    break;
	} else {
	    udp_input(buf, len);
	    break;
	}
	/*
    case NEXT_HEADER_TCP:
	break;
	*/
    default:
#ifdef ENABLE_PRINTF_DEBUG
	printf("unknown IPv6 next header: 0x%X\n", next_header);
	call PrintfFlush.flush();
#endif /* ENABLE_PRINTF_DEBUG */
	break;
    }
}

/* call the right fct for processing the IPv6 header */
void layer3_input(uint8_t *buf, uint16_t len)
{
    uint8_t *dispatch = buf;
    buf++;
    len--;

    /* uncompressed IPv6 */
    if (*dispatch == DISPATCH_UNCOMPRESSED_IPV6) {
	ipv6_input_uncompressed(buf, len);
    }
    /* LOWPAN_HC1 compressed IPv6 */
    else if (*dispatch == DISPATCH_COMPRESSED_IPV6) {
	//call Leds.led1Toggle();
 	return ipv6_input_compressed(buf, len);
    }
    /* unknown dispatch value if we got here */
    else {
	//TODO: report an error
    }
}

/* process the optional 6lowpan headers */
void TRUSTEDBLOCK lowpan_input(uint8_t* buf, uint8_t len )
{
      uint8_t *dispatch;
      struct lowpan_broadcast_hdr *bc_hdr;
      struct lowpan_frag_hdr *frag_hdr;
      int i;

      frag_buf_t *frag;
      uint16_t dgram_tag;
      uint16_t dgram_size;
      uint8_t dgram_offset;
      frag_info_t *p;
      frag_info_t **q;
      uint8_t last_frag;

      dispatch = buf;
      /* --- 6lowpan optional headers --- */
      /* Mesh Addressing header */
      if ( (*dispatch & DISPATCH_MESH_MASK) == DISPATCH_MESH) {
	  // check if we're the final recipient in the mesh addressing header
	  buf++;
	  len--;

	  /* Hops Left */
	  if ((*dispatch & 0x0F) == 0) {
	      goto discard_packet;
	  }

	  /* Final Destination Address */
	  if (*dispatch & DISPATCH_MESH_F_FLAG) {
	      rx_pkt.hw_dst_addr.type = HW_ADDR_LONG;
	      memcpy(&rx_pkt.hw_dst_addr.addr_long, buf,
		     sizeof(rx_pkt.hw_dst_addr.addr_long));
	      buf += sizeof(rx_pkt.hw_dst_addr.addr_long);
	      len -= sizeof(rx_pkt.hw_dst_addr.addr_long);
	  } else {
	      rx_pkt.hw_dst_addr.type = HW_ADDR_SHORT;
	      memcpy(&rx_pkt.hw_dst_addr.addr_short, buf,
		     sizeof(rx_pkt.hw_dst_addr.addr_short));
	      buf += sizeof(rx_pkt.hw_dst_addr.addr_short);
	      len -= sizeof(rx_pkt.hw_dst_addr.addr_short);
	  }

	  /* check if we're the recipient */
	  if (! hw_addr_is_for_me(&rx_pkt.hw_dst_addr)) {
	      // TODO: if mesh forwarding enabled, then forward
	      goto discard_packet;
	  }

	  /* Originator Address */
	  if (*dispatch & DISPATCH_MESH_O_FLAG) {
	      rx_pkt.hw_src_addr.type = HW_ADDR_LONG;
	      memcpy(&rx_pkt.hw_src_addr.addr_long, buf,
		     sizeof(rx_pkt.hw_src_addr.addr_long));
	      buf += sizeof(rx_pkt.hw_src_addr.addr_long);
	      len -= sizeof(rx_pkt.hw_src_addr.addr_long);
	  } else {
	      rx_pkt.hw_src_addr.type = HW_ADDR_SHORT;
	      memcpy(rx_pkt.hw_src_addr.addr_short, buf,
		     sizeof(rx_pkt.hw_src_addr.addr_short));
	      buf += sizeof(rx_pkt.hw_src_addr.addr_short);
	      len -= sizeof(rx_pkt.hw_src_addr.addr_short);
	  }

	  dispatch = buf;
      }
      if (*dispatch == DISPATCH_BC0) { /* Broadcast header */
	  bc_hdr = (struct lowpan_broadcast_hdr *) buf;
	  // do something usefull with bc_hdr->seq_no...
	  
	  buf += (sizeof(struct lowpan_broadcast_hdr));
	  len -= (sizeof(struct lowpan_broadcast_hdr));
	  dispatch = buf;
      }

      /* fragment header */
      if ((*dispatch & DISPATCH_FRAG_MASK)
	  == DISPATCH_FIRST_FRAG
	  || (*dispatch & DISPATCH_FRAG_MASK)
	  == DISPATCH_SUBSEQ_FRAG
	  ) {
	  frag_hdr = (struct lowpan_frag_hdr *) buf;
	  buf += sizeof(struct lowpan_frag_hdr);
	  len -= sizeof(struct lowpan_frag_hdr);

	  /* collect information about the fragment */
	  dgram_tag = get_16t(&frag_hdr->dgram_tag);
	  //dgram_size = get_16t(&frag_hdr->dgram_size);
	  //dgram_size &= htons(0x07FF);
	  dgram_size = frag_hdr->dgram_size8[1];
	  dgram_size += (frag_hdr->dgram_size8[0] & 0x07) << 8;
	  dgram_size = htons(dgram_size);
	  if ((*dispatch & DISPATCH_FRAG_MASK) == DISPATCH_SUBSEQ_FRAG) {
	      dgram_offset = *buf;
	      buf += 1;
	      len -= 1;
	  } else {
	      dgram_offset = 0;
	  }

#ifdef ENABLE_PRINTF_DEBUG