ptpd-mpc831x.patch

来自「最新版IAR FOR ARM（EWARM）5.11中的代码例子」· PATCH 代码 · 共 610 行 · 第 1/2 页

+
+void mpc831x_get_curr_time(TimeInternal *time)
+{
+	struct ifreq 		if_data;
+	struct mpc831x_cnt	hw_cnt;
+	unsigned long long	board_time = 0;
+
+	if_data.ifr_data = (void *)(&hw_cnt);
+	memcpy(if_data.ifr_name, mpc831x_if_name, IFACE_NAME_LENGTH);
+	if(!ioctl(mpc831x_netPath->eventSock, PTP_GET_CNT, &if_data))
+	{
+		board_time = hw_cnt.high;
+		board_time = (board_time << 32) | hw_cnt.low;
+		//DBG( "mpc831x_get_curr_time board_time = %llu\n", board_time );
+
+		time->seconds = BOARD_TIME_TO_SEC(board_time);
+		time->nanoseconds = BOARD_TIME_TO_NSEC(board_time, time->seconds);
+		DBG( "mpc831x_get_curr_time seconds (%d), nanoseconds (%d)\n", \
+			time->seconds, time->nanoseconds );
+	} 
+	else
+	{
+		perror( "Error in mpc831x_get_curr_time()" );
+	}
+}
+
+void mpc831x_set_curr_time(TimeInternal *time)
+{
+	struct ifreq 		if_data;
+	struct mpc831x_cnt	hw_cnt;
+	unsigned long long	board_time = 0;
+
+	board_time = ((unsigned long long)time->seconds) * ((unsigned long long)1000000000) + 
+		     	((unsigned long long)time->nanoseconds);
+
+	board_time = board_time/1000;
+	board_time = board_time*1000;
+
+	hw_cnt.high = (board_time >> 32) & 0xffffffff;
+	hw_cnt.low = (board_time) & 0xffffffff;
+
+	if_data.ifr_data = (void *)(&hw_cnt);
+	memcpy(if_data.ifr_name, mpc831x_if_name, IFACE_NAME_LENGTH);
+	if(!ioctl(mpc831x_netPath->eventSock, PTP_SET_CNT, &if_data))
+	{
+                //DBG( "mpc831x_set_curr_time board_time = %llu Hex (0x%llx)\n", board_time, board_time );
+		DBG( "mpc831x_set_curr_time seconds (%d), nanoseconds (%d)\n", \
+			time->seconds, time->nanoseconds );
+	} 
+	else
+	{
+		perror( "Error in mpc831x_set_curr_time()" );
+	}
+}
+
+void mpc831x_get_tx_time(TimeInternal *time)
+{
+	struct ifreq 		if_data;
+	struct mpc831x_cnt	hw_cnt;
+	unsigned long long	board_time = 0;
+
+	if_data.ifr_data = (void *)(&hw_cnt);
+	memcpy(if_data.ifr_name, mpc831x_if_name, IFACE_NAME_LENGTH);
+	if(!ioctl(mpc831x_netPath->eventSock, PTP_GET_TX_TIMESTAMP, &if_data))
+	{
+		board_time = hw_cnt.high;
+                board_time = (board_time << 32) | hw_cnt.low;
+                //DBG( "mpc831x_get_tx_time board_time = %llu\n", board_time );
+		
+		time->seconds = BOARD_TIME_TO_SEC(board_time);
+		time->nanoseconds = BOARD_TIME_TO_NSEC(board_time, time->seconds);
+		DBG( "mpc831x_get_tx_time seconds (%d), nanoseconds (%d)\n", \
+			time->seconds, time->nanoseconds );
+	} 
+	else
+	{
+		perror( "Error in mpc831x_get_tx_time()" );
+	}
+}
+
+void mpc831x_get_rx_time(TimeInternal *time)
+{
+	struct ifreq 		if_data;
+	struct mpc831x_cnt	hw_cnt;
+	unsigned long long	board_time;
+
+	if_data.ifr_data = (void *)(&hw_cnt);
+	memcpy(if_data.ifr_name, mpc831x_if_name, IFACE_NAME_LENGTH);
+	if(!ioctl(mpc831x_netPath->eventSock, PTP_GET_RX_TIMESTAMP, &if_data))
+	{
+		board_time = hw_cnt.high;
+                board_time = (board_time << 32) | hw_cnt.low;
+                //DBG( "mpc831x_get_rx_time board_time = %llu\n", board_time );
+
+		time->seconds = BOARD_TIME_TO_SEC(board_time);
+		time->nanoseconds = BOARD_TIME_TO_NSEC(board_time, time->seconds);
+		DBG( "mpc831x_get_rx_time seconds (%d), nanoseconds (%d)\n", \
+			time->seconds, time->nanoseconds );
+	} 
+	else
+	{
+		perror( "Error in mpc831x_get_rx_time()" );
+	}
+}
+
+void mpc831x_adj_freq(Integer32 freq)
+{
+ 	struct ifreq    if_data;
+
+	if_data.ifr_data = (void *)(&freq);
+	memcpy(if_data.ifr_name, mpc831x_if_name, IFACE_NAME_LENGTH);
+	if( !ioctl(mpc831x_netPath->eventSock, PTP_ADJ_FREQ, &if_data) )
+	{
+		DBG( "Set frequency in mpc831x_adj_freq (%d)\n", freq );
+	}
+	else
+	{
+		perror( "Error in mpc831x_adj_freq" );
+	}
+}
+
+void mpc831x_adj_addend(Integer32 adj)
+{
+ 	struct ifreq    if_data;
+
+	if_data.ifr_data = (void *)(&adj);
+	memcpy(if_data.ifr_name, mpc831x_if_name, IFACE_NAME_LENGTH);
+	if( !ioctl(mpc831x_netPath->eventSock, PTP_ADJ_ADDEND, &if_data) )
+	{
+		DBG( "Set addend in mpc831x_adj_addend (%d)\n", adj );
+	}
+	else
+	{
+		perror( "Error in mpc831x_adj_addend" );
+	}
+}
+
diff --git a/src/mpc831x.h b/src/mpc831x.h
new file mode 100644
index 0000000..363e0bf
--- /dev/null
+++ b/src/mpc831x.h
@@ -0,0 +1,32 @@
+#ifndef MPC831X_H
+#define MPC831X_H
+
+#include <linux/types.h>
+
+/* Various IOCTL's supported by our driver */
+#define PTP_GET_RX_TIMESTAMP    SIOCDEVPRIVATE
+#define PTP_GET_TX_TIMESTAMP    (SIOCDEVPRIVATE + 1)
+#define PTP_SET_CNT             (SIOCDEVPRIVATE + 2)
+#define PTP_GET_CNT             (SIOCDEVPRIVATE + 3)
+#define PTP_ADJ_FREQ            (SIOCDEVPRIVATE + 4)
+#define PTP_ADJ_ADDEND		(SIOCDEVPRIVATE + 5)
+
+struct mpc831x_cnt
+{
+	uint32_t	high;
+	uint32_t	low;
+};
+
+/* Global variables containing interface information */
+NetPath *mpc831x_netPath;
+char mpc831x_if_name[IFACE_NAME_LENGTH];
+
+
+void mpc831x_get_curr_time(TimeInternal *time);
+void mpc831x_set_curr_time(TimeInternal *time);
+void mpc831x_get_tx_time(TimeInternal *time);
+void mpc831x_get_rx_time(TimeInternal *time);
+void mpc831x_adj_freq(Integer32 freq);
+void mpc831x_adj_addend(Integer32 adj);
+
+#endif //MPC831X_H
diff --git a/src/probe.c b/src/probe.c
index 5454e30..8e7a1b9 100644
--- a/src/probe.c
+++ b/src/probe.c
@@ -64,7 +64,7 @@ void probe(RunTimeOpts *rtOpts, PtpClock *ptpClock)
     if(rtOpts->probe_management_key > 0)
       break;
   }
-  
+ 
   getTime(&finish);
   finish.seconds += PTP_SYNC_INTERVAL_TIMEOUT(ptpClock->sync_interval);
   for(;;)
@@ -73,7 +73,7 @@ void probe(RunTimeOpts *rtOpts, PtpClock *ptpClock)
     interval.nanoseconds = 0;
     netSelect(&interval, &ptpClock->netPath);
     
-    netRecvEvent(0, ptpClock->msgIbuf, 0, &ptpClock->netPath);
+    netRecvEvent(0, ptpClock->msgIbuf, 0, &ptpClock->netPath, ptpClock->port_state);
     
     if(netRecvGeneral(0, ptpClock->msgIbuf, &ptpClock->netPath))
     {
@@ -90,6 +90,7 @@ void probe(RunTimeOpts *rtOpts, PtpClock *ptpClock)
       fflush(stdout);
     }
     
+    printf( "getTime probe (now)\n" ); 
     getTime(&now);
     if( now.seconds > finish.seconds || (now.seconds == finish.seconds
       && now.nanoseconds > finish.nanoseconds) )
diff --git a/src/protocol.c b/src/protocol.c
index 4ae14d4..20d9562 100644
--- a/src/protocol.c
+++ b/src/protocol.c
@@ -1,6 +1,9 @@
 /* protocol.c */
 
 #include "ptpd.h"
+#ifdef CONFIG_MPC831X
+#include "mpc831x.h"
+#endif
 
 Boolean doInit(RunTimeOpts*,PtpClock*);
 void doState(RunTimeOpts*,PtpClock*);
@@ -281,7 +284,7 @@ void handle(RunTimeOpts *rtOpts, PtpClock *ptpClock)
   if(!ptpClock->message_activity)
     netSelect(&wait, &ptpClock->netPath);
   
-  if(netRecvEvent(0, ptpClock->msgIbuf, &time, &ptpClock->netPath))
+  if(netRecvEvent(0, ptpClock->msgIbuf, &time, &ptpClock->netPath, ptpClock->port_state))
   {
     subTime(&time, &time, &rtOpts->inboundLatency);
   }
@@ -496,6 +499,10 @@ void handleDelayReq(MsgHeader *header, TimeInternal *time, Boolean isFromSelf, R
       || header->sourceCommunicationTechnology == PTP_DEFAULT
       || ptpClock->clock_communication_technology == PTP_DEFAULT )
     {
+#ifdef CONFIG_MPC831X
+      /* Get the RX time from our HW counter */
+      mpc831x_get_rx_time(time);
+#endif
       issueDelayResp(time, &ptpClock->msgTmpHeader, rtOpts, ptpClock);
     }
     
@@ -505,6 +512,11 @@ void handleDelayReq(MsgHeader *header, TimeInternal *time, Boolean isFromSelf, R
     if(isFromSelf)
     {
       DBG("handleDelayReq: self\n");
+
+#ifdef CONFIG_MPC831X
+      /* Get the TX time from our HW counter */
+      mpc831x_get_tx_time(time);
+#endif
       
       ptpClock->delay_req_send_time.seconds = time->seconds;
       ptpClock->delay_req_send_time.nanoseconds = time->nanoseconds;
@@ -540,7 +552,7 @@ void handleDelayResp(MsgHeader *header, Octet *msgIbuf, Boolean isFromSelf, RunT
       DBG("handleDelayResp: ignore from self\n");
       return;
     }
-    
+   
     resp = &ptpClock->msgTmp.resp;
     msgUnpackDelayResp(ptpClock->msgIbuf, resp);
     
@@ -554,11 +566,12 @@ void handleDelayResp(MsgHeader *header, Octet *msgIbuf, Boolean isFromSelf, RunT
       && !memcmp(header->sourceUuid, ptpClock->parent_uuid, PTP_UUID_LENGTH) )
     {
       ptpClock->sentDelayReq = FALSE;
-      
+    
       toInternalTime(&ptpClock->delay_req_receive_time, &resp->delayReceiptTimestamp, &ptpClock->halfEpoch);
       
       if(ptpClock->delay_req_send_time.seconds)
       {
+        DBG( "handleDelayResp: from master\n" );
         updateDelay(&ptpClock->delay_req_send_time, &ptpClock->delay_req_receive_time,
           &ptpClock->owd_filt, rtOpts, ptpClock);
         
@@ -626,7 +639,8 @@ void issueSync(RunTimeOpts *rtOpts, PtpClock *ptpClock)
   
   ++ptpClock->last_sync_event_sequence_number;
   ptpClock->grandmaster_sequence_number = ptpClock->last_sync_event_sequence_number;
-  
+ 
+  printf( "getTime issueSync\n" ); 
   getTime(&internalTime);
   fromInternalTime(&internalTime, &originTimestamp, ptpClock->halfEpoch);
   msgPackSync(ptpClock->msgObuf, FALSE, &originTimestamp, ptpClock);
@@ -660,14 +674,16 @@ void issueDelayReq(RunTimeOpts *rtOpts, PtpClock *ptpClock)
   ptpClock->sentDelayReq = TRUE;
   ptpClock->sentDelayReqSequenceId = ++ptpClock->last_sync_event_sequence_number;
   
+  printf( "getTime issueDelayReq\n" ); 
   getTime(&internalTime);
   fromInternalTime(&internalTime, &originTimestamp, ptpClock->halfEpoch);
   msgPackDelayReq(ptpClock->msgObuf, FALSE, &originTimestamp, ptpClock);
   
   if(!netSendEvent(0, ptpClock->msgObuf, DELAY_REQ_PACKET_LENGTH, &ptpClock->netPath))
     toState(PTP_FAULTY, rtOpts, ptpClock);
-  else
+  else{
     DBGV("sent delay request message\n");
+  }
 }
 
 void issueDelayResp(TimeInternal *time, MsgHeader *header, RunTimeOpts *rtOpts, PtpClock *ptpClock)