ipp.nc

tinyos2.0版本驱动

	return FALSE;
}

uint8_t cmp_ipv6_addr(const ip6_addr_t *addr1, const ip6_addr_t *addr2)
{
    return memcmp(addr1, addr2, sizeof(ip6_addr_t));
}

uint8_t ipv6_addr_is_for_me(const ip6_addr_t *addr)
{
    //TODO: loopback addr (::1)
    //TODO: interface-local addr FF01::1
    if (cmp_ipv6_addr(addr, &global_addr) == 0 ||
	cmp_ipv6_addr(addr, &linklocal_addr) == 0 ||
	ipv6_addr_is_linklocal_allnodes(addr) ||
	(ipv6_addr_is_solicited_node_multicast_prefix(addr)
	 && (((addr->addr[13] == global_addr.addr[13])
	      && (addr->addr[14] == global_addr.addr[14])
	      && (addr->addr[15] == global_addr.addr[15])
	      ) ||
	     ((addr->addr[13] == linklocal_addr.addr[13])
		  && (addr->addr[14] == linklocal_addr.addr[14])
	      && (addr->addr[15] == linklocal_addr.addr[15])
	      )
	     )
	 )
	)
	return 1;
    else
	return 0;
}

/* determine the right src_addr given a dst_addr */
ip6_addr_t * determine_src_ipv6_addr(const ip6_addr_t *dst_addr)
{
    if (ipv6_addr_is_linklocal(dst_addr)) {
	return &linklocal_addr;
    } else {
	return &global_addr;
    }
}

uint8_t cmp_hw_addr(const hw_addr_t *addr1, const hw_addr_t *addr2)
{
    // for short addresses compare only the first two bytes
    if (addr1->type == HW_ADDR_SHORT && addr2->type == HW_ADDR_SHORT) {
	return memcmp(addr1->addr_short, addr2->addr_short, 2);
    } else {
	return memcmp(addr1, addr2, sizeof(hw_addr_t));
    }
}

uint8_t hw_addr_is_broadcat(const hw_addr_t *hw_addr)
{
    if (hw_addr->type == HW_ADDR_SHORT
	&& hw_addr->addr_short[0] == 0xFF
	&& hw_addr->addr_short[1] == 0xFF)
	return 1;
    // TODO: long address
    else return 0;
}

uint8_t hw_addr_is_for_me(const hw_addr_t *addr)
{
    am_addr_t am_addr = call AMPacket.address();
    if (hw_addr_is_broadcat(addr) ||
	(addr->addr_short[0] == (uint8_t) am_addr
	 && addr->addr_short[1] == (uint8_t) (am_addr >> 8))
	)
	return 1;
    else
	return 0;
}
/*---------------------------------------------------------------------------*/

void increment_g_dgram_tag()
{
    uint16_t tmp = ntohs(g_dgram_tag);
    if (tmp == 0xFFFF)
	tmp = 0;
    else
	tmp++;
    g_dgram_tag = htons(tmp);
}

void lowpan_pkt_clear(lowpan_pkt_t *pkt)
{
    memset(pkt, 0, sizeof(*pkt));
    pkt->header_begin = pkt->header + sizeof(pkt->header);
}

/*
void frag_buf_clear(frag_buf_t *frag_buf)
{
    memset(frag_buf, 0, sizeof(*frag_buf));
    frag_buf->buf_begin = frag_buf->buf;
}
*/

frag_buf_t * find_fragment(hw_addr_t *hw_src_addr, hw_addr_t *hw_dst_addr,
			   uint16_t dgram_size, uint16_t dgram_tag)
{
    int i;
    for (i = 0; i< FRAG_BUFS; i++) {
	//printf("find_frag\n");
	if (frag_bufs[i].frag_timeout != FRAG_FREE) {
	    //printf("find: [%d] %d\n", i, frag_bufs[i].frag_timeout);
	    /*
	  printf("find: tag: 0x%04X, size: %d\n",
		 get_16t(&frag_bufs[i].dgram_tag),
		 ntohs(get_16t(&frag_bufs[i].dgram_size)));
	    */
	    if (   get_16t(&(frag_bufs[i].dgram_tag)) == dgram_tag
		&& get_16t(&(frag_bufs[i].dgram_size)) == dgram_size
		&& cmp_hw_addr(&frag_bufs[i].hw_src_addr, hw_src_addr) == 0
		&& cmp_hw_addr(&frag_bufs[i].hw_dst_addr, hw_dst_addr) == 0
		) {
		return &(frag_bufs[i]);
	    }
	} else {
	    //printf("find: [%d] FREE\n", i);
	}
    }
    return NULL;
}

void free_frag_list(frag_info_t *p)
{
    frag_info_t *q;
    while (p) {
	q = p->next;
	call FragInfoPool.put(p);
	p = q;
    }
}
/*---------------------------------------------------------------------------*/
  
    void ip_init()
    {
	//int i;
	lastport = 1024;
	memset(udp_conns, 0, sizeof(udp_conns));
	//memset(&global_addr, 0, sizeof(global_addr));
	memset(&linklocal_addr, 0, sizeof(linklocal_addr));
	memset(frag_bufs, 0, sizeof(frag_bufs));
	/*
	for(i=0;i<FAG_BUFS,i++) {
	    frag_buf_clear(frag_bufs[i]);
	}
	*/
    }

    uint16_t udp_assign_port()
    {
	int c;
	
	/* Find an unused local port. */
    again:
	++lastport;
	
	if (lastport >= 32000) {
	    lastport = 4096;
	}
	
	for (c = 0; c < COUNT_UDP_CONNS; ++c) {
	    if (udp_conns[c].lport == lastport) {
		goto again;
	    }
	}
	
	return lastport;
    }    

/* ========================= IPv6 - output ================================= */
task void sendTask()
{
    lowpan_pkt_t *pkt = send_queue;
    struct lowpan_frag_hdr *frag_hdr;
    uint8_t *payload;
    uint8_t frame_len;
    uint8_t len; /* length of the fragment just being sent
		  * excluding the 6lowpan optional headers */
    uint8_t remaining_len; /* how much more data can we fit 
			    * into this fragment */

    uint8_t *tmp_cpy_buf; /* simplifies memcpy */
    uint8_t tmp_cpy_len; /* simplifies memcpy */

    if (!pkt || g_send_pending) {
	return;
    }
    
    //len = pkt->header_len + pkt->app_data_len - pkt->dgram_offset*8;
    if (pkt->header_len + pkt->app_data_len <= LINK_DATA_MTU) {
	/* fragmentation not needed */
	frame_len = pkt->header_len + pkt->app_data_len;
	/* prepare the AM */
	call Packet.clear(&g_msg);
	call Packet.setPayloadLength(&g_msg, frame_len);
	payload = call Packet.getPayload(&g_msg, frame_len);
	// memset(payload, 0 , payload_len);
	// should check payload_len here
	
	/* copy header */
	if (pkt->header_begin && pkt->header_len)
	    memcpy(payload, pkt->header_begin, pkt->header_len);
	payload += pkt->header_len;

	/* copy app_data */
	if (pkt->app_data_begin && pkt->app_data_len)
	    memcpy(payload, pkt->app_data_begin, pkt->app_data_len);

    } else {
	/* do fragmentation */
	if (pkt->dgram_offset == 0) {
	    /* first fragment */
	    increment_g_dgram_tag();
	    set_16t(&pkt->dgram_size,
		    htons(pkt->header_len + pkt->app_data_len));
	    
	    /* align fragment length at an 8-byte multiple */
	    len = LINK_DATA_MTU - sizeof(struct lowpan_frag_hdr);
	    len -= len%8;
	    frame_len = len + sizeof(struct lowpan_frag_hdr);
	} else {
	    /* subsequent fragment */
	    if (pkt->header_len + pkt->app_data_len - pkt->dgram_offset*8
		<= LINK_DATA_MTU - sizeof(struct lowpan_frag_hdr)
		- sizeof(uint8_t)) {
		/* last fragment -- does not have to be aligned
		 * at an 8-byte multiple */
		len = pkt->header_len + pkt->app_data_len
		    - pkt->dgram_offset*8;
	    } else {
		/* align fragment length at an 8-byte multiple */
		len = LINK_DATA_MTU - sizeof(struct lowpan_frag_hdr)
		    - sizeof(uint8_t);
		len -= len%8;
	    }
	    frame_len = len + sizeof(struct lowpan_frag_hdr)
		+ sizeof(uint8_t);
	}
    
	/* prepare the AM */
	call Packet.clear(&g_msg);
	call Packet.setPayloadLength(&g_msg,frame_len);
	payload = call Packet.getPayload(&g_msg, frame_len);
	remaining_len = frame_len;
	if (remaining_len != frame_len) {
	    //TODO: report an error
#ifdef ENABLE_PRINTF_DEBUG
	    printf("payload length does not match requested length\n");
	    call PrintfFlush.flush();
#endif /* ENABLE_PRINTF_DEBUG */
	    return;
	}

	/* fill in the fragment header */
	frag_hdr = (struct lowpan_frag_hdr *) payload;
	set_16t(&frag_hdr->dgram_size, pkt->dgram_size);
	set_16t(&frag_hdr->dgram_tag, g_dgram_tag);
	payload += sizeof(struct lowpan_frag_hdr);
	remaining_len -= sizeof(struct lowpan_frag_hdr);
	
	if (pkt->dgram_offset == 0) {
	    /* first fragment */
	    frag_hdr->dispatch |= DISPATCH_FIRST_FRAG;
	} else {
	    /* subsequent fragment */
	    frag_hdr->dispatch |= DISPATCH_SUBSEQ_FRAG;
	    *payload = pkt->dgram_offset;
	    payload += sizeof(uint8_t);
	}

	/* copy header */
	if (pkt->header_begin
	    && pkt->header_len
	    && pkt->header_len > pkt->dgram_offset*8
	       /* don't copy the header if offset is beyond it*/
	    ) {
	    /* determine what has to be copied */
	    tmp_cpy_buf = pkt->header_begin + pkt->dgram_offset*8;
	    tmp_cpy_len = min(pkt->header_len - pkt->dgram_offset*8,
			  remaining_len);
	    /* copy it */
	    memcpy(payload, tmp_cpy_buf, tmp_cpy_len);
	    payload += tmp_cpy_len;
	    remaining_len -= tmp_cpy_len;
	}
	
	/* copy app_data */
	if (remaining_len
	    && pkt->app_data_begin
	    && pkt->app_data_len
	    ) {
	    /* determine what has to be copied */
	    if (pkt->dgram_offset*8 > pkt->header_len) {
		tmp_cpy_buf = pkt->app_data_begin
		              + pkt->dgram_offset*8 - pkt->header_len;
	    } else {
		/* header has been copied only now, offset not yet updated */
		tmp_cpy_buf = pkt->app_data_begin;
	    }
	    tmp_cpy_len = min(remaining_len,
			      pkt->app_data_len
			      - (pkt->dgram_offset*8 - pkt->header_len));
	    /* copy it */
	    memcpy(payload, tmp_cpy_buf, tmp_cpy_len);
	    payload += tmp_cpy_len;
	    remaining_len -= tmp_cpy_len;
	}
		
	/* update the offset - in 8-byte multiples */
	pkt->dgram_offset += len/8;
	if (len%8) {
	    /* last fragment with a special length */
	    pkt->dgram_offset++;
	}
    }

    /* send the AM */
    g_send_pending = 1;
    call AMSend.send(AM_BROADCAST_ADDR, &g_msg, frame_len);
    //call AMSend.send(0x12, &g_msg, frame_len);
}

event void AMSend.sendDone(message_t* msg, error_t error)
{
    uint16_t len;
    lowpan_pkt_t *pkt = send_queue;

    g_send_pending = 0;

    if (!send_queue) {
	// somethign really went wrong...
	return;
    }

    len = pkt->header_len + pkt->app_data_len;
    if (len <= LINK_DATA_MTU || pkt->dgram_offset*8 >= len){
	/* packet has been completely sent, we can move on to the next one */

	/* UDPClient.sendDone notification */
	if (send_queue->notify_num != LOWPAN_PKT_NO_NOTIFY) {
	    signal UDPClient.sendDone[send_queue->notify_num - 1]
		(SUCCESS, send_queue->app_data);
	}

	/* deallocation of app_data (fragment reassembly buffer) */
	if (send_queue->app_data_dealloc == APP_DATA_DEALLOC_TRUE
	    && send_queue->app_data) {
	    call AppDataPool.put((app_data_t*) send_queue->app_data);
	    send_queue->app_data = NULL;
	}
	
	pkt = send_queue->next;
	call SendPktPool.put(send_queue);
	send_queue = pkt;
    }
    if (send_queue) {
	post sendTask();
    }
}

void ipv6_output_uncompressed(lowpan_pkt_t *pkt, const uint8_t next_header)
{
    struct ip6_hdr *hdr;
    lowpan_pkt_t *p;

    pkt->header_begin -= sizeof(struct ip6_hdr);
    pkt->header_len += sizeof(struct ip6_hdr);
    hdr = (struct ip6_hdr *) pkt->header_begin;

    /* fill in the IPv6 header */
    hdr->vtc = IPV6_VERSION; /* IPv6 version */
    /* payload length */
    set_16t(&hdr->plen, htons(pkt->header_len + pkt->app_data_len
			      - sizeof(struct ip6_hdr)));
    
    hdr->nxt_hdr = next_header;
    hdr->hlim = IP_HOP_LIMIT; /* hop limit */
    
    memcpy(&hdr->src_addr, &pkt->ip_src_addr, sizeof(hdr->src_addr));
    memcpy(&hdr->dst_addr, &pkt->ip_dst_addr, sizeof(hdr->dst_addr));

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

    //TODO: check if neighbor is information available
    //  if yes
    //    fill in hw_addr
    //    append to send_queue
    //  else
    //    append to neighbor_queue
    //    request ND, add an entry into the neighbor table

    /* 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();
}

/* determines length of the inline carried fields for the HC1 encoding
 * the return value is the number of bits, bot bytes !!!
 */
int get_hc1_length(uint8_t hc1_enc)
{
    int len = 0;
    /* Hop Limit always carried inline */
    len += 8;
    
    /* source IP address */
    if ((hc1_enc & HC1_SRC_PREFIX_MASK) == HC1_SRC_PREFIX_INLINE)
	len += 64;
     
    if ((hc1_enc & HC1_SRC_IFACEID_MASK) == HC1_SRC_IFACEID_INLINE)
	len += 64;

    /* destination IP address */
    if ((hc1_enc & HC1_DST_PREFIX_MASK) == HC1_DST_PREFIX_INLINE)
	len += 64;
     
    if ((hc1_enc & HC1_DST_IFACEID_MASK) == HC1_DST_IFACEID_INLINE)
	len += 64;

    /* Traffic Class and Flow Label */
    if ((hc1_enc & HC1_TCFL_MASK) == HC1_TCFL_INLINE)
	len += 24;

    /* Next Header */
    if ((hc1_enc & HC1_NEXTHDR_MASK) == HC1_NEXTHDR_INLINE)