ipp.nc

tinyos2.0版本驱动

	len += 8;

    return len;
}

void ipv6_compressed_output(lowpan_pkt_t *pkt, const uint8_t next_header,
			    uint8_t hc2_enc, bool hc2_present)
{
    lowpan_pkt_t *p;
    uint8_t hc1_enc = 0;

    /* HC2 compression */
    if (hc2_present) {
	hc1_enc |= HC1_HC2_PRESENT;
    } else {    
	hc1_enc |= HC1_HC2_NONE;
    }

    /* next header */
    switch (next_header) {
    case NEXT_HEADER_UDP:
	hc1_enc |= HC1_NEXTHDR_UDP;
	break;
    case NEXT_HEADER_ICMP6:
	hc1_enc |= HC1_NEXTHDR_ICMP;
	break;
    case NEXT_HEADER_TCP:
	hc1_enc |= HC1_NEXTHDR_TCP;
	break;
    default:
	hc1_enc |= HC1_NEXTHDR_INLINE;

	pkt->header_begin -= sizeof(next_header);
	pkt->header_len += sizeof(next_header);
	*(pkt->header_begin) = next_header;
	break;
    }

    /* we're always sending packets with TC anf FL zero */
    hc1_enc |= HC1_TCFL_ZERO;
    
    /* destination address interface identifier */
    hc1_enc |= HC1_DST_IFACEID_INLINE;
    
    pkt->header_begin -= 8;
    pkt->header_len += 8;
    memcpy(pkt->header_begin, ((void*)&(pkt->ip_dst_addr)) + 8, 8);

    /* destination address prefix */
    if (ipv6_addr_is_linklocal_unicast(&pkt->ip_dst_addr)) {
	hc1_enc |= HC1_DST_PREFIX_LINKLOCAL;
    } else {
	hc1_enc |= HC1_DST_PREFIX_INLINE;

	pkt->header_begin -= 8;
	pkt->header_len += 8;
	memcpy(pkt->header_begin, &(pkt->ip_dst_addr), 8);
    }

    /* source address interface identifier */
    hc1_enc |= HC1_SRC_IFACEID_INLINE;
    
    pkt->header_begin -= 8;
    pkt->header_len += 8;
    memcpy(pkt->header_begin, ((void*)&(pkt->ip_src_addr)) + 8, 8);

    /* source address prefix */
    if (ipv6_addr_is_linklocal_unicast(&pkt->ip_src_addr)) {
	hc1_enc |= HC1_SRC_PREFIX_LINKLOCAL;
    } else {
	hc1_enc |= HC1_SRC_PREFIX_INLINE;

	pkt->header_begin -= 8;
	pkt->header_len += 8;
	memcpy(pkt->header_begin, &(pkt->ip_src_addr), 8);
    }

    /* Hop Limit */
    pkt->header_begin -= sizeof(uint8_t);
    pkt->header_len += sizeof(uint8_t);
    *pkt->header_begin = IP_HOP_LIMIT;
    
    /* HC2 encoding field */
    if (hc2_present) {
	pkt->header_begin -= sizeof(uint8_t);
	pkt->header_len += sizeof(uint8_t);
	*(pkt->header_begin) = hc2_enc;
    }

    /* HC1 encoding field */
    pkt->header_begin -= sizeof(uint8_t);
    pkt->header_len += sizeof(uint8_t);
    *(pkt->header_begin) = hc1_enc;

    /* set 6lowpan dispatch value */
    pkt->header_begin -= sizeof(uint8_t);
    pkt->header_len += sizeof(uint8_t);
    *(pkt->header_begin) = DISPATCH_COMPRESSED_IPV6;

    /* append pkt to send queue */
    if(!send_queue) {
	send_queue = pkt;
    } else {
	for(p=send_queue; p->next; p=p->next);
	p->next = pkt;
    }

    /* schedule sendTask */
    post sendTask();
}

void icmpv6_output(lowpan_pkt_t *pkt,
		   uint8_t type, uint8_t code)
{
    struct icmp6_hdr *hdr;
    uint16_t cksum = 0;
    /* fill in the source address if not set */
    if (ipv6_addr_is_zero(&pkt->ip_src_addr)) {
	memcpy(&pkt->ip_src_addr,
	       determine_src_ipv6_addr(&pkt->ip_dst_addr),
	       sizeof(pkt->ip_src_addr));
    }

    /* fill in the ICMPv6 header */
    pkt->header_begin -= sizeof(struct icmp6_hdr);
    pkt->header_len += sizeof(struct icmp6_hdr);
    hdr = (struct icmp6_hdr *) pkt->header_begin;

    hdr->type = type;
    hdr->code = code;

    /* calculate the  checksum */
    set_16t(&hdr->cksum, 0);
    cksum = ipv6_chksum(&pkt->ip_src_addr, &pkt->ip_dst_addr,
			NEXT_HEADER_ICMP6,
			pkt->header_len + pkt->app_data_len, cksum);
    cksum = ip_chksum((void*)hdr, sizeof(struct icmp6_hdr), cksum);
    cksum = ip_chksum(pkt->app_data_begin, pkt->app_data_len,
			   cksum);
    cksum = ~cksum;
    set_16t(&hdr->cksum, cksum);
    
    ipv6_compressed_output(pkt, NEXT_HEADER_ICMP6, 0, FALSE);
}

error_t udp_uncompressed_output(void* buf, uint16_t len,
				const ip6_addr_t *src_addr,
				const ip6_addr_t *dst_addr,
				uint16_t src_port,
				uint16_t dst_port,
				uint8_t udp_client_num)
{
    struct udp_hdr *hdr;
    lowpan_pkt_t *pkt;
    uint16_t cksum = 0;

    if (!dst_addr) return FAIL;

    pkt = call SendPktPool.get();
    if (!pkt) return FAIL;

    lowpan_pkt_clear(pkt);
    
    /* set the UDPCliemt number to allow for signalling sendDone */
    pkt->notify_num = udp_client_num;

    /* set application data */
    pkt->app_data = buf;
    pkt->app_data_begin = buf;
    pkt->app_data_len = len;
    
    /* set IP addresses */
    memcpy(&pkt->ip_dst_addr, dst_addr, sizeof(pkt->ip_dst_addr));
    if (src_addr) {
	memcpy(&pkt->ip_src_addr, src_addr, sizeof(pkt->ip_src_addr));
    } else {
	memcpy(&pkt->ip_src_addr,
	       determine_src_ipv6_addr(dst_addr),
	       sizeof(pkt->ip_src_addr));
    }

    /* fill in the UDP header */
    pkt->header_begin -= sizeof(struct udp_hdr);
    pkt->header_len += sizeof(struct udp_hdr);
    hdr = (struct udp_hdr *) pkt->header_begin;
    /* src port */
    set_16t(&hdr->srcport, src_port);
    /* dst port */
    set_16t(&hdr->dstport, dst_port);
    /* length */
    set_16t(&hdr->len,htons(len + sizeof(struct udp_hdr)));
    /* checksum */
    set_16t(&hdr->chksum, 0);
    cksum = ip_chksum((uint8_t*) hdr, sizeof(struct udp_hdr), cksum);
    cksum = ipv6_chksum(&pkt->ip_dst_addr,
			&pkt->ip_src_addr,
			NEXT_HEADER_UDP,
			sizeof(struct udp_hdr) + len, cksum);
    cksum = ip_chksum(buf, len, cksum);
    if (cksum != 0xFFFF) {
	cksum = ~cksum;
    }
    set_16t(&hdr->chksum, cksum);

    ipv6_compressed_output(pkt, NEXT_HEADER_UDP, 0, FALSE);

    return SUCCESS;
}

error_t udp_compressed_output(void* buf, uint16_t len,
			      const ip6_addr_t *src_addr,
			      const ip6_addr_t *dst_addr,
			      uint16_t src_port,
			      uint16_t dst_port,
			      uint8_t udp_client_num)
{
    lowpan_pkt_t *pkt;
    uint16_t cksum = 0;
    uint16_t hc2_enc = 0;
    uint16_t udp_len = htons(len + sizeof(struct udp_hdr));

    if (!dst_addr) return FAIL;

    pkt = call SendPktPool.get();
    if (!pkt) return FAIL;

    lowpan_pkt_clear(pkt);
    
    /* set the UDPCliemt number to allow for signalling sendDone */
    pkt->notify_num = udp_client_num;

    /* set application data */
    pkt->app_data = buf;
    pkt->app_data_begin = buf;
    pkt->app_data_len = len;
    
    /* set IP addresses */
    memcpy(&pkt->ip_dst_addr, dst_addr, sizeof(pkt->ip_dst_addr));
    if (src_addr) {
	memcpy(&pkt->ip_src_addr, src_addr, sizeof(pkt->ip_src_addr));
    } else {
	memcpy(&pkt->ip_src_addr,
	       determine_src_ipv6_addr(dst_addr),
	       sizeof(pkt->ip_src_addr));
    }

    /* Checksum */
    cksum = 0;
    cksum = ip_chksum((void*) &src_port, sizeof(src_port), cksum);
    cksum = ip_chksum((void*) &dst_port, sizeof(src_port), cksum);
    cksum = ip_chksum((void*) &udp_len, sizeof(udp_len), cksum);
    cksum = ipv6_chksum(&pkt->ip_dst_addr,
			&pkt->ip_src_addr,
			NEXT_HEADER_UDP,
			sizeof(struct udp_hdr) + len, cksum);
    cksum = ip_chksum(buf, len, cksum);
    if (cksum != 0xFFFF) {
	cksum = ~cksum;
    }

    /* HC_UDP encoding */
    /* Checksum */
    pkt->header_begin -= sizeof(cksum);
    pkt->header_len += sizeof(cksum);
    set_16t(pkt->header_begin, cksum);

    /* Length */
    //hc2_enc |= HC2_UDP_LEN_COMPR;
    hc2_enc |= HC2_UDP_LEN_INLINE;
    pkt->header_begin -= sizeof(udp_len);
    pkt->header_len += sizeof(udp_len);
    set_16t(pkt->header_begin, udp_len);

    /* Destination Port */
    hc2_enc |= HC2_UDP_DST_PORT_INLINE;
    pkt->header_begin -= sizeof(dst_port);
    pkt->header_len += sizeof(dst_port);
    set_16t(pkt->header_begin, dst_port);

    /* Source Port */
    hc2_enc |= HC2_UDP_SRC_PORT_INLINE;
    pkt->header_begin -= sizeof(src_port);
    pkt->header_len += sizeof(src_port);
    set_16t(pkt->header_begin, src_port);
    
    ipv6_compressed_output(pkt, NEXT_HEADER_UDP, hc2_enc, TRUE);

    return SUCCESS;
}
/* ========================== IPv6 - input ================================= */
void icmpv6_input(uint8_t* buf, uint16_t len)
{
    lowpan_pkt_t *pkt;
    struct icmp6_hdr *hdr = (struct icmp6_hdr *)buf;

    /* Compute and check the IP header checksum. */
    if (ipv6_chksum_data(&rx_pkt.ip_src_addr, &rx_pkt.ip_dst_addr,
		    NEXT_HEADER_ICMP6, buf, len, 0)
	!= 0xffff) {
#ifdef ENABLE_PRINTF_DEBUG
	printf("icmpv6_input(): checksum failed\n");
	call PrintfFlush.flush();
#endif /* ENABLE_PRINTF_DEBUG */
	call Leds.led0Toggle();
	return;
    }

    buf += sizeof(struct icmp6_hdr);
    len -= sizeof(struct icmp6_hdr);
    
    switch (hdr->type) {
    case ICMP_TYPE_ECHO_REQUEST:
	/* ICMP code has to be 0 */
	if (hdr->code != 0) {
	    return;
	}
	
	call Leds.led2Toggle();
 	/* send back an ICMP ECHO REPLY */

	/* allocate a packet for the reply */
	pkt = call SendPktPool.get();
	if (!pkt) {
#ifdef ENABLE_PRINTF_DEBUG
	    printf("icmpv6_input() - failed to alloc pkt\n");
	    call PrintfFlush.flush();
#endif /* ENABLE_PRINTF_DEBUG */
	    return;
	}
	lowpan_pkt_clear(pkt);
	
	/* copy/set ICMP data */
	if (rx_pkt.app_data) {
	    /* fragment reassembly took place - ICMP data is in app_data buf */
	    pkt->app_data = rx_pkt.app_data;
	    pkt->app_data_begin = buf;
	    pkt->app_data_len = len;
	    pkt->app_data_dealloc = rx_pkt.app_data_dealloc;

	    rx_pkt.app_data_dealloc = APP_DATA_DEALLOC_FALSE;
	    rx_pkt.app_data = NULL;
	} else {
	    /* there is no app_data buf, everything fits into the header buf */
	    pkt->header_begin -= len;
	    my_memcpy(pkt->header_begin, buf, len);
	    pkt->app_data_begin = pkt->header_begin;
	    pkt->app_data_len = len;
	}
	
	/* set destination address */
	memcpy(&pkt->ip_dst_addr, &rx_pkt.ip_src_addr,
	       sizeof(pkt->ip_dst_addr));
	// source address determined automatically

	icmpv6_output(pkt, ICMP_TYPE_ECHO_REPLY, 0);
	break;
    case ICMP_TYPE_ECHO_REPLY:
	break;
	
    }
}

/* UDP input processing. */
void udp_input(uint8_t* buf, uint16_t len)
{
    struct udp_conn *conn;
    int c;
    struct udp_hdr *hdr = (struct udp_hdr *)buf;
    
    /* Compute and check the IP header checksum. */
    if (ipv6_chksum_data(&rx_pkt.ip_src_addr, &rx_pkt.ip_dst_addr,
		    NEXT_HEADER_UDP, buf, len, 0)
	!= 0xffff) {
#ifdef ENABLE_PRINTF_DEBUG
	printf("udp_input(): checksum failed\n");
	call PrintfFlush.flush();
#endif /* ENABLE_PRINTF_DEBUG */
	call Leds.led0Toggle();
	return;
    }

    if (htons(len) != hdr->len) {
#ifdef ENABLE_PRINTF_DEBUG
	printf("length check failed\n");
	printf("reported length: %d\n", len);
	printf("UDP header len: %d\n", ntohs(hdr->len));
	call PrintfFlush.flush();
#endif /* ENABLE_PRINTF_DEBUG */
	return;
    }
    /* 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(src_addr, sizeof(ip6_addr_t));
	*/
	if ( (conn->lport != 0 && conn->lport == hdr->dstport) &&
	     (conn->rport == 0 || conn->rport == hdr->srcport) &&
	     (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(): no connection matched - dropping UDP packet\n");
    call PrintfFlush.flush();
#endif /* ENABLE_PRINTF_DEBUG */
    return;
    
 udp_match_found:
    len -= sizeof(struct udp_hdr);
    if (len > 0) {
	signal UDPClient.receive[c](&rx_pkt.ip_src_addr, ntohs(hdr->srcport),
				    buf+sizeof(struct udp_hdr), len);
    }
}

void udp_input_compressed(uint8_t* buf, uint16_t len, uint8_t hc2_enc)
{
    struct udp_conn *conn;
    int c;
    uint16_t src_port;
    uint16_t dst_port;
    uint16_t chksum;
    uint16_t tmp_chksum;
    uint16_t tmp_len;
    
    /* UDP Source Port */
    if ((hc2_enc & HC2_UDP_SRC_PORT_MASK) == HC2_UDP_SRC_PORT_INLINE) {
	src_port = get_16t(buf);
	buf += sizeof(src_port);
	len -= sizeof(src_port);
    } else {
	//TODO