multihoproutem.nc

传感器网络中的嵌入式操作系统源代码

    // First pass: min hopcount
    for (n = 0; n < NUM_NEIGHBORS; n++) {
      if ((neighbors[n].addr != EMPTY) &&
	  (neighbors[n].goodness != UNINIT_GOODNESS) &&          
	  (neighbors[n].goodness > GOODNESS_THRESHOLD)) {
	if (neighbors[n].hopcount < best_hopcount) {
	  best_hopcount = neighbors[n].hopcount;
	}
      }
    }

    // Second pass: max goodness at min good hopcount
    for (n = 0; n < NUM_NEIGHBORS; n++) {
      if ((neighbors[n].addr != EMPTY) &&
	  (neighbors[n].hopcount == best_hopcount) &&
	  (neighbors[n].goodness != UNINIT_GOODNESS) &&          
	  (neighbors[n].goodness > GOODNESS_THRESHOLD)) {
	if (neighbors[n].goodness > best_goodness) {
	  best_goodness = neighbors[n].goodness;
	  best = n;
	}
      }
    }
    if ((best != 255) &&
	((pParent == NULL) || 
	 ((my_hopcount > (best_hopcount + 1)) &&
	  (pParent->goodness < best_goodness)))) {    // Found new parent
      pParent = &neighbors[best];
      my_hopcount = neighbors[best].hopcount + 1;
      parent_count = 0;
      update_count++;
    } else { // no good candidate
      dbg(DBG_ROUTE, "MHop: No new parent. BstGdns=%d\n",best_goodness);
      //pParent = NULL;
      //my_hopcount = INIT_HOPCOUNT;
    }
  }

  /*
   * Update Routing State
   *
   */

  static void break_cycle() {
    dbg(DBG_USR1, "MHop: Breaking cycle\n");

    // Drop our parent
    pParent = NULL;
    my_hopcount = INIT_HOPCOUNT;
    post route_update();
  }

  task void update_routing() {
    dbg(DBG_ROUTE,"Updating routing\n");
    update_neighbors();

    if ((pParent == NULL) ||
	(pParent->goodness < PARENT_THRESHOLD) || 
	(parent_count > MAX_PARENT_COUNT))
      post route_update();
  }


  /***********************************************************************
   * Commands and events
   ***********************************************************************/

  event result_t Timer.fired() {
    // dbg(DBG_USR1, "MHop: -\n");
    timer_ticks++;

    if (timer_ticks % TIMER_PLQ_UPDATE_COUNT == 0) {
      dbg(DBG_ROUTE, "MHop: posting update routing\n");
      post update_routing();
    }
    return SUCCESS;
  }

  command result_t Send.send[uint8_t id](TOS_MsgPtr pMsg, uint16_t PayloadLen) {

    TOS_MHopMsg *pMHMsg = (TOS_MHopMsg *)pMsg->data;
    uint16_t usMHLength = offsetof(TOS_MHopMsg,data) + PayloadLen;

    if ((pParent == NULL) || (usMHLength > TOSH_DATA_LENGTH)) {
      return FAIL;
    }

    dbg(DBG_ROUTE,"MHop: send 0x%x\n",pParent->addr);
    pMHMsg->sourceaddr = pMHMsg->originaddr = TOS_LOCAL_ADDRESS;
    pMHMsg->hopcount = my_hopcount;
    pMHMsg->seqno = cur_seqno++;

    dbg(DBG_ROUTE,"MHop: out pkt 0x%x\n",pMHMsg->seqno);
    
    if (call SendMsg.send[id](pParent->addr, usMHLength, pMsg) != SUCCESS) {
      return FAIL;
    }
    return SUCCESS;

  } 

  command void *Send.getBuffer[uint8_t id](TOS_MsgPtr pMsg, uint16_t* length) {
    
    TOS_MHopMsg *pMHMsg = (TOS_MHopMsg *)pMsg->data;
    
    *length = TOSH_DATA_LENGTH - offsetof(TOS_MHopMsg,data);

    return (&pMHMsg->data[0]);

  }

#if 0  
  static void forward(uint16_t destaddr, TOS_MHopMsg *pRcvMsg, uint16_t Len, uint8_t id) {
    TOS_MHopMsg *pFwdMsg;

    if (((iFwdBufHead + 1) % FWD_QUEUE_SIZE) == iFwdBufTail) {
      // Drop message if forwarding queue is full.
      return;
    }
    
    pFwdMsg = (TOS_MHopMsg *) &FwdBuffer[iFwdBufHead].data;

    memcpy(pFwdMsg,pRcvMsg,sizeof(TOS_MHopMsg));
    pFwdMsg->sourceaddr = TOS_LOCAL_ADDRESS;
    pFwdMsg->hopcount = my_hopcount;
    pFwdMsg->seqno = cur_seqno++;

    dbg(DBG_USR1, "MHop: Forwarding packet (seqno 0x%x)\n", cur_seqno);
    if (call SendMsg.send[id](destaddr, Len,&FwdBuffer[iFwdBufHead]) == SUCCESS) {
      iFwdBufHead++; iFwdBufHead %= FWD_QUEUE_SIZE;
    }
  }
#endif

  static TOS_MsgPtr mForward(TOS_MsgPtr pMsg, uint8_t id, uint16_t DestAddr) {
    TOS_MsgPtr	pNewBuf = pMsg;
    TOS_MHopMsg *pMHMsg = (TOS_MHopMsg *)pMsg->data;

    if (((iFwdBufHead + 1) % FWD_QUEUE_SIZE) != iFwdBufTail) {
      pMHMsg->sourceaddr = TOS_LOCAL_ADDRESS;
      pMHMsg->hopcount = my_hopcount;
      pMHMsg->seqno = cur_seqno++;
      
      if (call SendMsg.send[id](DestAddr,pMsg->length,pMsg) == SUCCESS) {
	pNewBuf = FwdBufList[iFwdBufHead];
	FwdBufList[iFwdBufHead] = pMsg;
	iFwdBufHead++; iFwdBufHead %= FWD_QUEUE_SIZE;
      }
    }

    return pNewBuf;

  }

  event TOS_MsgPtr ReceiveMsg.receive[uint8_t id](TOS_MsgPtr pMsg) {
    TOS_MHopNeighbor	*pNeighbor;
    TOS_MHopMsg		*pMHMsg = (TOS_MHopMsg *)pMsg->data;
    uint16_t		PayloadLen = pMsg->length - offsetof(TOS_MHopMsg,data);
    bool		isDuplicate;

    call Leds.greenToggle();
    dbg(DBG_ROUTE, "MHop: Msg Rcvd, src 0x%02x, org 0x%02x, parent 0x%02x\n", 
	pMHMsg->sourceaddr, pMHMsg->originaddr, 0 /*pMHMsg->parentaddr*/);

    // Update link information
    pNeighbor = link_update(pMHMsg,&isDuplicate);

    // Check for cycles in the route tree
    if (pMHMsg->originaddr == pMsg->addr) {
      if (pMsg->addr == TOS_LOCAL_ADDRESS) 
	break_cycle();
    }
    // If this is a root beacon, may use it to override parent selection
    else if (id == AM_MULTIHOPMSG) {
      dbg(DBG_ROUTE, "MHop: Rootbeacon src=0x%x, hc=0x%x \n", pMHMsg->sourceaddr, my_hopcount);
      dbg(DBG_ROUTE, "MHop: RB goodness = %d\n",pNeighbor->goodness);
      
      if (pNeighbor->goodness > ROOT_BEACON_THRESHOLD)  {
	dbg(DBG_ROUTE, "MHop: Setting parent to %x\n", pMHMsg->sourceaddr);
	my_hopcount = pMHMsg->hopcount+1; // had better be 1

	if (pParent != pNeighbor) {
	  parent_count = 0;
	  pParent = pNeighbor;
	}
      }

      if (0 == pMHMsg->hopcount) {
	pMHMsg->hopcount++;
	//forward (TOS_BCAST_ADDR, pMHMsg, pMsg->length, id);
	mForward(pMsg,id,TOS_BCAST_ADDR);
      }
    } 
    // Ordinary message requiring forwarding
    else if ((pMsg->addr == TOS_LOCAL_ADDRESS) &&  // Addressed to local node
	     (!isDuplicate) && // Not a duplicate message from child
	     (my_hopcount < pMHMsg->hopcount)) { // Isn't going down the tree
      if ((signal Intercept.intercept[id](pMsg,&pMHMsg->data[0],PayloadLen)) == SUCCESS) {
	if (!pParent) { 
	  // If no parent, drop message and initiate a routing update
	  post route_update();
	}
	else {
	  // Otherwise forward it
	  parent_count++;
	  //forward(pParent->addr, pMHMsg, pMsg->length, id);
	  pMsg = mForward(pMsg,id,pParent->addr);
	}
      }
    } 
    else {
      // Snoop the packet for permiscuous applications
      signal Snoop.intercept[id](pMsg,&pMHMsg->data[0],PayloadLen);
    }

    return pMsg;
  }

  event result_t SendMsg.sendDone[uint8_t id](TOS_MsgPtr pMsg, bool success) {
    dbg(DBG_ROUTE, "MHop: senddone 0x%x 0x%x\n", pMsg, success);  
#ifdef PLATFORM_MICA
    if (pParent) {
      if (success == SUCCESS) {
	call Leds.redOff();
	pParent->recv_count++;
      }
      else {
	call Leds.redOn();
	pParent->fail_count++;
      }
    }
#endif
    if (pMsg == FwdBufList[iFwdBufTail]) { // Msg was from forwarding queue
      iFwdBufTail++; iFwdBufTail %= FWD_QUEUE_SIZE;
    } else {
      signal Send.sendDone[id](pMsg, success);
    } 
    return SUCCESS;
  }

  command uint16_t RouteControl.getParent() {
	if (pParent == NULL)
	  return 0xffff;
    return pParent->addr;
  }

  command uint8_t RouteControl.getQuality() {
	if (pParent == NULL)
	  return 0xff;
    return pParent->goodness;
  }

  command uint8_t RouteControl.getDepth() {
    return my_hopcount;
  }

  command uint8_t RouteControl.getOccupancy() {
	uint16_t uiOutstanding = (uint16_t)iFwdBufTail - (uint16_t)iFwdBufHead;
	uiOutstanding %= FWD_QUEUE_SIZE;
    return (uint8_t)uiOutstanding;
  }

  command uint16_t RouteControl.getSender(TOS_MsgPtr msg) {
    TOS_MHopMsg		*pMHMsg = (TOS_MHopMsg *)msg->data;
	return pMHMsg->sourceaddr;
  }

  comand result_t RouteControl.setUpdateInterval(uint16_t Interval) {
    result_t Result;

    call Timer.stop();
    timer_rate = (Interval * 1024);  // * 1024 to make the math simpler
    Result = call Timer.start(TIMER_REPEAT,gUpdateInterval);

    return Result;


  }

  comand result_t RouteControl.manualUpdate() {

  }


  default event result_t Send.sendDone[uint8_t id](TOS_MsgPtr pMsg, result_t success) {
    return SUCCESS;
  }

  default event result_t Intercept.intercept[uint8_t id](TOS_MsgPtr pMsg, void* payload, 
							 uint16_t payloadLen) {
    return SUCCESS;
  }

  default event result_t Snoop.intercept[uint8_t id](TOS_MsgPtr pMsg, void* payload, 
						       uint16_t payloadLen) {
    return SUCCESS;
  }

  default event TOS_MsgPtr Receive.receive[uint8_t id](TOS_MsgPtr pMsg, void* payload, 
						       uint16_t payloadLen) {
    return pMsg;
  }



}