From 31bef23ef3377514af595000d2fa2cce07bd4531 Mon Sep 17 00:00:00 2001
From: kusy <kusy>
Date: Fri, 6 Feb 2009 06:38:49 +0000
Subject: [PATCH] committing new timestamping code

---
 tos/chips/cc2420/receive/CC2420ReceiveP.nc   | 23 ++++++++----
 tos/chips/cc2420/transmit/CC2420TransmitP.nc | 39 +++++++++-----------
 2 files changed, 33 insertions(+), 29 deletions(-)

diff --git a/tos/chips/cc2420/receive/CC2420ReceiveP.nc b/tos/chips/cc2420/receive/CC2420ReceiveP.nc
index b50ad035..5bd4e852 100644
--- a/tos/chips/cc2420/receive/CC2420ReceiveP.nc
+++ b/tos/chips/cc2420/receive/CC2420ReceiveP.nc
@@ -285,16 +285,25 @@ implementation {
         call SpiResource.release();
       }
       
-      if ( m_timestamp_size ) {
-        if ( rxFrameLength > 10 ) {
-          call PacketTimeStamp.set(m_p_rx_buf, m_timestamp_queue[ m_timestamp_head ]);
+      //new packet is buffered up, or we don't have timestamp in fifo, or ack
+      if ( ( m_missed_packets && call FIFO.get() ) || !call FIFOP.get()
+            || !m_timestamp_size
+            || rxFrameLength <= 10) {
+        call PacketTimeStamp.clear(m_p_rx_buf);
+      }
+      else {
+          if (m_timestamp_size==1)
+            call PacketTimeStamp.set(m_p_rx_buf, m_timestamp_queue[ m_timestamp_head ]);
           m_timestamp_head = ( m_timestamp_head + 1 ) % TIMESTAMP_QUEUE_SIZE;
           m_timestamp_size--;
-        }
-      } else {
-        call PacketTimeStamp.clear(m_p_rx_buf);
+
+          if (m_timestamp_size>0) {
+            call PacketTimeStamp.clear(m_p_rx_buf);
+            m_timestamp_head = 0;
+            m_timestamp_size = 0;
+          }
       }
-      
+
       // We may have received an ack that should be processed by Transmit
       // buf[rxFrameLength] >> 7 checks the CRC
       if ( ( buf[ rxFrameLength ] >> 7 ) && rx_buf ) {
diff --git a/tos/chips/cc2420/transmit/CC2420TransmitP.nc b/tos/chips/cc2420/transmit/CC2420TransmitP.nc
index bbf76f09..5e45f8d7 100644
--- a/tos/chips/cc2420/transmit/CC2420TransmitP.nc
+++ b/tos/chips/cc2420/transmit/CC2420TransmitP.nc
@@ -236,12 +236,10 @@ implementation {
   }
   
   
-  inline uint32_t time16to32(uint16_t time, uint32_t recent_time)
+  inline uint32_t getTime32(uint16_t time)
   {
-    if ((recent_time&0xFFFF)<time)
-      return ((recent_time-0x10000UL)&0xFFFF0000UL)|time;
-    else
-      return (recent_time&0xFFFF0000UL)|time;
+    uint32_t recent_time=call BackoffTimer.getNow();
+    return recent_time + (int16_t)(time - recent_time);
   }
 
   /**
@@ -258,8 +256,9 @@ implementation {
    * would have picked up and executed had our microcontroller been fast enough.
    */
   async event void CaptureSFD.captured( uint16_t time ) {
-    uint32_t time32 = time16to32(time, call BackoffTimer.getNow());
+    uint32_t time32;
     atomic {
+      time32 = getTime32(time);
       switch( m_state ) {
         
       case S_SFD:
@@ -284,16 +283,11 @@ implementation {
         releaseSpiResource();
         call BackoffTimer.stop();
 
-        
-        if ( ( ( (call CC2420PacketBody.getHeader( m_msg ))->fcf >> IEEE154_FCF_FRAME_TYPE ) & 7 ) == IEEE154_TYPE_DATA ) {
-          call PacketTimeStamp.set(m_msg, time32);
-        }
-        
         if ( call SFD.get() ) {
           break;
         }
         /** Fall Through because the next interrupt was already received */
-        
+
       case S_EFD:
         sfdHigh = FALSE;
         call CaptureSFD.captureRisingEdge();
@@ -311,7 +305,7 @@ implementation {
         /** Fall Through because the next interrupt was already received */
         
       default:
-        if ( !m_receiving ) {
+        if ( !m_receiving && sfdHigh == FALSE ) {
           sfdHigh = TRUE;
           call CaptureSFD.captureFallingEdge();
           call CC2420Receive.sfd( time32 );
@@ -323,16 +317,17 @@ implementation {
           }
         }
         
-        sfdHigh = FALSE;
-        call CaptureSFD.captureRisingEdge();
-        m_receiving = FALSE;
-        if ( time - m_prev_time < 10 ) {
-          call CC2420Receive.sfd_dropped();
-	  if (m_msg)
-	    call PacketTimeStamp.clear(m_msg);
+        if ( sfdHigh == TRUE ) {
+          sfdHigh = FALSE;
+          call CaptureSFD.captureRisingEdge();
+          m_receiving = FALSE;
+          if ( time - m_prev_time < 10 ) {
+            call CC2420Receive.sfd_dropped();
+            if (m_msg)
+              call PacketTimeStamp.clear(m_msg);
+          }
+          break;
         }
-        break;
-      
       }
     }
   }
-- 
2.39.2