wrc_ptp.c 3.79 KB
Newer Older
1 2 3 4 5 6 7 8 9
#include <stdio.h>
#include <inttypes.h>
#include <errno.h>

#include "ptpd.h"
#include "ptpd_netif.h"
#include "timer.h"
#include "softpll_ng.h"
#include "wrc_ptp.h"
10
#include "pps_gen.h"
11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50

static RunTimeOpts rtOpts = {
   .ifaceName = { "wr1" },
   .announceInterval = DEFAULT_ANNOUNCE_INTERVAL,
   .syncInterval = DEFAULT_SYNC_INTERVAL,
   .clockQuality.clockAccuracy = DEFAULT_CLOCK_ACCURACY,
   .clockQuality.clockClass = DEFAULT_CLOCK_CLASS,
   .clockQuality.offsetScaledLogVariance = DEFAULT_CLOCK_VARIANCE,
   .priority1 = DEFAULT_PRIORITY1,
   .priority2 = DEFAULT_PRIORITY2,
   .domainNumber = DEFAULT_DOMAIN_NUMBER,
   .currentUtcOffset = DEFAULT_UTC_OFFSET,
   .noResetClock = DEFAULT_NO_RESET_CLOCK,
   .noAdjust = NO_ADJUST,
   .inboundLatency.nanoseconds = DEFAULT_INBOUND_LATENCY,
   .outboundLatency.nanoseconds = DEFAULT_OUTBOUND_LATENCY,
   .s = DEFAULT_DELAY_S,
   .ap = DEFAULT_AP,
   .ai = DEFAULT_AI,
   .max_foreign_records = DEFAULT_MAX_FOREIGN_RECORDS,

   /**************** White Rabbit *************************/
   .autoPortDiscovery = FALSE,     /*if TRUE: automagically discovers how many ports we have (and how many up-s); else takes from .portNumber*/
   .portNumber 		= 1,
   .calPeriod     = WR_DEFAULT_CAL_PERIOD,
   .E2E_mode 		  = TRUE,
   .wrStateRetry	= WR_DEFAULT_STATE_REPEAT,
   .wrStateTimeout= WR_DEFAULT_STATE_TIMEOUT_MS,
   .phyCalibrationRequired = FALSE,
	.disableFallbackIfWRFails = TRUE,
         
   .primarySource = FALSE,
   .wrConfig      = WR_S_ONLY,
   .masterOnly    = FALSE,

   /********************************************************/
};

static   PtpPortDS *ptpPortDS;
static   PtpClockDS ptpClockDS;
51
static int ptp_enabled = 0, ptp_mode = WRC_MODE_UNKNOWN;
52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80

int wrc_ptp_init()
{
	Integer16 ret;
	
  netStartup();

  ptpPortDS = ptpdStartup(0, NULL, &ret, &rtOpts, &ptpClockDS);
  initDataClock(&rtOpts, &ptpClockDS);

  //initialize sockets
  if(!netInit(&ptpPortDS->netPath, &rtOpts, ptpPortDS))
  {
    PTPD_TRACE(TRACE_WRPC, NULL,"failed to initialize network\n");
    return -1;
  }
  
  ptpPortDS->linkUP = FALSE;
  ptp_enabled = 0;
  return 0;
}

#define LOCK_TIMEOUT_FM (4 * TICS_PER_SECOND)
#define LOCK_TIMEOUT_GM (60 * TICS_PER_SECOND)

int wrc_ptp_set_mode(int mode)
{
	uint32_t start_tics, lock_timeout = 0;

81 82
	ptp_mode = 0;
	
83
  wrc_ptp_stop();
84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115
	
	switch(mode)
	{
		case WRC_MODE_GM:	
			rtOpts.primarySource = TRUE;
			rtOpts.wrConfig = WR_M_ONLY;
			rtOpts.masterOnly = TRUE;
			spll_init(SPLL_MODE_GRAND_MASTER, 0, 1);
			lock_timeout = LOCK_TIMEOUT_GM;
			break;

		case WRC_MODE_MASTER:
			rtOpts.primarySource = FALSE;
			rtOpts.wrConfig = WR_M_ONLY;
			rtOpts.masterOnly = TRUE;
			spll_init(SPLL_MODE_FREE_RUNNING_MASTER, 0, 1);
			lock_timeout = LOCK_TIMEOUT_FM;
			break;

	case WRC_MODE_SLAVE:
			rtOpts.primarySource = FALSE;
			rtOpts.wrConfig = WR_S_ONLY;
			rtOpts.masterOnly = FALSE;			
      spll_init(SPLL_MODE_SLAVE, 0, 1);
			break;
	}

  initDataClock(&rtOpts, &ptpClockDS);
		
	start_tics = timer_get_tics();
	
	mprintf("Locking PLL");
116 117 118

	pps_gen_enable_output(0);

119 120 121 122 123 124 125 126 127 128 129 130 131 132
	while(!spll_check_lock(0) && lock_timeout)
	{
		timer_delay(TICS_PER_SECOND);
		mprintf(".");
		if(timer_get_tics() - start_tics > lock_timeout)
		{
			mprintf("\nLock timeout.\n");
			return -ETIMEDOUT;
		} else if (uart_read_byte() == 27)
		{
			mprintf("\n");
			return -EINTR;
		}
	}
133 134 135 136
	
	if(mode == WRC_MODE_MASTER || mode == WRC_MODE_GM)
		pps_gen_enable_output(1);
	
137
	mprintf("\n");
138
	ptp_mode = mode;
139 140 141
	return 0;
}

142 143 144 145 146
int wrc_ptp_get_mode()
{
	return ptp_mode;
}

147 148 149
int wrc_ptp_start()
{
  ptpPortDS->linkUP = FALSE;
150
  wr_servo_reset();
151 152 153 154 155 156 157 158 159
  initDataClock(&rtOpts, &ptpClockDS);

  ptp_enabled = 1;
  return 0;
}

int wrc_ptp_stop()
{
	ptp_enabled = 0;
160
  wr_servo_reset();
161 162 163 164 165 166 167 168 169 170 171 172
	return 0;
}

int wrc_ptp_update()
{
	if(ptp_enabled)
	{
	  singlePortLoop(&rtOpts, ptpPortDS, 0);
	  sharedPortsLoop(ptpPortDS);
	}
	return 0;
}