ss_time_lib.c

#include "edtinc.h"
#include "edt_ss_vco.h"
#include "edt_bitload.h"
#include "edt_threads.h"

#include <sys/timeb.h>

#ifdef WIN32
#include <process.h>
#endif


#include <math.h>

#include "ss_time_lib.h"



#define USECS_FACTOR  (1000000.0 / (double) (1 << 20))

/*
* return the microseconds value for the current second 
*
*/

#ifdef WIN32

int usecs_difference = 0;
int usecs_calibrated = 0;
double realtime_diff = 0.0;
double mean_trend = 0.0;

#define POLL_MARGIN 975
void edt_calibrate_usecs(int test_loops)

{
    int i;
    int loops = 1;
    double sum = 0.0, whole, msec, diff = 0, t, testms, last_diff;


    FILETIME ft;
    uint64_t dt;
    SYSTEMTIME tm;
    uint64_t dt_1970;

    if (test_loops > 1)
        loops = test_loops;

    /* compute 1970 time */
    tm.wYear = 1970;
    tm.wMonth = 1;
    tm.wHour = 0;
    tm.wMinute = 0;
    tm.wSecond = 0;
    tm.wMilliseconds = 0;
    tm.wDay = 1;
    tm.wDayOfWeek = 1;

    SystemTimeToFileTime(&tm, &ft);

    dt_1970 = (uint64_t) ft.dwHighDateTime << 32 | (uint64_t)ft.dwLowDateTime;


    /* look for zero-crossing */


    /*  uses edt_timestamp() */
    mean_trend = 0;


    for (i=0;i<loops;i++)
    {
        Sleep(10);

        t = edt_timestamp();

        msec = 1000000.0 * modf(t, &whole);

        GetSystemTimeAsFileTime(&ft);
        dt = (uint64_t) ft.dwHighDateTime << 32 | (uint64_t)ft.dwLowDateTime;

        dt -= dt_1970;

        testms = dt * 0.0000001;

        diff = t - testms;

        sum += diff;

        if (i>0)
            mean_trend += diff - last_diff;

        last_diff = diff;

    }

    realtime_diff = (sum / (double) loops);
    if (loops > 1)
        mean_trend /= (loops - 1);

    usecs_calibrated = 1;

}

void old_edt_calibrate_usecs()

{
    int i;
    int loops = 1;
    time_t t, t1;
    struct timeb tb1;
    double sum = 0.0, dtsum = 0.0, whole, frac, last_frac = 0,
        trend = 0, dt;
    /* look for zero-crossing */


    /*  uses edt_timestamp() */

    for (i=0;i<loops;i++)
    {
        ftime(&tb1);

        if (tb1.millitm < POLL_MARGIN)
            Sleep(POLL_MARGIN - tb1.millitm);

        t1 = time(&t);

        do {
            t = time(&t);
            dt = edt_timestamp();
        } while (t == t1);

        frac = modf(dt, &whole);
        trend = frac - last_frac;
        last_frac = frac;
        printf("frac = %f trend= %f\n", frac, trend);
        sum += frac;
        dtsum += (dt - t);

    }

    usecs_difference = (u_int) ((sum / (double) loops) * 1000000.0);
    realtime_diff = (dtsum / (double) loops);
    printf("usecs_difference = %d realtime_diff = %f\n", usecs_difference, realtime_diff);
    usecs_calibrated = 1;

}


double
edt_sys_timestamp()

{


    double t;

    if (!usecs_calibrated)
        edt_calibrate_usecs(0);


    t = edt_timestamp();

    return t - realtime_diff;

}



#else

#define edt_sys_timestamp edt_timestamp

#endif

int 
edt_usecs()

{

#ifdef WIN32

    double t;

    if (!usecs_calibrated)
        edt_calibrate_usecs(0);

    t = (edt_sys_timestamp() * 1000000.0);
    t = fmod(t, 1000000.0);

    return (int) t;

#elif defined(VXWORKS)

    struct timespec     endtime ;
    clock_gettime(CLOCK_REALTIME,&endtime);
    return endtime.tv_nsec / 1000;

#else                           /* _NT_ */
    struct timeval endtime;

    gettimeofday(&endtime, (struct timezone *) NULL);

    return endtime.tv_usec;

#endif                          /* _NT_ */

}

/* used to compensate for time between end of wait and 
actual zero-crossing */

static int wait_zero_offset = 0;
static int wait_zero_calibrated = 0;

/*
*
* Wait for system time zero-crossing 
* margin is #of microseconds before zero to satisfy wait
* (normally wait_zero_offset)
*
*/


static void
edt_wait_for_zero(int margin)

{
    int usec;
    int lastusec = 0;

    int test = 1000000 - margin;

    usec = edt_usecs();

    edt_usleep(1000 - (usec/1000) - 100);
    while (usec < test && usec >= lastusec)
    {
        lastusec = usec;

        usec = edt_usecs();
    }
}

void 
edt_sstm_setup(EdtTimeController *tm, char *bitfile)

{

    if (bitfile)
        edt_bitload(tm->dev_p, ".", bitfile, 0, 0);
    else 
        edt_bitload(tm->dev_p, ".", "c3_demux.bit", 0, 0);
}

void edt_sstm_strobe(EdtTimeController *tm, unsigned int bits)

{
    int cmd;
    int count = 0;

    do
    {
        cmd = edt_reg_read(tm->dev_p, tm->cmd);
        if (cmd & EDT_SSTM_CMD_LOCK)
            edt_msleep(0);
    } while (cmd & EDT_SSTM_CMD_LOCK && (count++ < 5));


    cmd &= 0x70;

    edt_reg_write(tm->dev_p, tm->cmd, cmd | EDT_SSTM_CMD_LOCK);

    edt_reg_write(tm->dev_p, tm->cmd, cmd | bits);
}

/*
*
* attempt to find the delta between the zero crossing and setting the 
* strobe register value, to offset the wait to be as close to 
* zero as possible 
* uses ZERO_OFFSET_MAX check to avoid occasional snoozes
*
*/

static void
edt_sstm_calibrate_wait(EdtTimeController *tm)

{

    int i1, i2;

    if (wait_zero_calibrated)
        return;

    do 
    {
        edt_wait_for_zero(0);

        i1 = edt_usecs();

        edt_sstm_strobe(tm, 0);

        i2  = edt_usecs();

        wait_zero_offset = (i1+i2)/2;
        printf("wait_zero_offset = (%d + %d) / 2 = %d\n", 
            i1, i2, wait_zero_offset);

    } while (wait_zero_offset > EDT_SSTM_ZERO_OFFSET_MAX);

}

void edt_sstm_latch_time(EdtTimeController *tm)

{
    edt_sstm_strobe(tm, EDT_SSTM_LATCH);
}

int edt_sstm_get_adjust_enabled(EdtTimeController *tm)

{
    int cfg = edt_reg_read(tm->dev_p, tm->cmd);

    if (cfg & EDT_SSTM_ADJ_EN)
        return 1;
    else
        return 0;

}

int edt_sstm_get_adjust_sign(EdtTimeController *tm)

{
    int cfg = edt_reg_read(tm->dev_p, tm->cmd);

    if (cfg & EDT_SSTM_ADJ_PLUS)
        return 1;
    else
        return -1;

}

u_int edt_sstm_get_adjust_ticks(EdtTimeController *tm)

{

    return edt_sstm_get_adjust_sign(tm) *
        edt_reg_read(tm->dev_p, tm->adj_value);
}


u_int edt_sstm_get_seconds(EdtTimeController *tm)

{
    return edt_reg_read(tm->dev_p, tm->secs);
}

u_int edt_sstm_get_counts(EdtTimeController *tm)

{
    return edt_reg_read(tm->dev_p, tm->counts) >> 12;
}

u_int 
edt_sstm_get_usecs(EdtTimeController *tm)

{
    u_int count = edt_sstm_get_counts(tm);

    return (u_int) (count * USECS_FACTOR);

}

void 
edt_sstm_get_time_parts(EdtTimeController *tm, u_int *seconds, u_int *usecs)

{
    edt_sstm_latch_time(tm);

    *seconds = edt_sstm_get_seconds(tm);
    *usecs = edt_sstm_get_usecs(tm);
}

double 
edt_sstm_timestamp(EdtTimeController *tm)

{

    edt_sstm_latch_time(tm);

    return (double) edt_sstm_get_seconds(tm) + 
        (0.000001) * (USECS_FACTOR) * edt_sstm_get_counts(tm);
}

void edt_sstm_set_secs(EdtTimeController *tm, unsigned int second)

{
    edt_reg_write(tm->dev_p, tm->set, second);
    edt_sstm_strobe(tm, EDT_SSTM_COPY);
}


void edt_sstm_set(EdtTimeController *tm, unsigned int second)

{
    printf("Setting seconds to %d\n", second);

    edt_reg_write(tm->dev_p, tm->set, second+1);
    edt_wait_for_zero(wait_zero_offset);
    edt_sstm_strobe(tm, EDT_SSTM_COPY);
}

void edt_sstm_set_to_sys(EdtTimeController *tm)

{
    time_t t;
    edt_sstm_calibrate_wait(tm);
    edt_wait_for_zero(0);
    edt_msleep(500);
    t = time(&t);
    edt_sstm_set(tm, (u_int) t);
}

void edt_sstm_set_to_sys_error(EdtTimeController *tm, int error)

{
    time_t t;
    edt_sstm_calibrate_wait(tm);
    edt_wait_for_zero(0);
    edt_msleep(500);
    t = time(&t);
    if (error < 0)
    {
        edt_reg_write(tm->dev_p, tm->set, (uint_t) (t + ( error / 1000 )) );
        edt_wait_for_zero(wait_zero_offset);

        edt_msleep(1000 - (abs(error) % 1000));
        edt_sstm_strobe(tm, EDT_SSTM_COPY);

    }
    else
    {

        edt_reg_write(tm->dev_p, tm->set, (uint_t) (t + 1 + error / 1000));
        edt_wait_for_zero(wait_zero_offset);

        edt_msleep(error % 1000);

        edt_sstm_strobe(tm, EDT_SSTM_COPY);

    }
}

void edt_sstm_disable_adjust(EdtTimeController *tm)

{
    edt_reg_and(tm->dev_p, tm->cmd, ~EDT_SSTM_ADJ_EN);
}

void edt_sstm_enable_adjust(EdtTimeController *tm)

{
    edt_reg_or(tm->dev_p, tm->cmd,  EDT_SSTM_ADJ_EN );
}

void edt_sstm_set_adj_sign(EdtTimeController *tm, int positive)

{
    edt_reg_and(tm->dev_p, tm->cmd, ~EDT_SSTM_ADJ_PLUS);
    edt_reg_or(tm->dev_p, tm->cmd,  ((positive)?EDT_SSTM_ADJ_PLUS:0));
}

int edt_sstm_get_adj_sign(EdtTimeController *tm)

{
    edt_reg_and(tm->dev_p, tm->cmd, ~EDT_SSTM_ADJ_PLUS);
    return edt_reg_read(tm->dev_p, tm->cmd) & EDT_SSTM_ADJ_PLUS;
}

void edt_sstm_set_adj_ticks(EdtTimeController *tm, int ticks, int positive)

{
    edt_reg_write(tm->dev_p, tm->set, abs(ticks));

    edt_sstm_disable_adjust(tm);
    edt_sstm_strobe(tm, EDT_SSTM_COPY_ADJ );
    edt_sstm_set_adj_sign(tm, positive);
    edt_sstm_enable_adjust(tm);

    tm->ticks = ticks;

    printf("set adj to %d\n", ticks);
}

double edt_sstm_sys_error(EdtTimeController *tm)

{
    double t1, t2, st1;
    t1 = edt_sstm_timestamp(tm);
    st1 = edt_sys_timestamp();
    t2 = edt_sstm_timestamp(tm);
    return ((t1 + t2) * 0.5) - st1;
}

char *
edt_printable_time(double sys_t, char *buf)

{
    time_t t;
    struct tm *ltime;
    int usecs;
    double whole;
    static char sbuf[17];
    char *cp = buf;

    if (!cp)
        cp = sbuf;

    t = (time_t) sys_t;

    ltime = localtime(&t);

    if (!ltime)
        return buf;

    usecs = (int) (modf(sys_t,&whole) * 1000000.0);

    sprintf(cp, "%02d:%02d:%02d.%06d",
        ltime->tm_hour, ltime->tm_min, ltime->tm_sec, usecs);

    return cp;
}


double
edt_sstm_print(EdtTimeController *tm)

{
    double error, sys_t;
    char printbuf[32];

    sys_t = edt_sys_timestamp();

    error = edt_sstm_sys_error(tm);     

    printf("%s    %.6f\n", edt_printable_time(sys_t, printbuf),error);

    return error;

}


static double
double_mean(double *v, int n)

{
    double sum = 0.0;
    int i;

    for (i=0;i<n;i++)
        sum += v[i];

    return (sum / (double) n);
}

static int
double_compare(const void *p1, const void *p2)

{
    double d1 = *(double *) p1;
    double d2 = *(double *) p2;

    return (d1 > d2)?1:((d1 == d2)?0:-1);
}



static int adj_sample_seconds = 3;
static int adj_samples = 10;

void edt_sstm_set_drift_sampling(int seconds, int samples)

{
    adj_sample_seconds = seconds;
    adj_samples = samples;
}

int edt_sstm_get_adj_sample_secs()

{
    return adj_sample_seconds;
}

int edt_sstm_get_adj_samples()

{
    return adj_samples;
}



int edt_sstm_ticks_from_drift(double drift)

{
    double t;

    if (drift == 0.0)
        return 0;

    t = USECS_FACTOR;

    t = (int) (1000000.0 / drift);

    return (int) t;
}

void edt_sstm_set_adj_from_drift(EdtTimeController *tm, double drift)

{
    int ticks = (int) edt_sstm_ticks_from_drift(drift);

    edt_sstm_set_adj_ticks(tm, ticks, (drift >= 0.0));


}

double edt_sstm_measure_drift(EdtTimeController *tm)

{

    double t1 = 0, t2 = 0, st1 = 0, st2 = 0;

    double *d1_v = NULL, *d2_v = NULL, *st1_v = NULL, *st2_v = NULL;

    double d1, d2;
    double drift;
    double extraticks;

    int i;
    int count;
    int adj_state = edt_reg_read(tm->dev_p, tm->cmd) & EDT_SSTM_ADJ_EN;
    int sleep_interval = adj_sample_seconds * 1000000;

    edt_sstm_timestamp(tm);

    edt_sstm_disable_adjust(tm);


    d1_v = (double *) calloc(2*adj_samples +1 , sizeof(double));
    d2_v = (double *) calloc(2*adj_samples +1 , sizeof(double));
    st1_v = (double *) calloc(2*adj_samples +1 , sizeof(double));
    st2_v = (double *) calloc(2*adj_samples +1 , sizeof(double));


    printf("Sampling basic error rate ...\n");
    edt_usleep(100);

    t1 = 0;
    st1 = 0;
    t2 = 0;
    st2 = 0;
    d1 = 0;
    d2 = 0;


    for (i=0;i<adj_samples;i++)
    {     
        t1 = edt_sstm_timestamp(tm);
        st1 = edt_sys_timestamp();
        t2 = edt_sstm_timestamp(tm);
        d1_v[i] =  st1 - ((t1 + t2) * 0.5);
        st1_v[i] = st1;
    }

    edt_usleep(sleep_interval);


    count = 0;

    for (i=0;i<adj_samples;i++)
    {
        t1 = edt_sstm_timestamp(tm);
        st1 = edt_sys_timestamp();
        t2 = edt_sstm_timestamp(tm);
        d2_v[i] =  st1 - ((t1 + t2) * 0.5);
        st2_v[i] = st1;
    }

    /* calc means, remove outliers, then recalc */
    /* first sort arrays */
    qsort(d1_v, adj_samples, sizeof(double), double_compare);
    qsort(d2_v, adj_samples, sizeof(double), double_compare);
    qsort(st1_v, adj_samples, sizeof(double), double_compare);
    qsort(st2_v, adj_samples, sizeof(double), double_compare);

    /* find mean of each array */
    /* toss out high and low */

    d1 = double_mean(d1_v + 1, (adj_samples-2));
    d2 = double_mean(d2_v + 1, (adj_samples-2));
    st1 = double_mean(st1_v + 1, adj_samples-2);
    st2 = double_mean(st2_v + 1, adj_samples-2);


    d1 *= 1000000.0;
    d2 *= 1000000.0;

    drift = (d2 - d1) / (st2 - st1);


#ifdef VERBOSE

    printf("d1 = %.6f d2 = %.6f\n",     d1, d2);
    printf("st1 = %.6f st2 = %.6f\n",   st1, st2);

    for (i=0;i<adj_samples*2;i++)
    {
        printf("d1_v[%d] = %.5f d2_v[%d] = %.6f\n",
            i, d1_v[i], i, d2_v[i]);
    }

    for (i=0;i<adj_samples;i++)
    {
        printf("st1_v[%d] = %.5f st2_v[%d] = %.6f\n",
            i, st1_v[i], i, st2_v[i]);
    }
#endif

    extraticks = edt_sstm_ticks_from_drift(drift);

    printf("drift in %.0f secs = %.1f ticks = %.0f\n", 
        (st2 - st1), 
        drift, 
        extraticks);

    /* turn asjust back to start value */
    if (adj_state)
        edt_reg_or(tm->dev_p, tm->cmd, EDT_SSTM_ADJ_EN);

    free(d1_v);
    free(d2_v);
    free(st1_v);
    free(st2_v);

    return drift;
}

THREAD_FUNC_DECLARE 
edt_sstm_adjust_thread(void *p)

{
    EdtTimeController *tm = (EdtTimeController *) p;

    double max_err;
    int ticks;
    int sign;
    int delta;
    
    double last_error;
    double drift_values[5];
    int nvalues = 0;
    int start_ticks;

    if (tm == NULL)
        return (THREAD_RETURN) -1;

    max_err = tm->max_error * 0.000001;

    tm->done = 0;

    tm->err = edt_sstm_sys_error(tm);

    /* we assume we're close to the right adjustment */

    start_ticks = edt_sstm_get_adjust_ticks(tm);

    while (tm->active)
    {

        last_error = tm->err;


        tm->err = edt_sstm_sys_error(tm);

        drift_values[nvalues % 5] = tm->err - last_error;
        nvalues++;
        
        tm->adjust_drift =  double_mean(drift_values, (nvalues > 5)?5:nvalues);

        ticks = edt_sstm_get_adjust_ticks(tm);
        sign = (edt_sstm_get_adjust_sign(tm) > 0);

        delta = (int)(-tm->err * 1000000 * tm->adjustment_scalar);


        if (sign)
            ticks = start_ticks + delta;
        else
            ticks = start_ticks - delta;
        printf("adjusting drift = %f err = %f delta = %d\n", tm->adjust_drift, tm->err, delta);

        edt_sstm_set_adj_ticks(tm, ticks, sign);
        edt_sstm_enable_adjust(tm);

        
        edt_msleep(tm->adjust_sample);

    }

    tm->done = 1;

    return (THREAD_RETURN) 0;
}


EdtTimeController *
edt_sstm_launch_adjuster(EdtTimeController *tm)

{

#ifdef __APPLE__
    mac_thread_t thread;
#else
    thread_t thread;
#endif

    LaunchThread(thread, edt_sstm_adjust_thread, (void *) tm);
    tm->thread = thread;

    return tm;

}

void
edt_sstm_adjuster_stop(EdtTimeController *tm)

{
    if (!tm->active)
        return;

    tm->active = 0;
    while (!tm->done)
        edt_msleep(100);
}

void
edt_sstm_adjuster_start(EdtTimeController *tm)
{
#ifdef __APPLE__
    mac_thread_t thread;
#else
    thread_t thread;
#endif
    int ticks;

    if (tm->active)
        return;

    /* flags for terminatting sample thread */

    tm->done = 0;
    tm->active = TRUE;

   /* calculate drift */

    tm->drift = edt_sstm_measure_drift(tm);
    ticks = edt_sstm_ticks_from_drift(tm->drift);
    edt_sstm_set_adj_ticks(tm, ticks, (ticks >= 0.0));
    
    /* set to system time */
        
    edt_sstm_set_to_sys(tm);

    LaunchThread(thread, edt_sstm_adjust_thread, (void *) tm);
    tm->thread = thread;

}

EDTAPI void edt_init_time_controller(EdtTimeController *tm,
                                     EdtDev *edt_p,
                                     u_int address_base)

{
    tm->dev_p = edt_p;
    tm->counts = address_base + (EDT_SSTM_COUNTS & 0xf0000ff);
    tm->secs = address_base + (EDT_SSTM_SECS & 0xf0000ff);
    tm->set = address_base + (EDT_SSTM_SET & 0xf0000ff);
    tm->adj_value = address_base + (EDT_SSTM_ADJ_VALUE & 0xf0000ff);
    tm->cmd = address_base + (EDT_SSTM_CMD & 0xf0000ff);
}

---