Fix queueing and sequence numbers.

diff --git a/tos/lib/net/lqi/LqiForwardingEngineP.nc b/tos/lib/net/lqi/LqiForwardingEngineP.nc
index e3dc5d003f27145af7478874dfd85fe09309eab5..17a15be40b9060b24d052a73b96d488e715ffbc7 100644 (file)
--- a/tos/lib/net/lqi/LqiForwardingEngineP.nc
+++ b/tos/lib/net/lqi/LqiForwardingEngineP.nc
@@ -94,12 +94,14 @@ module LqiForwardingEngineP {
   uses {
     interface Receive as SubReceive;
     interface AMSend as SubSend;
+    interface AMSend as SubSendMine;
     interface RouteControl as RouteSelectCntl;
     interface RouteSelect;
     interface Leds;
     interface Packet as SubPacket;
     interface AMPacket;
     interface RootControl;
+    interface Random;
     interface PacketAcknowledgements;
   }
 }
@@ -118,7 +120,8 @@ implementation {
   uint8_t iFwdBufHead, iFwdBufTail;
   uint16_t sendFailures = 0;
   uint8_t fail_count = 0;
-
+  int fwdbusy = 0;
+  
   lqi_header_t* getHeader(message_t* msg) {
     return (lqi_header_t*) call SubPacket.getPayload(msg, NULL);
   }
@@ -166,7 +169,7 @@ implementation {
       return FAIL;
     }
     call PacketAcknowledgements.requestAck(pMsg);
-    if (call SubSend.send(call AMPacket.destination(pMsg), pMsg, len) != SUCCESS) {
+    if (call SubSendMine.send(call AMPacket.destination(pMsg), pMsg, len) != SUCCESS) {
       sendFailures++;
       return FAIL;
     }
@@ -199,36 +202,53 @@ implementation {
     } 
     return -1;
   }
+
+  static char* fields(message_t* msg) {
+    static char mbuf[1024];
+    lqi_header_t* hdr = getHeader(msg);
+    sprintf(mbuf, "origin = %hu, seqno = %hu, oseqno = %hu, hopcount =%hu", hdr->originaddr, hdr->seqno, hdr->originseqno, hdr->hopcount);
+    return mbuf;
+  }
+
+  static void forward(message_t* msg);
   
   static message_t* mForward(message_t* msg) {
-    message_t* newMsg = msg;
     int8_t buf = get_buff();
     call Leds.led2Toggle();
-    
+    dbg("LQI", " Asked to forward packet @%s:\t%s\n", sim_time_string(), fields(msg));
     if (buf == -1) {
-      dbg("LQI", "Dropped packet due to no space in queue.\n");
+      dbg("LQI", "%s Dropped packet due to no space in queue.\n", __FUNCTION__);
       return msg;
     }
-    
     if ((call RouteSelect.selectRoute(msg, 0)) != SUCCESS) {
       FwdBufBusy[(uint8_t)buf] = 0;
+      dbg("LQI", "%s Dropped packet due to no route.\n", __FUNCTION__);
       return msg;
     }
- 
+    else {
+      message_t* newMsg = FwdBufList[(uint8_t)buf];
+      FwdBufList[(uint8_t)buf] = msg;
+      forward(msg);
+      return newMsg;
+    }
+  }
+  
+  static void forward(message_t* msg) {
     // Failures at the send level do not cause the seq. number space to be 
     // rolled back properly.  This is somewhat broken.
-    call PacketAcknowledgements.requestAck(msg);
-    if (call SubSend.send(call AMPacket.destination(msg),
-                         msg,
-                         call SubPacket.payloadLength(msg) == SUCCESS)) {
-      newMsg = FwdBufList[(uint8_t)buf];
-      FwdBufList[(uint8_t)buf] = msg;
+    if (fwdbusy) {
+      dbg("LQI", "%s forwarding busy, wait for later.\n", __FUNCTION__);
+      return;
     }
-    else{
-      FwdBufBusy[(uint8_t)buf] = 0;
-      sendFailures++;
+    else {
+      call PacketAcknowledgements.requestAck(msg);
+      if (call SubSend.send(call AMPacket.destination(msg),
+                           msg,
+                           call SubPacket.payloadLength(msg)) == SUCCESS) {
+       dbg("LQI", "%s: Send to %hu success.\n", __FUNCTION__, call AMPacket.destination(msg));
+      }
+      fwdbusy = TRUE;
     }
-    return newMsg;    
   }
 
   event message_t* SubReceive.receive(message_t* msg, void* payload, uint8_t len) {
@@ -237,9 +257,11 @@ implementation {
     len -= sizeof(lqi_header_t);
 
     if (call RootControl.isRoot()) {
+      dbg("LQI", "LQI Root is receiving packet from node %hu @%s\n", getHeader(msg)->originaddr, sim_time_string());
       return signal Receive.receive[id](msg, payload, len);
     }
     else if (signal Intercept.forward[id](msg, payload, len)) {
+      dbg("LQI", "LQI fwd is forwarding packet from node %hu @%s\n", getHeader(msg)->originaddr, sim_time_string());
       return mForward(msg);
     }
     else {
@@ -247,31 +269,78 @@ implementation {
     }
   }
   
-
+  message_t* nextMsg() {
+    int i;
+    int inc = call Random.rand16();
+    for (i = 0; i < FWD_QUEUE_SIZE; i++) {
+      int pindex = (i + inc) % FWD_QUEUE_SIZE;
+      if (FwdBufBusy[pindex]) {
+       return FwdBufList[pindex];
+      }
+    }
+    return NULL;
+  }
+  
   event void SubSend.sendDone(message_t* msg, error_t success) {
     int8_t buf;
+    message_t* nextToSend;
     if (!call PacketAcknowledgements.wasAcked(msg) &&
        call AMPacket.destination(msg) != TOS_BCAST_ADDR &&
        fail_count < 5){
       call RouteSelect.selectRoute(msg, 1);
+      call PacketAcknowledgements.requestAck(msg);
       if (call SubSend.send(call AMPacket.destination(msg),
                            msg,
                            call SubPacket.payloadLength(msg)) == SUCCESS) {
+       dbg("LQI", "Packet not acked, retransmit:\t%s\n", fields(msg));
        fail_count ++;
       } else {
+       dbg("LQI", "Packet not acked, retransmit fail:\t%s\n", fields(msg));
        sendFailures++;
       }
     }
+    else if (fail_count >= 5) {
+      dbg("LQI", "Packet failed:\t%s\n", fields(msg));
+    }
+    else if (call PacketAcknowledgements.wasAcked(msg)) {
+      dbg("LQI", "Packet acked:\t%s\n", fields(msg));
+    }
     
     fail_count = 0;
-
     buf = is_ours(msg);
-
-    if (buf != -1) { // Msg was from forwarding queue
+    if (buf != -1) {
       FwdBufBusy[(uint8_t)buf] = 0;
-    } else {
-      signal Send.sendDone(msg, success);
-    } 
+    }
+    
+    nextToSend = nextMsg();
+    fwdbusy = FALSE;
+         
+    if (nextToSend != NULL) {
+      forward(nextToSend);
+    }
+    
+    dbg("LQI", "Packet not longer busy:\t%s\n", fields(msg));
+  }
+
+  event void SubSendMine.sendDone(message_t* msg, error_t success) {
+    if (!call PacketAcknowledgements.wasAcked(msg) &&
+       call AMPacket.destination(msg) != TOS_BCAST_ADDR &&
+       fail_count < 5){
+      call RouteSelect.selectRoute(msg, 1);
+      call PacketAcknowledgements.requestAck(msg);
+      if (call SubSendMine.send(call AMPacket.destination(msg),
+                           msg,
+                           call SubPacket.payloadLength(msg)) == SUCCESS) {
+       dbg("LQI", "Packet not acked, retransmit:\t%s\n", fields(msg));
+       fail_count ++;
+      } else {
+       dbg("LQI", "Packet not acked, retransmit fail:\t%s\n", fields(msg));
+       sendFailures++;
+      }
+    }
+    
+    fail_count = 0;
+    signal Send.sendDone(msg, success);
   }
 
 
@@ -350,7 +419,9 @@ implementation {
   }
   command void* Packet.getPayload(message_t* msg, uint8_t* len) {
     void* rval = call SubPacket.getPayload(msg, len);
-    *len -= sizeof(lqi_header_t);
+    if (len != NULL) {
+      *len -= sizeof(lqi_header_t);
+    }
     rval += sizeof(lqi_header_t);
     return rval;
   }

diff --git a/tos/lib/net/lqi/LqiRoutingEngineP.nc b/tos/lib/net/lqi/LqiRoutingEngineP.nc
index 8a099e88ac72e49d813092799edfd9c574ad97b3..c19bed0e29a2b6197171a49da50d45526a15636d 100644 (file)
--- a/tos/lib/net/lqi/LqiRoutingEngineP.nc
+++ b/tos/lib/net/lqi/LqiRoutingEngineP.nc
@@ -119,7 +119,7 @@ implementation {
     dbg("LQI","MultiHopRSSI Sending route update msg.\n");
 
     if (gbCurrentParent != TOS_BCAST_ADDR) {
-      dbg("LQO","MultiHopRSSI: Parent = %d\n", gbCurrentParent);
+      dbg("LQI","MultiHopRSSI: Parent = %d\n", gbCurrentParent);
     }
     
     if (msgBufBusy) {
@@ -229,12 +229,13 @@ implementation {
     if (isRoot) {
       return FAIL;
     }
-    
+
     if (hdr->originaddr != TOS_NODE_ID && resend == 0) {
       // supress duplicate packets
       for (i = 0; i < MHOP_HISTORY_SIZE; i++) {
         if ((gRecentPacketSender[i] == call AMPacket.source(msg)) &&
             (gRecentPacketSeqNo[i] == hdr->seqno)) {
+         dbg("LQI", "%s no route as this is a duplicate!\n", __FUNCTION__);
           return FAIL;
         }
       }
@@ -251,6 +252,7 @@ implementation {
           gbCurrentLinkEst = 0x7fff;
           gbCurrentParent = TOS_BCAST_ADDR;
           gbCurrentHopCount = ROUTE_INVALID;
+         dbg("LQI", "%s no route as we are in a cycle!\n", __FUNCTION__);
           return FAIL;
         }
       }
@@ -258,11 +260,12 @@ implementation {
       gRecentOriginPacketSeqNo[gRecentOriginIndex] = hdr->originseqno;
       gRecentOriginIndex = (gRecentOriginIndex + 1) % MHOP_HISTORY_SIZE;
     }
-    
+
     if (resend == 0) {
       hdr->seqno = gCurrentSeqNo++;
     }
     
+    dbg("LQI", "LQI setting destination to %hu and link quality ?\n", gbCurrentParent);
     call AMPacket.setDestination(msg, gbCurrentParent);
 
     return SUCCESS;
@@ -273,7 +276,8 @@ implementation {
 
     header->originaddr = TOS_NODE_ID;
     header->originseqno = gCurrentSeqNo;
-
+    header->seqno = gCurrentSeqNo;
+    
     if (isRoot) {
       header->hopcount = 0;
     }
@@ -281,6 +285,7 @@ implementation {
       header->hopcount = gbCurrentHopCount;
     }
 
+    dbg("LQI", "LQI setting hopcount to %hhu\n", gbCurrentHopCount);
     return SUCCESS;
   }
 
@@ -330,7 +335,7 @@ implementation {
       lqi_beacon_msg_t* bMsg = (lqi_beacon_msg_t*)payload;
       am_addr_t source = call AMPacket.source(msg);
       uint8_t lqi = call CC2420Packet.getLqi(msg);
-      
+      dbg("LQI,LQIRoute", "LQI receiving routing beacon from %hu with LQI %hhu that advertises %hu.\n", source, lqi, bMsg->cost);
       if (source == gbCurrentParent) {
        // try to prevent cycles
        if (bMsg->parent != TOS_NODE_ID) {
@@ -338,6 +343,7 @@ implementation {
          gbCurrentParentCost = bMsg->cost;
          gbCurrentLinkEst = adjustLQI(lqi);
          gbCurrentHopCount = bMsg->hopcount + 1;
+         dbg("LQI,LQIRoute", "  -- Not a loop\n");
        }
        else {
          gLastHeard = 0;
@@ -345,6 +351,7 @@ implementation {
          gbCurrentLinkEst = 0x7fff;
          gbCurrentParent = TOS_BCAST_ADDR;
          gbCurrentHopCount = ROUTE_INVALID;
+         dbg("LQI,LQIRoute", "  -- Detected a loop\n");
        }
        
       } else {
@@ -365,9 +372,14 @@ implementation {
          gbCurrentParentCost = bMsg->cost;
          gbCurrentLinkEst = adjustLQI(lqi);    
          gbCurrentHopCount = bMsg->hopcount + 1;
+         dbg("LQI,LQIRoute", "  -- Not a cycle.\n");
+       }
+       else {
+         dbg("LQI,LQIRoute", "  -- CYCLE.\n");
        }
       }
     }
+    dbg("LQI,LQIRoute", "Set my count to %hhu, my link to %hu and my cost to %hu.\n", gbCurrentHopCount, gbCurrentLinkEst, gbCurrentParentCost);
 
     return msg;
   }
@@ -377,4 +389,4 @@ implementation {
   }
   
 }
-
+  

diff --git a/tos/lib/net/lqi/MultiHopLqiP.nc b/tos/lib/net/lqi/MultiHopLqiP.nc
index 4b84194a23b265a910460ea66471e7fc0060fa8c..1f58f4b555db1b6b2b38c8d6ce1bbe90fb14dc87 100644 (file)
--- a/tos/lib/net/lqi/MultiHopLqiP.nc
+++ b/tos/lib/net/lqi/MultiHopLqiP.nc
@@ -87,6 +87,7 @@ implementation {
     new AMSenderC(AM_LQI_BEACON_MSG) as BeaconSender,
     new AMReceiverC(AM_LQI_BEACON_MSG) as BeaconReceiver,
     new AMSenderC(AM_LQI_DATA_MSG) as DataSender,
+    new AMSenderC(AM_LQI_DATA_MSG) as DataSenderMine,
     new AMReceiverC(AM_LQI_DATA_MSG) as DataReceiver,
     new TimerMilliC(), 
     NoLedsC, LedsC,
@@ -114,12 +115,14 @@ implementation {
   Forwarder.RouteSelectCntl -> Router.RouteControl;
   Forwarder.RouteSelect -> Router;
   Forwarder.SubSend -> DataSender;
+  Forwarder.SubSendMine -> DataSenderMine;
   Forwarder.SubReceive -> DataReceiver;
   Forwarder.Leds -> LedsC;
   Forwarder.AMPacket -> ActiveMessageC;
   Forwarder.SubPacket -> ActiveMessageC;
   Forwarder.PacketAcknowledgements -> ActiveMessageC;
   Forwarder.RootControl -> Router;
+  Forwarder.Random -> RandomC;
   
   Router.AMSend -> BeaconSender;
   Router.Receive -> BeaconReceiver;