/*---------------------------------------------------------------------------*/
void
lpm_exit()
{
if((REG(SYS_CTRL_PMCTL) & SYS_CTRL_PMCTL_PM3) == SYS_CTRL_PMCTL_PM0) {
/* We either just exited PM0 or we were not sleeping in the first place.
* We don't need to do anything clever */
return;
}
LPM_STATS_ADD(REG(SYS_CTRL_PMCTL) & SYS_CTRL_PMCTL_PM3,
RTIMER_NOW() - sleep_enter_time);
/* Adjust the system clock, since it was not counting while we were sleeping
* We need to convert sleep duration from rtimer ticks to clock ticks and
* this will cost us some accuracy */
clock_adjust((clock_time_t)
((RTIMER_NOW() - sleep_enter_time) / RTIMER_CLOCK_TICK_RATIO));
/* Restore system clock to the 32 MHz XOSC */
select_32_mhz_xosc();
/* Restore PMCTL to PM0 for next pass */
REG(SYS_CTRL_PMCTL) = SYS_CTRL_PMCTL_PM0;
/* Remember IRQ energest for next pass */
ENERGEST_IRQ_SAVE(irq_energest);
ENERGEST_ON(ENERGEST_TYPE_CPU);
ENERGEST_OFF(ENERGEST_TYPE_LPM);
}
/***********************************************************************
*
* Function: cfg_user_reset
*
* Purpose: Reset user configuration data
*
* Processing:
* See function.
*
* Parameters: None
*
* Outputs: None
*
* Returns: Nothing
*
* Notes: None
*
**********************************************************************/
void cfg_user_reset(void)
{
s1l_board_cfg.armclk = 208000000;
s1l_board_cfg.hclkdiv = 2;
s1l_board_cfg.pclkdiv = 16;
clock_adjust();
cfg_save(&syscfg);
}
/***********************************************************************
*
* Function: cmd_clock
*
* Purpose: Clock command
*
* Processing:
* Parse the string elements for the clock command and sets the
* new clock value.
*
* Parameters: None
*
* Outputs: None
*
* Returns: TRUE if the command was processed, otherwise FALSE
*
* Notes: May not work in DDR systems.
*
**********************************************************************/
BOOL_32 cmd_clock(void)
{
UNS_32 freqr, armclk, hclkdiv, pclkdiv;
UNS_8 dclk [32];
BOOL_32 processed = TRUE;
/* Get arguments */
armclk = cmd_get_field_val(1);
hclkdiv = cmd_get_field_val(2);
pclkdiv = cmd_get_field_val(3);
if (!((hclkdiv == 1) || (hclkdiv == 2) || (hclkdiv == 4)))
{
processed = FALSE;
}
if ((pclkdiv < 1) || (pclkdiv > 32))
{
processed = FALSE;
}
if ((armclk < 33) || (armclk > 550))
{
term_dat_out_crlf(clockerr_msg);
processed = FALSE;
}
else
{
armclk = armclk * 1000 * 1000;
}
if (processed == TRUE)
{
sys_saved_data.clksetup.armclk = armclk;
sys_saved_data.clksetup.hclkdiv = hclkdiv;
sys_saved_data.clksetup.pclkdiv = pclkdiv;
freqr = clock_adjust();
if (freqr == 0)
{
term_dat_out_crlf(clockbad_msg);
}
else
{
term_dat_out(clock_msg);
str_makedec(dclk, freqr);
term_dat_out_crlf(dclk);
sys_save(&sys_saved_data);
}
}
return TRUE;
}
/***********************************************************************
*
* Function: cfg_load
*
* Purpose: Load an S1L conmfiguariton from non-volatile memory
*
* Processing:
* See function.
*
* Parameters:
* pCfg : Pointer to config structure to populate
*
* Outputs: None
*
* Returns: FALSE if the structure wasn't loaded, otherwise TRUE
*
* Notes: None
*
**********************************************************************/
BOOL_32 cfg_load(S1L_CFG_T *pCfg)
{
UNS_8 *psspcfg = (UNS_8 *) pCfg;
UNS_8 *cfg = (UNS_8*)&cfg_check_data;
UNS_32 idx = 0;
/* Config SSP for correct mode */
board_spi_config();
/* Read Dummy Byte to Clear FIFO */
*cfg = board_spi_read(idx);
/* Read S1L Configuration & Verification Tag */
for (idx = 0; idx < 4; idx++)
{
*cfg = board_spi_read(key_offset + idx);
cfg++;
}
/* Return False if Verification tag doesn't match */
if (cfg_check_data != KEYCHECKVAL)
return FALSE;
/* Read configuration structure */
for (idx = 0; idx < sizeof(S1L_CFG_T); idx++)
{
*psspcfg = board_spi_read(cfg_check_offset + idx);
psspcfg++;
}
/* Read board configuration structure */
psspcfg = (UNS_8 *) &s1l_board_cfg;
for (idx = 0; idx < sizeof(S1L_BOARD_CFG_T); idx++)
{
*psspcfg = board_spi_read(board_cfg_offset + idx);
psspcfg++;
}
clock_adjust();
return TRUE;
}
/*---------------------------------------------------------------------------*/
void
lpm_exit()
{
if((REG(SYS_CTRL_PMCTL) & SYS_CTRL_PMCTL_PM3) == SYS_CTRL_PMCTL_PM0) {
/* We either just exited PM0 or we were not sleeping in the first place.
* We don't need to do anything clever */
return;
}
/*
* When returning from PM1/2, the sleep timer value (used by RTIMER_NOW()) is
* not up-to-date until a positive edge on the 32-kHz clock has been detected
* after the system clock restarted. To ensure an updated value is read, wait
* for a positive transition on the 32-kHz clock by polling the
* SYS_CTRL_CLOCK_STA.SYNC_32K bit, before reading the sleep timer value.
*/
while(REG(SYS_CTRL_CLOCK_STA) & SYS_CTRL_CLOCK_STA_SYNC_32K);
while(!(REG(SYS_CTRL_CLOCK_STA) & SYS_CTRL_CLOCK_STA_SYNC_32K));
LPM_STATS_ADD(REG(SYS_CTRL_PMCTL) & SYS_CTRL_PMCTL_PM3,
RTIMER_NOW() - sleep_enter_time);
/* Adjust the system clock, since it was not counting while we were sleeping
* We need to convert sleep duration from rtimer ticks to clock ticks and
* this will cost us some accuracy */
clock_adjust((clock_time_t)
((RTIMER_NOW() - sleep_enter_time) / RTIMER_CLOCK_TICK_RATIO));
/* Restore system clock to the 32 MHz XOSC */
select_32_mhz_xosc();
/* Restore PMCTL to PM0 for next pass */
REG(SYS_CTRL_PMCTL) = SYS_CTRL_PMCTL_PM0;
/* Remember IRQ energest for next pass */
ENERGEST_IRQ_SAVE(irq_energest);
ENERGEST_ON(ENERGEST_TYPE_CPU);
ENERGEST_OFF(ENERGEST_TYPE_LPM);
}
/*
* clock_filter(p, offset, delay, dispersion) - select the best from the
* latest eight delay/offset samples.
*/
static int clock_filter(struct p *p, double offset, double delay, double disp)
{
double dst[NSTAGE];
int ord[NSTAGE];
double dtemp, etemp;
int i, j, k, m, ret;
ret = get_old_offset(&dtemp);
if (ret != NTP_OK) return ret;
dprint("clock_filter: delay:%f, offset:%f, old_offset:%f, real_offset:%f, disp:%f\n",
delay, offset, dtemp, (offset - dtemp), disp);
offset -= dtemp;
j = p->nextf;
p->f[j].offset = offset;
p->f[j].delay = delay;
p->f[j].disp = disp;
p->f[j].t = clock.now;
j = (j + 1) % NSTAGE;
p->nextf = j;
dtemp = PHI * (clock.now - p->update);
p->update = clock.now;
for (i = NSTAGE - 1; i >= 0; i--) {
if (i != 0)
p->f[j].disp += dtemp;
if (p->f[j].disp >= MAXDISPERSE) {
p->f[j].disp = MAXDISPERSE;
dst[i] = MAXDISPERSE;
} else if (p->update - p->f[j].t > ULOG2D(ALLAN)) {
dst[i] = p->f[j].delay + p->f[j].disp;
} else {
dst[i] = p->f[j].delay;
}
ord[i] = j;
j = (j + 1) % NSTAGE;
}
for (i = 1; i < NSTAGE; i++) {
for (j = 0; j < i; j++) {
if (dst[j] > dst[i]) {
k = ord[j];
ord[j] = ord[i];
ord[i] = k;
etemp = dst[j];
dst[j] = dst[i];
dst[i] = etemp;
}
}
}
m = 0;
for (i = 0; i < NSTAGE; i++) {
/*p->f[i].order = (u_char) ord[i];*/
if (dst[i] >= MAXDISPERSE || (m >= 2 && dst[i] >= MAXDISTANCE))
continue;
m++;
}
dprint("clock_filter: m:%i\n", m);
p->disp = p->jitter = 0;
k = ord[0];
for (i = NSTAGE - 1; i >= 0; i--) {
j = ord[i];
p->disp = FWEIGHT * (p->disp + p->f[j].disp);
if (i < m)
p->jitter += SQUARE(p->f[j].offset - p->f[k].offset);
}
if (m == 0) {
return NTP_ERR_IGNORED;
}
etemp = fabs(p->offset - p->f[k].offset);
p->offset = p->f[k].offset;
p->delay = p->f[k].delay;
if (m > 1)
p->jitter /= m - 1;
p->jitter = max(SQRT(p->jitter), LOG2D(PRECISION));
dprint("clock_filter: p->offset:%f, p->disp:%f, p->jitter:%f\n", p->offset, p->disp, p->jitter);
if (p->disp < MAXDISTANCE &&
p->f[k].disp < MAXDISTANCE &&
etemp > SGATE * p->jitter &&
p->f[k].t - p->t < 2.0 * ULOG2D(p->poll)) {
return NTP_ERR_IGNORED;
}
if (p->f[k].t <= p->t) {
return NTP_ERR_IGNORED;
}
p->t = p->f[k].t;
if (root_distance(p) >= MAXDISTANCE + PHI * ULOG2D(p->poll))
return NTP_ERR_IGNORED;
if (fabs(p->offset) < PGATE * p->jitter) {
dprint("clock_filter: p->poll++\n");
tc_counter += p->poll;
if (tc_counter > CLOCK_LIMIT) {
tc_counter = CLOCK_LIMIT;
if (p->poll < MAXPOLL) {
tc_counter = 0;
p->poll++;
}
}
} else {
dprint("clock_filter: p->poll--\n");
tc_counter -= p->poll << 1;
if (tc_counter < -CLOCK_LIMIT) {
tc_counter = -CLOCK_LIMIT;
if (p->poll > MINPOLL) {
tc_counter = 0;
p->poll--;
}
}
}
dprint("clock_filter: p->poll:%i, tc_counter:%i\n", p->poll, tc_counter);
return clock_adjust(p);
}
/***********************************************************************
*
* Function: cfg_load
*
* Purpose: Load an S1L configuariton
*
* Processing:
* See function.
*
* Parameters:
* pCfg : Pointer to config structure to populate
*
* Outputs: None
*
* Returns: FALSE if the structure wasn't loaded, otherwise TRUE
*
* Notes: None
*
**********************************************************************/
BOOL_32 cfg_load(S1L_CFG_T *pCfg)
{
UNS_32 sector, blk = adjusted_num_blocks - 1;
UNS_32 cfg_check_data = 0;
UNS_32 pg_sz = sysConfig.nandgeom->data_bytes_per_page;
if (cfg_size_check() == FALSE)
return FALSE;
while (blk < adjusted_num_blocks)
{
/* If bad block skip-it */
if (flash_is_bad_block(blk) != TRUE)
{
INT_32 offset = 0, page = 0;
INT_32 sz_bcfg = sizeof(s1l_board_cfg);
INT_32 sz_cfg = sizeof(*pCfg);
UNS_8 *ptr_cfg = (UNS_8 *)pCfg;
UNS_8 *ptr_bcfg = (UNS_8 *)&s1l_board_cfg;
/* Read first page of sector */
sector = conv_to_sector(blk, page);
if(flash_read_sector(sector, secdat, NULL) < 0)
return FALSE;
/* Check for the signature */
memcpy(&cfg_check_data, secdat, sizeof(cfg_check_data));
offset += sizeof(cfg_check_data);
if (cfg_check_data != CHECK_KEY)
return FALSE;
page ++;
/* Copy structure by structure */
while (sz_bcfg > 0 || sz_cfg > 0)
{
/* Copy S1l Configuration structure */
if (sz_cfg > 0)
{
UNS_32 tmp = sz_cfg > (pg_sz - offset) ? pg_sz - offset : sz_cfg;
memcpy(ptr_cfg, (UNS_8 *)secdat + offset, tmp);
sz_cfg -= tmp;
offset += tmp;
ptr_cfg += tmp;
}
/* Copy Board configuration structure */
if (sz_bcfg > 0 && offset < pg_sz)
{
UNS_32 tmp = sz_bcfg > (pg_sz - offset) ? pg_sz - offset : sz_bcfg;
memcpy(ptr_bcfg, (UNS_8 *)secdat + offset, tmp);
sz_bcfg -= tmp;
offset += tmp;
ptr_bcfg += tmp;
}
if (sz_bcfg > 0 || sz_cfg > 0)
{
sector = conv_to_sector(blk, page);
if(flash_read_sector(sector, secdat, NULL) < 0)
return FALSE;
page ++;
offset = 0;
}
}
#ifdef ENABLE_CKLSWITCHING
clock_adjust();
#endif
return TRUE;
} else {
adjusted_num_blocks ++;
// sysinfo.sysrtcfg.bl_num_blks = adjusted_num_blocks;
}
blk ++;
}
return FALSE;
}
