int wrc_ptp_set_mode(int mode)
{
uint32_t start_tics, lock_timeout = 0;
ptp_mode = 0;
wrc_ptp_stop();
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");
shw_pps_gen_enable_output(0);
while (!spll_check_lock(0) && lock_timeout) {
timer_delay_ms(1000);
mprintf(".");
if (time_after(timer_get_tics(), start_tics + lock_timeout)) {
mprintf("\nLock timeout.\n");
return -ETIMEDOUT;
} else if (uart_read_byte() == 27) {
mprintf("\n");
return -EINTR;
}
}
mprintf("\n");
if (mode == WRC_MODE_MASTER || mode == WRC_MODE_GM)
shw_pps_gen_enable_output(1);
ptp_mode = mode;
return 0;
}
/* Returns 1 if it did something */
static int temp_w1_refresh(struct wrc_temp *t)
{
static int done;
if (!done) {
nextt = timer_get_tics() + 1000;
done++;
}
/* Odd iterations: send the command, even iterations: read back */
int phase = niterations & 1;
static int intervals[] = {
200, (1000 * CONFIG_TEMP_POLL_INTERVAL) - 200
};
if (time_before(timer_get_tics(), nextt))
return 0;
nextt += intervals[phase];
niterations++;
switch(phase) {
case 0:
w1_read_temp_bus(&wrpc_w1_bus, W1_FLAG_NOWAIT);
break;
case 1:
temp_w1_data[0].t =
w1_read_temp_bus(&wrpc_w1_bus, W1_FLAG_COLLECT);
break;
}
return 1;
}
void timer_delay(uint32_t tics)
{
uint32_t t_end;
// timer_init(1);
t_end = timer_get_tics() + tics;
while (time_before(timer_get_tics(), t_end))
;
}
void syslog_init(void)
{
syslog_socket = ptpd_netif_create_socket(&__static_syslog_socket, NULL,
PTPD_SOCK_UDP, 514 /* time */);
syslog_addr.sport = syslog_addr.dport = htons(514);
tics_zero = timer_get_tics();
}
int syslog_poll(void)
{
struct wr_sockaddr addr;
char buf[256];
char b[32];
unsigned char mac[6];
unsigned char ip[4];
static uint32_t down_tics;
int len = 0;
uint32_t now;
/* for temperature state */
static uint32_t next_temp_report, next_temp_check;
/* for servo-state (accesses ppsi internal variables */
extern struct pp_instance *ppi;
struct wr_servo_state *s;
static uint32_t prev_tics;
static int track_ok_count, prev_servo_state = -1;
if (IS_HOST_PROCESS)
s = NULL;
else
s = &((struct wr_data *)ppi->ext_data)->servo_state;
if (ip_status == IP_TRAINING)
return 0;
if (!syslog_addr.daddr)
return 0;
now = timer_get_tics();
if (!tics) {
/* first time ever, or new syslog server */
tics = now - 1;
get_mac_addr(mac);
len = syslog_header(buf, SYSLOG_DEFAULT_LEVEL, ip);
len += pp_sprintf(buf + len, "(%s) Node up "
"since %i seconds", format_mac(b, mac),
(tics - tics_zero) / 1000);
goto send;
}
if (link_status == LINK_WENT_DOWN)
down_tics = now;
if (link_status == LINK_UP && down_tics) {
down_tics = now - down_tics;
len = syslog_header(buf, SYSLOG_DEFAULT_LEVEL, ip);
len += pp_sprintf(buf + len, "Link up after %i.%03i s",
down_tics / 1000, down_tics % 1000);
down_tics = 0;
goto send;
}
if (!prev_tics)
prev_tics = now;
if (s && s->state == WR_TRACK_PHASE &&
prev_servo_state != WR_TRACK_PHASE) {
/* we reached sync: log it */
track_ok_count++;
prev_servo_state = s->state;
prev_tics = now - prev_tics;
if (track_ok_count == 1) {
len = syslog_header(buf, SYSLOG_DEFAULT_LEVEL, ip);
len += pp_sprintf(buf + len,
"Tracking after %i.%03i s",
prev_tics / 1000, prev_tics % 1000);
goto send;
}
len = syslog_header(buf, SYSLOG_DEFAULT_LEVEL, ip);
len += pp_sprintf(buf + len,
"%i-th re-rtrack after %i.%03i s",
track_ok_count,
prev_tics / 1000, prev_tics % 1000);
goto send;
}
if (s && s->state != WR_TRACK_PHASE &&
prev_servo_state == WR_TRACK_PHASE) {
prev_servo_state = s->state;
prev_tics = now;
len = syslog_header(buf, SYSLOG_DEFAULT_LEVEL, ip);
len += pp_sprintf(buf + len, "Lost track");
goto send;
}
if (!next_temp_check) {
next_temp_check = now + 1000;
next_temp_report = 0;
}
if (time_after_eq(now, next_temp_check)) {
struct wrc_onetemp *t = NULL;
int over_t = 0;
next_temp_check += 1000;
while ( (t = wrc_temp_getnext(t)) )
over_t += (t->t > (CONFIG_TEMP_HIGH_THRESHOLD << 16));
/* report if over temp, and first report or rappel time */
if (over_t && (!next_temp_report
|| time_after_eq(now, next_temp_report))) {
next_temp_report = now + 1000 * CONFIG_TEMP_HIGH_RAPPEL;
len = syslog_header(buf, SYSLOG_DEFAULT_LEVEL, ip);
len += pp_sprintf(buf + len, "Temperature high: ");
len += wrc_temp_format(buf + len, sizeof(buf) - len);
goto send;
}
if (!over_t && next_temp_report) {
next_temp_report = 0;
len = syslog_header(buf, SYSLOG_DEFAULT_LEVEL, ip);
len += pp_sprintf(buf + len, "Temperature ok: ");
len += wrc_temp_format(buf + len, sizeof(buf) - len);
goto send;
}
}
return 0;
send:
memcpy(&syslog_addr.saddr, ip, 4);
fill_udp((void *)buf, len, &syslog_addr);
memcpy(&addr.mac, syslog_mac, 6);
ptpd_netif_sendto(syslog_socket, &addr, buf, len, 0);
return 1;
}
uint64_t ptpd_netif_get_msec_tics(void)
{
return timer_get_tics();
}
int external_align_fsm(volatile struct spll_external_state *s)
{
int v, done_sth = 0;
switch(s->align_state) {
case ALIGN_STATE_EXT_OFF:
break;
case ALIGN_STATE_WAIT_CLKIN:
if( !(SPLL->ECCR & SPLL_ECCR_EXT_REF_STOPPED) ) {
SPLL->ECCR |= SPLL_ECCR_EXT_REF_PLLRST;
s->align_state = ALIGN_STATE_WAIT_PLOCK;
done_sth++;
}
break;
case ALIGN_STATE_WAIT_PLOCK:
SPLL->ECCR &= (~SPLL_ECCR_EXT_REF_PLLRST);
if( SPLL->ECCR & SPLL_ECCR_EXT_REF_STOPPED )
s->align_state = ALIGN_STATE_WAIT_CLKIN;
else if( SPLL->ECCR & SPLL_ECCR_EXT_REF_LOCKED )
s->align_state = ALIGN_STATE_START;
done_sth++;
break;
case ALIGN_STATE_START:
if(s->helper->ld.locked) {
disable_irq();
mpll_start(s->main);
enable_irq();
s->align_state = ALIGN_STATE_START_MAIN;
done_sth++;
}
break;
case ALIGN_STATE_START_MAIN:
SPLL->AL_CR = 2;
if(s->helper->ld.locked && s->main->ld.locked) {
PPSG->CR = PPSG_CR_CNT_EN | PPSG_CR_PWIDTH_W(10);
PPSG->ADJ_NSEC = 3;
PPSG->ESCR = PPSG_ESCR_SYNC;
s->align_state = ALIGN_STATE_INIT_CSYNC;
pll_verbose("EXT: DMTD locked.\n");
done_sth++;
}
break;
case ALIGN_STATE_INIT_CSYNC:
if (PPSG->ESCR & PPSG_ESCR_SYNC) {
PPSG->ESCR = PPSG_ESCR_PPS_VALID; // enable PPS output (even though it's not aligned yet)
s->align_timer = timer_get_tics() + 2 * TICS_PER_SECOND;
s->align_state = ALIGN_STATE_WAIT_CSYNC;
done_sth++;
}
break;
case ALIGN_STATE_WAIT_CSYNC:
if(time_after_eq(timer_get_tics(), s->align_timer)) {
s->align_state = ALIGN_STATE_START_ALIGNMENT;
s->align_shift = 0;
pll_verbose("EXT: CSync complete.\n");
done_sth++;
}
break;
case ALIGN_STATE_START_ALIGNMENT:
if(align_sample(1, &v)) {
v %= ALIGN_SAMPLE_PERIOD;
if(v == 0 || v >= ALIGN_SAMPLE_PERIOD / 2) {
s->align_target = EXT_PERIOD_NS;
s->align_step = -100;
} else if (s > 0) {
s->align_target = 0;
s->align_step = 100;
}
pll_verbose("EXT: Align target %d, step %d.\n", s->align_target, s->align_step);
s->align_state = ALIGN_STATE_WAIT_SAMPLE;
done_sth++;
}
break;
case ALIGN_STATE_WAIT_SAMPLE:
if(!mpll_shifter_busy(s->main) && align_sample(1, &v)) {
v %= ALIGN_SAMPLE_PERIOD;
if(v != s->align_target) {
s->align_shift += s->align_step;
mpll_set_phase_shift(s->main, s->align_shift);
} else if (v == s->align_target) {
s->align_shift += EXT_PPS_LATENCY_PS;
mpll_set_phase_shift(s->main, s->align_shift);
s->align_state = ALIGN_STATE_COMPENSATE_DELAY;
}
done_sth++;
}
break;
case ALIGN_STATE_COMPENSATE_DELAY:
if(!mpll_shifter_busy(s->main)) {
pll_verbose("EXT: Align done.\n");
s->align_state = ALIGN_STATE_LOCKED;
done_sth++;
}
break;
case ALIGN_STATE_LOCKED:
if(!external_locked(s)) {
s->align_state = ALIGN_STATE_WAIT_CLKIN;
done_sth++;
}
break;
default:
break;
}
return done_sth != 0;
}
