ipp.nc

tinyos-2.x.rar

	  printf("off: %d\n", dgram_offset);
#endif /* ENABLE_PRINTF_DEBUG */
	  /*
	  printf("off: %d, f_b[%d] %d\n",
		 dgram_offset, 0, frag_bufs[0].frag_timeout);
	  */
	  frag = find_fragment(&rx_pkt.hw_src_addr, &rx_pkt.hw_dst_addr,
			       dgram_size, dgram_tag);
	  /*
	  if (frag) {
	      printf("frag found\n");
	  } else {
	      printf("frag NOT found\n");
	  }
	  */
	  if (frag) {
	      /* fragment reassembly buffer found */
	      /* check for overlap */
	      //TODO: ENABLE THIS PART !!!
// 	      for (p = frag->frag_list; p; p=p->next) {
// 		  if (dgram_offset == p->offset){
// 		      if (len == p->len) {
// 			  /* same offset, same len => discard this duplicate */
// 			  goto discard_packet;
// 		      } else {
// 			  /* same offset, but different len */
// 			  goto frag_overlap;
// 		      }
// 		  } else if (dgram_offset > p->offset
// 			     && dgram_offset < p->offset + p->len/8
// 			     ) {
// 		      /* offset inside another frag*/
// 		      goto frag_overlap;
// 		  }
// 	      }
	      /* no overlap found */
	      //printf("frag found: %d\n", frag->frag_timeout);
	      goto frag_reassemble;
	  } else {
	      /* fragment reassembly buffer not found - set up a new one */
	      // no match found -- need a new frag_buf_t
	      for (i = 0; i< FRAG_BUFS; i++) {
		  if (frag_bufs[i].frag_timeout == FRAG_FREE
		      && call AppDataPool.empty() == FALSE) {
		      frag = &frag_bufs[i];
		      set_16t(&frag->dgram_tag, get_16t(&frag_hdr->dgram_tag));
		      set_16t(&frag->dgram_size, dgram_size);
		      memcpy(&frag->hw_src_addr, &rx_pkt.hw_src_addr,
			     sizeof(frag->hw_src_addr));
		      memcpy(&frag->hw_dst_addr, &rx_pkt.hw_dst_addr,
			     sizeof(frag->hw_dst_addr));
		      frag->frag_timeout = FRAG_TIMEOUT;
		      frag->buf = (uint8_t *) call AppDataPool.get();
		      frag->frag_list = NULL;
		      /*
		      printf("new frag_buf[%d] %d\n", i,
			     frag_bufs[i].frag_timeout);
		      printf("frag pool size: %d\n", call FragInfoPool.size());
		      call PrintfFlush.flush();
		      */
		      goto frag_reassemble;
		  }
	      }
	      // no free slot for reassembling fragments
#ifdef ENABLE_PRINTF_DEBUG
	      printf("no free slot - discarding frag\n");
	      call PrintfFlush.flush();
#endif /* ENABLE_PRINTF_DEBUG */
	      goto discard_packet;
	  }
	  
      frag_overlap:
	  /* overlap - discard previous frags
	   * and restart fragment reassembly
	   */
	  free_frag_list(frag->frag_list);
	  frag->frag_list = NULL;
	  frag->frag_timeout = FRAG_TIMEOUT;
	  goto frag_reassemble;
	  
      frag_reassemble:
	  /*
	  printf("tag: 0x%04X, size: %d, off: %d, t: %d\n",
		 get_16t(&frag->dgram_tag),
		 ntohs(get_16t(&frag->dgram_size)),
		 dgram_offset,
		 frag->frag_timeout);
	  //printf("f_b[%d] %d\n", 0, frag_bufs[0].frag_timeout);
	  if (dgram_offset > 0) {
	      call PrintfFlush.flush();
	  }
	  */
	  /* copy buf data */
	  //if (dgram_offset*8 + len <= sizeof(frag->buf)) {
	  if (dgram_offset*8 + len <= FRAG_BUF_SIZE) {
	      memcpy(frag->buf + (dgram_offset*8), buf, len);
	  } else {
	      call Leds.led0Toggle();
	  }
	  
	  /* update frag_info */
	  p = call FragInfoPool.get();
	  if (!p) {
	      //out of memory - fragment reassembly failing
	      //TODO
	      call Leds.led0Toggle();
#ifdef ENABLE_PRINTF_DEBUG
	      printf("FAILED to alloc frag_info_t\n");
	      call PrintfFlush.flush();
#endif /* ENABLE_PRINTF_DEBUG */
	  } else {
	      p->offset = dgram_offset;
	      p->len = len;
	      
	      /* insert frag_info into the orderer list */
	      if (frag->frag_list) {
		  for(q = &(frag->frag_list); (*q)->next; q=&((*q)->next)) {
		      if (p->offset > (*q)->offset) {
			  break;
		      }
		  }
		  p->next = *q;
		  *q = p;
	      } else {
		  p->next = frag->frag_list;
		  frag->frag_list = p;
	      }
	  }

#ifdef ENABLE_PRINTF_DEBUG
	  if (dgram_offset > 20) {
	      printf("frag_list:\n");
	      //ntohs(get_16t(&frag->dgram_tag)),
	      //ntohs(get_16t(&frag->dgram_size)));
	      for (p=frag->frag_list;p;p=p->next) {
		  printf("off: %d, len: %d\n", p->offset, p->len);
	      }
	      call PrintfFlush.flush();
	  }
#endif /* ENABLE_PRINTF_DEBUG */

	  /* check if this is not the last fragment */
	  if (!dgram_offset) {
	      /* the first fragment cannot be the last one */
	      last_frag = 0;
	  } else {
	      last_frag=1;
	      dgram_offset = ntohs(dgram_size)/8;
	      for(p=frag->frag_list; p && dgram_offset; p=p->next) {
		  //debug("dgram_offset: %d, p->offset: %d, p->len: %d\n",
		  //  dgram_offset, p->offset, p->len);
		  if (p->offset + p->len/8 != dgram_offset) {
		      //debug("offset mismatch - not the last fragment\n");
		      last_frag = 0;
		      break;
		  }
		    dgram_offset = p->offset;
	      }
	  }
	  
	  if (last_frag) {
	      call Leds.led1Toggle();
	      /* prepare the complete packet to be passed up*/
	      lowpan_pkt_clear(&rx_pkt);
	      rx_pkt.app_data = frag->buf;
	      rx_pkt.app_data_dealloc = APP_DATA_DEALLOC_TRUE;
	      rx_pkt.header_begin = frag->buf;
	      rx_pkt.header_len = ntohs(dgram_size);

	      //debug("dumping reassembled datagram...\n");
	      //dump_serial_packet(pkt->buf_begin, pkt->len);

	      /* pass up the packet */
	      layer3_input(rx_pkt.header_begin, rx_pkt.header_len);
	      
	      /* deallocate all fragment info */
	      free_frag_list(frag->frag_list);
	      frag->frag_list = NULL;
	      frag->frag_timeout = FRAG_FREE;
     	      if (rx_pkt.app_data_dealloc == APP_DATA_DEALLOC_TRUE
		  && rx_pkt.app_data) {
		  /* deallocate the frag_buf */
		  call AppDataPool.put((app_data_t *) rx_pkt.app_data);
	      }
	  } else {
	      /* packet not yet complete */
	      return;
	  }
	  dispatch = buf;
      } else {
	  /* no fragmentation */

	  /* pass up the complete packet */
	  lowpan_pkt_clear(&rx_pkt);
	  rx_pkt.header_begin = buf;
	  rx_pkt.header_len = len;
	  layer3_input(buf, len);
      }
      
      
 discard_packet:
      // deallocate pkt
      // update stats
}

/* Receive an AM from the lower layer */
event TRUSTEDBLOCK message_t* Receive.receive(message_t* msg, void* payload, uint8_t len)
{
    am_addr_t am_addr;
    
    //call Leds.led0Toggle();
    
    /* 802.15.4 source address */
    rx_pkt.hw_src_addr.type = HW_ADDR_SHORT;
    am_addr = call AMPacket.source(msg);
    memcpy(&rx_pkt.hw_src_addr.addr_short, &am_addr, sizeof(am_addr_t));
    
    /* 802.15.4 destination address */
    rx_pkt.hw_dst_addr.type = HW_ADDR_SHORT;
    am_addr = call AMPacket.destination(msg);
    memcpy(&rx_pkt.hw_dst_addr.addr_short, &am_addr, sizeof(am_addr_t));
    
    lowpan_input(payload, len);
    return msg;
}

/******************************************
 *  Interface StdControl
 ******************************************/
  
command error_t IPControl.start() 
{
#ifdef ENABLE_PRINTF_DEBUG
    call PrintfControl.start();
#endif /* ENABLE_PRINTF_DEBUG */
    ip_init();
    linklocal_addr.addr[0] = 0xfe;
    linklocal_addr.addr[1] = 0x80;
    ipv6_iface_id_from_am_addr(call AMPacket.address(),
			       &(linklocal_addr.addr[8]));
    //set_16t((uint16_t *)&(linklocal_addr.addr[14]), am_addr);
    call MessageControl.start();
    return SUCCESS;
}

event void MessageControl.startDone(error_t err) {
    if (err == SUCCESS) {
	signal IPControl.startDone(err);
	call Timer.startPeriodic(1024); /* fire every second */
    }
    else {
	call MessageControl.start();
    }
}

command error_t IPControl.stop()
{
    call MessageControl.stop();
    call Timer.stop();      
#ifdef ENABLE_PRINTF_DEBUG
    call PrintfControl.stop();
#endif /* ENABLE_PRINTF_DEBUG */
    return SUCCESS;
}

event void MessageControl.stopDone(error_t err) {
    signal IPControl.stopDone(err);
}

/******************************************
 *  IP Interface
 ******************************************/
command void IP.getAddress(ip6_addr_t *addr)
{
    addr =  &global_addr;
    //uip_unpack_ipaddr( uip_global_addr, addr->addr );
}

command void IP.setAddress(const ip6_addr_t *addr)
{
    memcpy(&global_addr, addr, sizeof(*addr));
    //uip_pack_ipaddr(uip_global_addr,octet1,octet2,octet3,octet4);
}

command void IP.setAddressAutoconf(const ip6_addr_t *addr)
{
    memcpy(&global_addr, addr, sizeof(*addr));
    ipv6_iface_id_from_am_addr(call AMPacket.address(),
			       &(global_addr.addr[8]));
    //set_16t((uint16_t *)&(global_addr.addr[14]), am_addr);
}

/*****************************
 *  UDP functions
 *****************************/
command error_t UDPClient.listen[uint8_t num](uint16_t port)
{
    if (port) {
	memset(&udp_conns[num].ripaddr, 0,
	       sizeof(udp_conns[num].ripaddr));
	set_16t(&udp_conns[num].lport, htons(port));
    } else {
	set_16t(&udp_conns[num].lport, 0);
    }
    return SUCCESS;
}

command error_t
UDPClient.connect[uint8_t num](const ip6_addr_t *addr, const uint16_t port)
{
    struct udp_conn *conn = &udp_conns[num];
    
    if (addr && port) {
	memcpy(&conn->ripaddr, addr, sizeof(conn->ripaddr));
	set_16t(&conn->rport, htons(port));
    }
    else {
	memset(&conn->ripaddr, 0 , sizeof(conn->ripaddr));
	set_16t(&conn->rport, 0);
    }

    return SUCCESS;
}

command error_t
UDPClient.sendTo[uint8_t num](const ip6_addr_t *addr, uint16_t port,
			      const uint8_t *buf, uint16_t len)
{
    if (udp_conns[num].lport == 0) {
	set_16t(&udp_conns[num].lport, htons(udp_assign_port()));
    }
    return udp_compressed_output(buf, len,
				 NULL, addr,
				 udp_conns[num].lport, htons(port), num+1);
}

command error_t UDPClient.send[uint8_t num]( const uint8_t *buf, uint16_t len )
{
    if (udp_conns[num].rport == 0
	|| ipv6_addr_is_zero(&udp_conns[num].ripaddr))
	return FAIL;
    return call UDPClient.sendTo[num](&(udp_conns[num].ripaddr),
				      udp_conns[num].rport,
				      buf, len);
}

default event void
UDPClient.sendDone[uint8_t num](error_t result, void* buf)
{
}

default event void
UDPClient.receive[uint8_t num](const ip6_addr_t *addr, uint16_t port, 
			       uint8_t *buf, uint16_t len)
{
}

/******************************************
 *  Printf Timer
 ******************************************/
#ifdef ENABLE_PRINTF_DEBUG
event void PrintfFlush.flushDone(error_t error) {}
event void PrintfControl.startDone(error_t error) {}
event void PrintfControl.stopDone(error_t error) {}
static void dump_serial_packet(const unsigned char *packet, const int len)
{
    int i;
    printf("len: %d\n", len);
    //call PrintfFlush.flush();
    if (!packet) {
	printf("packet is NULL");
    } else {
	for (i = 0; i < len; i++)
	    printf("%02x ", packet[i]);
    }
    printf("\n");
    //call PrintfFlush.flush();
}
#endif /* ENABLE_PRINTF_DEBUG */
#ifndef ENABLE_PRINTF_DEBUG
static void dump_serial_packet(const unsigned char *packet, const int len)
{}
#endif /* ENABLE_PRINTF_DEBUG */

/******************************************
 *  Interface Timer
 ******************************************/

event void Timer.fired() {
    int i=0;

    /* heartbeat led */
    //call Leds.led0Toggle();
    
    /* discard timed-out and not yet assembled fragmented packet */
    for (i=0;i<FRAG_BUFS; i++) {
	if (frag_bufs[i].frag_timeout != FRAG_FREE) {
	    if (frag_bufs[i].frag_timeout > 0) {
		frag_bufs[i].frag_timeout--;
	    } else {
		/* fragment reassembly timed out */
		frag_bufs[i].frag_timeout = FRAG_FREE;
		free_frag_list(frag_bufs[i].frag_list);
		if (frag_bufs[i].buf) {
		    call AppDataPool.put((app_data_t *) frag_bufs[i].buf);
		}
		//call Leds.led0Toggle();
	    }
	}
    }
    
    //TODO: check for timed-out ND request and resend/give up and mark as such
    //TODO: check outgoing pkts queue and schedule ND or sending
    /*
      counter++;
      if (locked) {
      return;
      }
      else {
      //Packet.clear(&test_packet);
      uint8_t* data=(uint8_t*) call Packet.getPayload(&test_packet,NULL);
      if (call Packet.maxPayloadLength() < 1) {
      return;
      }
      
      data[0] = counter;
      call AMPacket.setSource(&test_packet, 0x14);
      if (call AMSend.send(3, &test_packet, 1) == SUCCESS) {
      //      if (call AMSend.send(AM_BROADCAST_ADDR, &test_packet, sizeof(test_serial_msg_t)) == SUCCESS) {
      locked = TRUE;
      }
      }
    */
}

}