linkestimatorp.nc

来自「tinyos-2.0源代码！转载而已！要的尽管拿！」· NC 代码 · 共 740 行 · 第 1/2 页

/* $Id: LinkEstimatorP.nc,v 1.1.2.21 2006/06/21 00:18:14 gnawali Exp $ */
/*
 * "Copyright (c) 2006 University of Southern California.
 * All rights reserved.
 *
 * Permission to use, copy, modify, and distribute this software and its
 * documentation for any purpose, without fee, and without written
 * agreement is hereby granted, provided that the above copyright
 * notice, the following two paragraphs and the author appear in all
 * copies of this software.
 *
 * IN NO EVENT SHALL THE UNIVERSITY OF SOUTHERN CALIFORNIA BE LIABLE TO
 * ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL
 * DAMAGES ARISING OUT OF THE USE OF THIS SOFTWARE AND ITS
 * DOCUMENTATION, EVEN IF THE UNIVERSITY OF SOUTHERN CALIFORNIA HAS BEEN
 * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 * THE UNIVERSITY OF SOUTHERN CALIFORNIA SPECIFICALLY DISCLAIMS ANY
 * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.  THE SOFTWARE
 * PROVIDED HEREUNDER IS ON AN "AS IS" BASIS, AND THE UNIVERSITY OF
 * SOUTHERN CALIFORNIA HAS NO OBLIGATION TO PROVIDE MAINTENANCE,
 * SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS."
 *
 */

/*
 @ author Omprakash Gnawali
 @ Created: April 24, 2006
 */

#include <Timer.h>
#include "LinkEstimator.h"

module LinkEstimatorP {
  provides {
    interface StdControl;
    interface AMSend as Send;
    interface Receive;
    interface LinkEstimator;
    interface Init;
    interface Packet;
    interface LinkSrcPacket;
  }

  uses {
    interface AMSend;
    interface AMPacket as SubAMPacket;
    interface Packet as SubPacket;
    interface Receive as SubReceive;
    interface AMSend as AMSendLinkEst;
    interface Receive as ReceiveLinkEst;
    interface Timer<TMilli>;
  }
}

implementation {

  // configure the link estimator and some constants
  enum {
    // If inbound link quality is above this threshold
    // do not evict a link
    EVICT_QUALITY_THRESHOLD = 0x50,
    // maximum link update rounds before we expire the link
    MAX_AGE = 6,
    // if received sequence number if larger than the last sequence
    // number by this gap, we reinitialize the link
    MAX_PKT_GAP = 10,
    MAX_QUALITY = 0xff,
    INVALID_RVAL = 0xff,
    INVALID_NEIGHBOR_ADDR = 0xff,
    INFINITY = 0xff,
    // update the link estimate this often
    TABLEUPDATE_INTERVAL = 4,
    // send a beacon this often unless user of
    // this component is sending a beacon atleast
    // at this rate
    BEACON_INTERVAL = 2,
    // decay the link estimate using this alpha
    // we use a denominator of 10, so this corresponds to 0.2
    ALPHA = 2 
  };

  // keep information about links from the neighbors
  neighbor_table_entry_t NeighborTable[NEIGHBOR_TABLE_SIZE];
  // link estiamtion sequence, increment every time a beacon is sent
  uint8_t linkEstSeq = 0;
  // use this message buffer
  // when this component needs to generate a message because
  // the user of this component is not sending packets frequently enough
  message_t linkEstPkt;
  // flag that prevents from sending linkest beacon before sendDone
  // for previous send is flagged.
  bool beaconBusy = FALSE;
  // we update the quality estimate when curEstInterval == TABLEUPDATE_INTERVAL
  uint8_t curEstInterval = 0;
  // we send out beacon if curBeaconInterval == BEACON_INTERVAL
  uint8_t curBeaconInterval = 0;
  // if there is not enough room in the packet to put all the neighbor table
  // entries, in order to do round robin we need to remember which entry
  // we sent in the last beacon
  uint8_t prevSentIdx = 0;

  // get the link estimation header in the packet
  linkest_header_t* getHeader(message_t* m) {
    return (linkest_header_t*)call SubPacket.getPayload(m, NULL);
  }

  // get the link estimation footer (neighbor entries) in the packet
  linkest_footer_t* getFooter(message_t* m, uint8_t len) {
    return (linkest_footer_t*)(len + (uint8_t *)call Packet.getPayload(m,NULL));
  }

  // add the link estimation header (seq no) and link estimation
  // footer (neighbor entries) in the packet. Call just before sending
  // the packet.
  uint8_t addLinkEstHeaderAndFooter(message_t *msg, uint8_t len) {
    uint8_t newlen;
    linkest_header_t *hdr;
    linkest_footer_t *footer;
    uint8_t i, j, k;
    uint8_t maxEntries, newPrevSentIdx;
    dbg("LI", "newlen1 = %d\n", len);
    hdr = getHeader(msg);
    footer = getFooter(msg, len);

    maxEntries = ((call SubPacket.maxPayloadLength() - len - sizeof(linkest_header_t))
		  / sizeof(linkest_footer_t));

    // Depending on the number of bits used to store the number
    // of entries, we can encode up to NUM_ENTRIES_FLAG using those bits
    if (maxEntries > NUM_ENTRIES_FLAG) {
      maxEntries = NUM_ENTRIES_FLAG;
    }
    dbg("LI", "Max payload is: %d, maxEntries is: %d\n", call SubPacket.maxPayloadLength(), maxEntries);

    j = 0;
    newPrevSentIdx = 0;
    for (i = 0; i < NEIGHBOR_TABLE_SIZE && j < maxEntries; i++) {
      k = (prevSentIdx + i + 1) % NEIGHBOR_TABLE_SIZE;
      if (NeighborTable[k].flags & VALID_ENTRY) {
	footer->neighborList[j].ll_addr = NeighborTable[k].ll_addr;
	footer->neighborList[j].inquality = NeighborTable[k].inquality;
	newPrevSentIdx = k;
	dbg("LI", "Loaded on footer: %d %d %d\n", j, footer->neighborList[j].ll_addr,
	    footer->neighborList[j].inquality);
	j++;
      }
    }
    prevSentIdx = newPrevSentIdx;

    hdr->ll_addr = call SubAMPacket.address();
    hdr->seq = linkEstSeq++;
    hdr->flags = 0;
    hdr->flags |= (NUM_ENTRIES_FLAG & j);
    newlen = sizeof(linkest_header_t) + len + j*sizeof(linkest_footer_t);
    dbg("LI", "newlen2 = %d\n", newlen);
    return newlen;
  }


  // given in and out quality, return the bi-directional link quality
  // q = q1 * q2 / 256
  uint8_t computeBidirLinkQuality(uint8_t inQuality, uint8_t outQuality) {
    return ((inQuality * outQuality) >> 8);
  }


  // initialize the given entry in the table for neighbor ll_addr
  void initNeighborIdx(uint8_t i, am_addr_t ll_addr) {
    neighbor_table_entry_t *ne;
    ne = &NeighborTable[i];
    ne->ll_addr = ll_addr;
    ne->lastseq = 0;
    ne->rcvcnt = 0;
    ne->failcnt = 0;
    ne->flags = (INIT_ENTRY | VALID_ENTRY);
    ne->inage = MAX_AGE;
    ne->outage = MAX_AGE;
    ne->inquality = 0;
    ne->outquality = 0;
  }

  // find the index to the entry for neighbor ll_addr
  uint8_t findIdx(am_addr_t ll_addr) {
    uint8_t i;
    for (i = 0; i < NEIGHBOR_TABLE_SIZE; i++) {
      if (NeighborTable[i].flags & VALID_ENTRY) {
	if (NeighborTable[i].ll_addr == ll_addr) {
	  return i;
	}
      }
    }
    return INVALID_RVAL;
  }

  // find an empty slot in the neighbor table
  uint8_t findEmptyNeighborIdx() {
    uint8_t i;
    for (i = 0; i < NEIGHBOR_TABLE_SIZE; i++) {
      if (NeighborTable[i].flags & VALID_ENTRY) {
      } else {
	return i;
      }
    }
      return INVALID_RVAL;
  }

  // find the index to the worst neighbor if inbound link
  // quality to is less than the given threshold
  uint8_t findWorstNeighborIdx(uint8_t filterThreshold) {
    uint8_t i, worstNeighborIdx, worstQuality, thisQuality;

    worstNeighborIdx = INVALID_RVAL;
    worstQuality = MAX_QUALITY;
    for (i = 0; i < NEIGHBOR_TABLE_SIZE; i++) {
      if (!(NeighborTable[i].flags & VALID_ENTRY)) {
	dbg("LI", "Invalid so continuing\n");
	continue;
      }
      if (!(NeighborTable[i].flags & MATURE_ENTRY)) {
	dbg("LI", "Not mature, so continuing\n");
	continue;
      }
      if (NeighborTable[i].flags & PINNED_ENTRY) {
	dbg("LI", "Pinned entry, so continuing\n");
	continue;
      }
      thisQuality = NeighborTable[i].inquality;
      if (thisQuality < worstQuality) {
	worstNeighborIdx = i;
	worstQuality = thisQuality;
      }
    }
    if (worstQuality <= filterThreshold) {
      return worstNeighborIdx;
    } else {
      return INVALID_RVAL;
    }
  }

  // update the quality of the link link: self->neighbor
  // this is found in the entries in the footer of incoming message
  void updateReverseQuality(am_addr_t neighbor, uint8_t outquality) {
    uint8_t idx;
    idx = findIdx(neighbor);
    if (idx != INVALID_RVAL) {
      NeighborTable[idx].outquality = outquality;
      NeighborTable[idx].outage = MAX_AGE;
    }
  }

  // we received seq from the neighbor in idx
  // update the last seen seq, receive and fail count
  // refresh the age
  void updateNeighborEntryIdx(uint8_t idx, uint8_t seq) {
    uint8_t packetGap;

    if (NeighborTable[idx].flags & INIT_ENTRY) {
      dbg("LI", "Init entry update\n");
      NeighborTable[idx].lastseq = seq;
      NeighborTable[idx].flags &= ~INIT_ENTRY;
    }
    
    packetGap = seq - NeighborTable[idx].lastseq;
    dbg("LI", "updateNeighborEntryIdx: prevseq %d, curseq %d, gap %d\n",
	NeighborTable[idx].lastseq, seq, packetGap);
    NeighborTable[idx].lastseq = seq;
    NeighborTable[idx].rcvcnt++;
    NeighborTable[idx].inage = MAX_AGE;
    if (packetGap > 0) {
      NeighborTable[idx].failcnt += packetGap - 1;
    }
    if (packetGap > MAX_PKT_GAP) {
      NeighborTable[idx].failcnt = 0;
      NeighborTable[idx].rcvcnt = 1;
      NeighborTable[idx].outage = 0;
      NeighborTable[idx].outquality = 0;
      NeighborTable[idx].inquality = 0;
    }
  }


  // update the inbound link quality by
  // munging receive, fail count since last update
  void updateNeighborTableEst() {
    uint8_t i, totalPkt;
    neighbor_table_entry_t *ne;
    uint8_t newEst;
    uint8_t minPkt;

    minPkt = TABLEUPDATE_INTERVAL / BEACON_INTERVAL;
    dbg("LI", "%s\n", __FUNCTION__);
    for (i = 0; i < NEIGHBOR_TABLE_SIZE; i++) {
      ne = &NeighborTable[i];
      if (ne->flags & VALID_ENTRY) {
	if (ne->inage > 0)
	  ne->inage--;
	if (ne->outage > 0)
	  ne->outage--;

	if ((ne->inage == 0) && (ne->outage == 0)) {
	  ne->flags ^= VALID_ENTRY;
	} else {
	  dbg("LI", "Making link: %d mature\n", i);
	  ne->flags |= MATURE_ENTRY;
	  totalPkt = ne->rcvcnt + ne->failcnt;
	  dbg("LI", "MinPkt: %d, totalPkt: %d\n", minPkt, totalPkt);
	  if (totalPkt < minPkt) {
	    totalPkt = minPkt;
	  }
	  if (totalPkt == 0) {
	    ne->inquality = (ALPHA * ne->inquality) / 10;
	  } else {
	    newEst = (255 * ne->rcvcnt) / totalPkt;
	    dbg("LI,LITest", "  %hu: %hhu -> %hhu", ne->ll_addr, ne->inquality, (ALPHA * ne->inquality + (10-ALPHA) * newEst)/10);
	    ne->inquality = (ALPHA * ne->inquality + (10-ALPHA) * newEst)/10;
	  }
	  ne->rcvcnt = 0;
	  ne->failcnt = 0;
	}
      }
      else {
	dbg("LI", " - entry %i is invalid.\n", (int)i);
      }
    }
  }

  // print the neighbor table. for debugging.
  void print_neighbor_table() {
    uint8_t i;
    neighbor_table_entry_t *ne;
    for (i = 0; i < NEIGHBOR_TABLE_SIZE; i++) {
      ne = &NeighborTable[i];
      if (ne->flags & VALID_ENTRY) {
	dbg("LI,LITest", "%d:%d inQ=%d, inA=%d, outQ=%d, outA=%d, rcv=%d, fail=%d, biQ=%d\n",
	    i, ne->ll_addr, ne->inquality, ne->inage, ne->outquality, ne->outage,
	    ne->rcvcnt, ne->failcnt, computeBidirLinkQuality(ne->inquality, ne->outquality));
      }
    }
  }

  // print the packet. for debugging.
  void print_packet(message_t* msg, uint8_t len) {
    uint8_t i;
    uint8_t* b;

    b = (uint8_t *)msg->data;
    for(i=0; i<len; i++)
      dbg_clear("LI", "%x ", b[i]);
    dbg_clear("LI", "\n");
  }

  // initialize the neighbor table in the very beginning
  void initNeighborTable() {
    uint8_t i;

    for (i = 0; i < NEIGHBOR_TABLE_SIZE; i++) {
      NeighborTable[i].flags = 0;
    }
  }

  command error_t StdControl.start() {
    dbg("LI", "Link estimator start\n");
    call Timer.startPeriodic(LINKEST_TIMER_RATE);
    return SUCCESS;
  }

  // when stop is called, the timer is stopped
  // this stops aging as well as outgoing beacons