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

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

#define POLL_MARGIN 975

void 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;

}

#if 0

void edt_calibrate_usecs_mm()

{
    int i;
    int loops = 1;
    time_t t, t1;
    double sum = 0.0, dtsum = 0.0, whole, frac,dt;
    struct timeb tb1;
    int ms1, ms2;

    /* look for zero-crossing */
 
    /* uses timeGetTime at 1 ms accuracy */

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

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

        t1 = time(&t1);

        do {
            
            ms1 = timeGetTime();
            t = time(&t);
            ms2 = timeGetTime();

        } while (t == t1);

        /* average  */
        dt = (double) ms1 * 0.001;

        frac = modf(dt, &whole);
        printf("frac = %f\n", frac);
        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;

}

#endif

double
edt_sys_timestamp()

{
#ifdef USE_MM_TIMER
    int ms;
#endif

    double t;

    if (!usecs_calibrated)
        edt_calibrate_usecs();

#ifdef USE_MM_TIMER
    ms = timeGetTime();

    t = ((double) ms * 0.001);
#else
    t = edt_timestamp();
#endif

    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();

    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, (void *) 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_msleep(1000 - (usec/1000) - 100);
    while (usec < test && usec >= lastusec)
    {
        lastusec = usec;
        
        usec = edt_usecs();
    }
}

void 
edt_sstm_setup(EdtDev *edt_p, char *bitfile)

{

    if (bitfile)
        edt_bitload(edt_p, ".", bitfile, 0, 0);
    else 
        edt_bitload(edt_p, ".", "c3_demux.bit", 0, 0);
}

void edt_sstm_strobe(EdtDev *edt_p, unsigned int bits)

{
    int cmd;
    int count = 0;

    do
    {
        cmd = edt_reg_read(edt_p, EDT_SSTM_CMD);
        if (cmd & EDT_SSTM_CMD_LOCK)
            edt_msleep(0);
    } while (cmd & EDT_SSTM_CMD_LOCK && (count++ < 5));

    
    cmd &= 0x70;
    
    edt_reg_write(edt_p, EDT_SSTM_CMD, cmd | EDT_SSTM_CMD_LOCK);

    edt_reg_write(edt_p, EDT_SSTM_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(EdtDev *edt_p)

{

    int i1, i2;

    if (wait_zero_calibrated)
        return;

    do 
    {
        edt_wait_for_zero(0);

        i1 = edt_usecs();

        edt_sstm_strobe(edt_p, 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(EdtDev *edt_p)

{
    edt_sstm_strobe(edt_p, EDT_SSTM_LATCH);
}

int edt_sstm_get_adjust_enabled(EdtDev *edt_p)

{
    int cfg = edt_reg_read(edt_p, EDT_SSTM_CMD);

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

int edt_sstm_get_adjust_sign(EdtDev *edt_p)

{
    int cfg = edt_reg_read(edt_p, EDT_SSTM_CMD);

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

u_int edt_sstm_get_adjust_ticks(EdtDev *edt_p)

{
     
    return edt_sstm_get_adjust_sign(edt_p) *
            edt_reg_read(edt_p, EDT_SSTM_ADJ_VALUE);
}


u_int edt_sstm_get_seconds(EdtDev *edt_p)

{
    return edt_reg_read(edt_p, EDT_SSTM_SECS);
}

u_int edt_sstm_get_counts(EdtDev *edt_p)

{
    return edt_reg_read(edt_p, EDT_SSTM_COUNTS) >> 12;
}

u_int 
edt_sstm_get_usecs(EdtDev *edt_p)

{
    u_int count = edt_sstm_get_counts(edt_p);
    
    return (u_int) (count * USECS_FACTOR);
    
}

void 
edt_sstm_get_time_parts(EdtDev *edt_p, u_int *seconds, u_int *usecs)

{
    edt_sstm_latch_time(edt_p);

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

double 
edt_sstm_timestamp(EdtDev *edt_p)

{

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

void edt_sstm_set_secs(EdtDev *edt_p, unsigned int second)

{
    edt_reg_write(edt_p, EDT_SSTM_SET, second);
    edt_sstm_strobe(edt_p, EDT_SSTM_COPY);
}


void edt_sstm_set(EdtDev *edt_p, unsigned int second)

{
    edt_reg_write(edt_p, EDT_SSTM_SET, second+1);
    edt_wait_for_zero(wait_zero_offset);
    edt_sstm_strobe(edt_p, EDT_SSTM_COPY);
}

void edt_sstm_set_to_sys(EdtDev *edt_p)

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

void edt_sstm_set_to_sys_error(EdtDev *edt_p, int error)

{
    time_t t;
    edt_sstm_calibrate_wait(edt_p);
    edt_wait_for_zero(0);
    edt_msleep(500);
    t = time(&t);
    if (error < 0)
    {
        edt_reg_write(edt_p, EDT_SSTM_SET, (uint_t) (t + ( error / 1000 )) );
        edt_wait_for_zero(wait_zero_offset);

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

    }
    else
    {

        edt_reg_write(edt_p, EDT_SSTM_SET, (uint_t) (t + 1 + error / 1000));
        edt_wait_for_zero(wait_zero_offset);

        edt_msleep(error % 1000);

        edt_sstm_strobe(edt_p, EDT_SSTM_COPY);

    }
}

void edt_sstm_disable_adjust(EdtDev *edt_p)

{
    edt_reg_and(edt_p, EDT_SSTM_CMD, ~EDT_SSTM_ADJ_EN);
}

void edt_sstm_enable_adjust(EdtDev *edt_p)

{
    edt_reg_or(edt_p, EDT_SSTM_CMD,  EDT_SSTM_ADJ_EN );
}

void edt_sstm_set_adj_sign(EdtDev *edt_p, int positive)

{
    edt_reg_and(edt_p, EDT_SSTM_CMD, ~EDT_SSTM_ADJ_PLUS);
    edt_reg_or(edt_p, EDT_SSTM_CMD,  ((positive)?EDT_SSTM_ADJ_PLUS:0));
}

void edt_sstm_set_adj_ticks(EdtDev *edt_p, int ticks, int positive)

{
    edt_reg_write(edt_p, EDT_SSTM_SET, abs(ticks));
 
    edt_sstm_disable_adjust(edt_p);
    edt_sstm_strobe(edt_p, EDT_SSTM_COPY_ADJ );
    edt_sstm_set_adj_sign(edt_p, positive);
    edt_sstm_enable_adjust(edt_p);

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

double edt_sstm_sys_error(EdtDev *edt_p)

{
    double t1, t2, st1;
    t1 = edt_sstm_timestamp(edt_p);
    st1 = edt_sys_timestamp();
    t2 = edt_sstm_timestamp(edt_p);
    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(EdtDev *edt_p)

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

    sys_t = edt_sys_timestamp();
 
    error = edt_sstm_sys_error(edt_p);  

    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)

{
    int ticks;

    if (drift == 0.0)
        return EDT_SSTM_MAX_ADJ;

    ticks = (int) ((1000000.0 / drift) * USECS_FACTOR);

    return ticks;
}

void edt_sstm_set_adj_from_drift(EdtDev *edt_p, double drift)

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

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

   
}

double edt_sstm_measure_drift(EdtDev *edt_p)

{
    
    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(edt_p, EDT_SSTM_CMD) & EDT_SSTM_ADJ_EN;
    int sleep_interval = adj_sample_seconds * 1000000;

    edt_sstm_timestamp(edt_p);

    edt_sstm_disable_adjust(edt_p);


    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;

    t1 = edt_sstm_timestamp(edt_p);
    count = 0;

    for (i=0;i<adj_samples;i++)
    {
        st1 = edt_sys_timestamp();
        d1_v[count] = (st1 - t1);
        t1 = edt_sstm_timestamp(edt_p);
        d1_v[count+1] = (st1 - t1);
        count += 2;
        st1_v[i] = st1;
    }

    edt_usleep(sleep_interval);
            
    t2 = edt_sstm_timestamp(edt_p);
       
    count = 0;

    for (i=0;i<adj_samples;i++)
    {
        st2 = edt_sys_timestamp();
        d2_v[count] = (st2 - t2);
        t2 = edt_sstm_timestamp(edt_p);
        d2_v[count+1] = (st2 - t2);
        count += 2;
        st2_v[i] = st2;
    }
    
    /* calc means, remove outliers, then recalc */
    /* first sort arrays */
    qsort(d1_v, adj_samples * 2, sizeof(double), double_compare);
    qsort(d2_v, adj_samples * 2, sizeof(double), double_compare);
    qsort(st1_v, adj_samples, sizeof(double), double_compare);
    qsort(st2_v, adj_samples, sizeof(double), double_compare);

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

    d1 = double_mean(d1_v + 2, (adj_samples-2) * 2);
    d2 = double_mean(d2_v + 2, (adj_samples-2) * 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(edt_p, EDT_SSTM_CMD, EDT_SSTM_ADJ_EN);
        
    free(d1_v);
    free(d2_v);
    free(st1_v);
    free(st2_v);

    return drift;
}



double edt_sstm_calc_convergence(EdtDev *edt_p, 
                             int revert_secs,
                             double drift)

{
    double errordrift = 0.0;
    
    if (revert_secs)
    {
        /* find total error */

        double error = -edt_sstm_sys_error(edt_p);

        errordrift = (error * 1000000.0 / (double) revert_secs) + drift;

        printf("error = %.6f drift = %.0f converge with %.0f\n",
                error, drift, errordrift);
    }

    return errordrift;
}  

double edt_sstm_adjust(EdtDev *edt_p,
                              double converge, 
                              double drift,
                              int *active)
                            
{
    int sign,
        start_sign;
    double err;
    double lasterr;
    double convergecheck;
    int go = 1;
    int duration;
    int i;

    err = edt_sstm_sys_error(edt_p);
    sign = start_sign = err > 0;
    err = fabs(err);

    convergecheck = fabs(converge)*0.000001;

    duration = (int) (err/convergecheck) * 5;

    printf("duration = %d\n", duration);

    edt_sstm_set_adj_from_drift(edt_p, converge);

    for (i=0;i<duration  && 
        (err > convergecheck) && go && (sign == start_sign);i++)
    {

        edt_msleep(200);
        lasterr = err;
        err = edt_sstm_sys_error(edt_p);
        sign = err > 0;
        err = fabs(err);

        /* only check active flag if non-NULL */
        if (active)
            go = *active;
        
    } 

    printf("done with i = %d err = %.6f check = %.6f sign = %d\n",
            i, err, convergecheck, sign);

    edt_sstm_set_adj_from_drift(edt_p, drift);

    return err;
}

void edt_sstm_iterate_adjust(EdtDev *edt_p, 
                             int revert_secs,
                             int maxiterations,
                             int tolerance,
                             int *active)

{
    double converge, drift;
    int iter = 0;
    double err;
    double test = (double)tolerance * 0.000001;

    drift = edt_sstm_measure_drift(edt_p);

    do {

        converge = edt_sstm_calc_convergence(edt_p, revert_secs,drift);

        err = edt_sstm_adjust(edt_p, converge, drift, active);

        iter ++ ;
                
        printf("Loop %d error %.6f\n", iter, err);

    } while (err > test && iter < maxiterations);

}

THREAD_FUNC_DECLARE 
edt_sstm_adjust_thread(void *p)

{
    EdtSSTimeAdjuster *adj_p = (EdtSSTimeAdjuster *) p;

    int iterations;
    int count;
    double err;
    double max_err;
    int loop = 0;

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

    max_err = adj_p->max_error * 0.000001;
    iterations = adj_p->iterations;
    if (!iterations)
        iterations = 1;
    adj_p->done = 0;

    while (adj_p->active)
    {

        printf("Going to auto_adjust\n");

        edt_sstm_print(adj_p->edt_p);

        err = edt_sstm_sys_error(adj_p->edt_p);

        if (fabs(err) > max_err)
        {

            edt_sstm_iterate_adjust(adj_p->edt_p, 
                adj_p->revert_seconds, 
                adj_p->iterations,
                adj_p->tolerance, 
                &adj_p->active);

            printf("Adjusted\n");
            edt_sstm_print(adj_p->edt_p);

        }
        else
            printf("Within tolerance %d\n", adj_p->max_error);

        count = 0;


        if (adj_p->loops != 0)
        {
            loop ++;
            if (loop >= adj_p->loops)
                adj_p->active = FALSE;
        }

        /* sleep for 200 ms */

        while (adj_p->active && (count < adj_p->check_interval * 5))
        {
            edt_msleep(200);
            count ++;
        }

    }

    adj_p->done = 1;

    return (THREAD_RETURN) 0;
}


EdtSSTimeAdjuster *
edt_sstm_launch_adjuster(EdtDev *edt_p, 
                            int check, 
                            int revert, 
                            int iterations, 
                            int max_usec_error,
                            int tolerance,
                            int loops)

{
    EdtSSTimeAdjuster *adj_p;
    thread_t thread;

    adj_p = (EdtSSTimeAdjuster *) calloc(1, sizeof(EdtSSTimeAdjuster));

    adj_p->edt_p = edt_p;

    adj_p->check_interval = check; /* seconds */
    adj_p->revert_seconds= revert;
    adj_p->iterations = iterations;    /* how many times to auto adjust */
    adj_p->active = TRUE;
    adj_p->max_error = max_usec_error;
    adj_p->tolerance = tolerance;
    adj_p->loops = loops;


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

    return adj_p;

}

void
edt_sstm_adjuster_stop(EdtSSTimeAdjuster *adj_p)

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

void
edt_sstm_adjuster_start(EdtSSTimeAdjuster *adj_p)

{
    thread_t thread;
    if (!adj_p->done)
        return;

    adj_p->active = TRUE;

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

}

---