flooding.c

SOS操作系统用于无线传感器网络节点的源代码

/* -*- Mode: C; tab-width:4 -*- */
/* ex: set ts=4 shiftwidth=4 softtabstop=4 cindent: */
#include <module.h>
#include "flooding.h"

//--------------------------------------------------------------
// Queue to hold info. about recently flooded packets
typedef struct{
  uint16_t originaddr;
  int16_t seqno;
  sos_pid_t dst_pid;
  uint8_t dest_msg_type;
} flood_queue_t;



//--------------------------------------------------------------
// Flooding Routing Module State
typedef struct {
  uint8_t flood_queue_tail;                    //! Tail of flooding queue
  flood_queue_t flood_queue[FLOOD_QUEUE_SIZE]; //! Flooding queue
  uint8_t seqno;                               //! Current sequence number at this node
} flooding_state_t;


static uint8_t flood_get_hdr_size(func_cb_ptr proto);
//static int8_t flood_pkt(func_cb_ptr proto, sos_pid_t dest_pid, uint8_t dest_msg_type, uint8_t* pktpayload, uint8_t payloadlen);
int8_t flood_pkt(sos_pid_t dest_pid, uint8_t dest_msg_type, uint8_t* pktpayload, uint8_t payloadlen,uint16_t daddr);
//by wmchen

static int8_t match_flood_header(flooding_state_t *s, flood_hdr_t *hdr);
static int8_t insert_flood_header(flooding_state_t *s, flood_hdr_t *hdr);
static int8_t module(void *state, Message *msg);

//--------------------------------------------------------------
// Module Declaration
static mod_header_t SOS_MODULE_HEADER mod_header  = {
	.mod_id         = MOD_FLOODING_PID,
	.state_size     = sizeof(flooding_state_t),
	.platform_type  = HW_TYPE /* or PLATFORM_ANY */,
	.processor_type = MCU_TYPE,
	.code_id        = ehtons(MOD_FLOODING_PID),
	.num_timers     = 1,
	.num_sub_func   = 0,
	.num_prov_func  = 2,
	.module_handler = module,	
        /*
        .funct[2] = {
		{flood_get_hdr_size, "Cvv0", MOD_FLOODING_PID, MOD_FLOODING_GET_HDR_SIZE},
		 {flood_pkt, "cyC4", MOD_FLOODING_PID, MOD_SEND_PKT}
	}, */
};

static int8_t module(void *state, Message *msg)
{
	// Cast state into correct structure
	flooding_state_t *s = (flooding_state_t*) state;

	switch (msg->type) {
		case MSG_INIT:
		{
			s->flood_queue_tail = 0;
			return SOS_OK;
		}
        /* add by wmchen*/
   /*     case MSG_SEND_PACKET: */
/* 	{ */
/* 	  flood_hdr_t *hdr = (flood_hdr_t*)(msg->data); */
/* 	  hdr->originaddr = ker_id(); */
/* 	  hdr->seqno = s->seqno; */
/* 	  hdr->originhopcount = 0; */
/* 	  hdr->maxhopcount = FLOOD_MAX_HOP_COUNT; */
/* 	  if (post_net(MOD_FLOODING_PID, MOD_FLOODING_PID, MSG_FLOOD_DATA_PKT, msg->len, msg->data, SOS_MSG_DYM_MANAGED, BCAST_ADDRESS) == SOS_OK) */
/* 		return SOS_TAKEN; */
/* 	  else */
/* 		return SOS_OK; */
/* 	} */


         /* end of adding */
	
/*   case MSG_SEND_PACKET: */
/* 	{ */
/* 	  flood_hdr_t *hdr = (flood_hdr_t*)(msg->data); */
/* 	  hdr->originaddr = ker_id(); */
/* 	  hdr->seqno = s->seqno; */
/* 	  hdr->originhopcount = 0; */
/* 	  hdr->maxhopcount = FLOOD_MAX_HOP_COUNT; */
/* 	  if (post_net(MOD_FLOODING_PID, MOD_FLOODING_PID, MSG_FLOOD_DATA_PKT, msg->len, msg->data, SOS_MSG_DYM_MANAGED, BCAST_ADDRESS) == SOS_OK) */
/* 		return SOS_TAKEN; */
/* 	  else */
/* 		return SOS_OK; */
/* 	} */

  case MSG_FLOOD_DATA_PKT:
	{
	  int i;
	  uint8_t* dupbuff;
          uint16_t *pp;
          uint8_t* temp;
	  uint8_t l;
	  flood_hdr_t *hdr = (flood_hdr_t*)msg->data;
	  if (match_flood_header(s, hdr) >= 0)  return SOS_OK; // We have already received and forwarded this packet before
	  insert_flood_header(s, hdr);
	  /*
          dupbuff = ker_malloc(msg->len, MOD_FLOODING_PID);
          if (dupbuff == NULL) return SOS_OK; // Out of memory
	  //memcpy(dupbuff, msg->data, msg->len);
	  for (i = 0; i < msg->len; i++) {
		dupbuff[i] = msg->data[i];
	  }
	  post_long(hdr->dst_pid, MOD_FLOODING_PID, hdr->dest_msg_type, msg->len, dupbuff, SOS_MSG_RELEASE);
          */
            /* change by wmchen */
          dupbuff = ker_malloc(msg->len+2, MOD_FLOODING_PID);
          if (dupbuff == NULL) return SOS_OK; // Out of memory
	  //memcpy(dupbuff, msg->data, msg->len);
	  for (i = 0; i < msg->len; i++) {
		dupbuff[i] = msg->data[i];
	  }
          pp=(uint16_t*)(&dupbuff[msg->len]);
          *pp=msg->saddr;  //the last 2 bytes is msg's saddr;
	  post_long(hdr->dst_pid, MOD_FLOODING_PID, hdr->dest_msg_type, msg->len+1, dupbuff, SOS_MSG_RELEASE);
          /* end change */

	  hdr->originhopcount++;
	  if (hdr->originhopcount == hdr->maxhopcount) return SOS_OK; // Max hop count reached
	
          /* add by wmchen*/
          temp = ker_malloc(msg->len-1, MOD_FLOODING_PID);
          if (temp == NULL) return SOS_OK; // Out of memory
	  //memcpy(dupbuff, msg->data, msg->len);
	  for (i = 0; i < msg->len-1; i++) {
		dupbuff[i] = msg->data[i];
	  }
          ker_free(msg->data);
          msg->data=temp;
          msg->len=msg->len-1;
          /* end of adding */

          l = msg->len;
	  hdr = (flood_hdr_t*)ker_msg_take_data(MOD_FLOODING_PID, msg);
	  if(hdr == NULL) return -ENOMEM;

	  post_net(MOD_FLOODING_PID, MOD_FLOODING_PID,
			  MSG_FLOOD_DATA_PKT, l, hdr,
			  SOS_MSG_RELEASE, BCAST_ADDRESS);
	  return SOS_OK;
	}
	
  case MSG_FINAL:
	{
	  return SOS_OK;
	}
	
  default: return -EINVAL;
  }
  return SOS_OK;
}


//--------------------------------------------------------------
// Match flooding header to suppress duplicate transmissions
static int8_t match_flood_header(flooding_state_t *s, flood_hdr_t *hdr)
{
  int i;
  for (i = 0; i < FLOOD_QUEUE_SIZE; i++){
	if (((hdr->originaddr) == (s->flood_queue[i].originaddr))&&
		((hdr->seqno) == (s->flood_queue[i].seqno))&&
		((hdr->dst_pid) == (s->flood_queue[i].dst_pid))&&
		((hdr->dest_msg_type) == (s->flood_queue[i].dest_msg_type)))
	  return i;
  }
  return -ESRCH;
}

//--------------------------------------------------------------
// Insert header at the head into the flooding queue
static int8_t insert_flood_header(flooding_state_t *s, flood_hdr_t *hdr)
{
  // This function does not check for duplicate insertion
  s->flood_queue[s->flood_queue_tail].originaddr = hdr->originaddr;
  s->flood_queue[s->flood_queue_tail].seqno = hdr->seqno;
  s->flood_queue[s->flood_queue_tail].dst_pid = hdr->dst_pid;
  s->flood_queue[s->flood_queue_tail].dest_msg_type = hdr->dest_msg_type;
  s->flood_queue_tail++;
  if (s->flood_queue_tail == FLOOD_QUEUE_SIZE)
	s->flood_queue_tail = 0;
  return SOS_OK;
}

//--------------------------------------------------------------
// Dynamic Function
static uint8_t flood_get_hdr_size(func_cb_ptr proto)
{
  return sizeof(flood_hdr_t);
}

//--------------------------------------------------------------
// Route a packet
/*static int8_t flood_pkt(func_cb_ptr proto, sos_pid_t dest_pid, uint8_t dest_msg_type, uint8_t* pktpayload, uint8_t payloadlen)*/
/* change above by wmchen */
int8_t flood_pkt(sos_pid_t dest_pid, uint8_t dest_msg_type, uint8_t* pktpayload, uint8_t payloadlen,uint16_t daddr)
/*end of change*/
{
  flooding_state_t* s;
  flood_hdr_t *hdr;
  sos_module_t *m = ker_get_module(MOD_FLOODING_PID);
  s = (flooding_state_t*)m->handler_state;

  hdr = (flood_hdr_t*)(pktpayload);
  hdr->originaddr = ker_id();
  hdr->seqno = s->seqno;
  hdr->originhopcount = 0;
  hdr->maxhopcount = FLOOD_MAX_HOP_COUNT;
  hdr->dst_pid = dest_pid;
  hdr->dest_msg_type = dest_msg_type;
  s->seqno++;
  insert_flood_header(s, hdr);
  ker_change_own(pktpayload, MOD_FLOODING_PID);
  /*
  return post_net(MOD_FLOODING_PID, MOD_FLOODING_PID,
		  MSG_FLOOD_DATA_PKT, payloadlen, pktpayload,
		  SOS_MSG_RELEASE, BCAST_ADDRESS);
  */
  //change by wmchen
  if (daddr==BCAST_ADDRESS)
    return post_net(MOD_FLOODING_PID, MOD_FLOODING_PID,
		  MSG_FLOOD_DATA_PKT, payloadlen, pktpayload,
		  SOS_MSG_RELEASE, BCAST_ADDRESS);
  else
    return post_net(MOD_FLOODING_PID, MOD_FLOODING_PID,
		  MSG_FLOOD_DATA_PKT, payloadlen, pktpayload,
		  SOS_MSG_RELEASE, daddr);


  //end of change
}

#ifndef _MODULE_
mod_header_ptr flooding_get_header()
{
	return sos_get_header_address(mod_header);
}
#endif