Revision 0:362932d519c6, committed 2010-12-03

Comitter:

the729

Date:

Fri Dec 03 20:53:52 2010 +0000

Commit message:

Changed in this revision

diff -r 000000000000 -r 362932d519c6 hdtimeval_math.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/hdtimeval_math.cpp	Fri Dec 03 20:53:52 2010 +0000
@@ -0,0 +1,67 @@
+#include "hdtimeval_math.h"
+
+void hdtv_add (hdtimeval_t * r, hdtimeval_t * v1, hdtimeval_t * v2)
+{
+    r->tv_sec = v1->tv_sec + v2->tv_sec;
+    r->ticks = v1->ticks + v2->ticks;
+    if (r->ticks >= MAX_TICK) {
+        r->tv_sec++;
+        r->ticks -= MAX_TICK;
+    }
+}
+
+void hdtv_div2 (hdtimeval_t * r, hdtimeval_t * v)
+{
+    if (v->tv_sec & 0x1) {
+        r->ticks = ((v->ticks+MAX_TICK)>>1);
+    } else {
+        r->ticks = (v->ticks>>1);
+    }
+    r->tv_sec = (v->tv_sec>>1);
+}
+
+void hdtv_div8 (hdtimeval_t * r, hdtimeval_t * v)
+{
+    r->ticks = ((v->ticks+MAX_TICK*(v->tv_sec & 0x7))>>3);
+    r->tv_sec = (v->tv_sec>>3);
+}
+
+void hdtv_sub (hdtimeval_t * r, hdtimeval_t * v1, hdtimeval_t * v2)
+{
+    r->tv_sec = v1->tv_sec - v2->tv_sec;
+    if (v1->ticks < v2->ticks) {
+        r->ticks = MAX_TICK + v1->ticks - v2->ticks;
+        r->tv_sec --;
+    } else {
+        r->ticks = v1->ticks - v2->ticks;
+    }
+}
+
+void hdtv_muldiv (hdtimeval_t * r, hdtimeval_t * v, hdtimeval_t * m, hdtimeval_t * d)
+{
+    double dblm, dbld, factor, rsec, rticks;
+    uint32_t irticks, c;
+    int32_t irsec;
+
+    dblm = (double)(m->tv_sec) * (double)MAX_TICK + (double)(m->ticks);
+    dbld = (double)(d->tv_sec) * (double)MAX_TICK + (double)(d->ticks);
+    factor = dblm / dbld;
+    
+    rsec = (double)(v->tv_sec) * factor;
+    rticks = (double)(v->ticks) * factor + (rsec - floor(rsec)) * (double)MAX_TICK;
+    irticks = (uint32_t)rticks;
+    irsec = (int32_t)rsec;
+    c = irticks / MAX_TICK;
+    r->tv_sec = irsec + c;
+    r->ticks = irticks - c * MAX_TICK;
+}
+
+void hdtv_totv(timeval_t * r, hdtimeval_t * v)
+{
+    r->tv_sec = v->tv_sec;
+    r->tv_usec = (v->ticks + 48)/96;
+    if (r->tv_usec >= 1000000) {
+        r->tv_sec ++;
+        r->tv_usec -= 1000000;
+    }
+}

diff -r 000000000000 -r 362932d519c6 hdtimeval_math.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/hdtimeval_math.h	Fri Dec 03 20:53:52 2010 +0000
@@ -0,0 +1,13 @@
+#ifndef HDTIMEVAL_MATH_H
+#define HDTIMEVAL_MATH_H
+
+#include "timesync.h"
+
+void hdtv_add (hdtimeval_t * r, hdtimeval_t * v1, hdtimeval_t * v2);
+void hdtv_div2 (hdtimeval_t * r, hdtimeval_t * v);
+void hdtv_div8 (hdtimeval_t * r, hdtimeval_t * v);
+void hdtv_sub (hdtimeval_t * r, hdtimeval_t * v1, hdtimeval_t * v2);
+void hdtv_muldiv (hdtimeval_t * r, hdtimeval_t * v, hdtimeval_t * m, hdtimeval_t * d);
+void hdtv_totv(timeval_t * r, hdtimeval_t * v);
+
+#endif //HDTIMEVAL_MATH_H

diff -r 000000000000 -r 362932d519c6 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Dec 03 20:53:52 2010 +0000
@@ -0,0 +1,27 @@
+#include "mbed.h"
+#include "timesync.h"
+
+DigitalOut myled(LED2);
+DigitalOut mypin(p21);
+
+timeval_t t;
+
+void pinToggle()
+{
+    mypin = 1;
+    myled = 1;
+    mypin = 0;
+    myled = 0;
+    t.tv_sec += 1;
+    runAtTime(&pinToggle, &t);
+}
+
+int main() {
+    timesync_init();
+    
+    t.tv_sec = 5;
+    t.tv_usec = 500000;
+    runAtTime(&pinToggle, &t);
+    while(1) {
+    }
+}

diff -r 000000000000 -r 362932d519c6 mbed.bld
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Fri Dec 03 20:53:52 2010 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/e2ac27c8e93e

diff -r 000000000000 -r 362932d519c6 timesync.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/timesync.h	Fri Dec 03 20:53:52 2010 +0000
@@ -0,0 +1,76 @@
+#ifndef TIMESYNC_H
+#define TIMESYNC_H
+
+#include "mbed.h"
+
+typedef unsigned int uint32_t;
+typedef int int32_t;
+typedef unsigned short uint16_t;
+typedef short int16_t;
+typedef unsigned char uint8_t;
+//typedef char int8_t;
+
+typedef struct timeval {
+    int32_t tv_sec;
+    uint32_t tv_usec;
+} timeval_t;
+
+typedef struct hdtimeval {
+    int32_t tv_sec;
+    uint32_t ticks;
+} hdtimeval_t;
+
+typedef union sync_pkt24 {
+    struct {
+        uint32_t rx_time;
+        uint16_t tx_time;
+    };
+    struct {
+        char no_use;
+        char raw[5];
+    };
+} sync_pkt24_t;
+
+typedef union sync_pkt32 {
+    struct {
+        uint32_t rx_time;
+        uint16_t tx_time;
+    };
+    char raw[6];
+} sync_pkt32_t;
+
+typedef union sync_pkt64 {
+    struct {
+        hdtimeval_t rx_time;
+        uint16_t tx_time;
+    };
+    char raw[10];
+} sync_pkt64_t;
+
+typedef enum {
+    SYNCTYPE_NONE = 0,
+    SYNCTYPE_64 = 0xA0,
+    SYNCTYPE_32 = 0xA3,
+    SYNCTYPE_24 = 0xA5
+} synctype_t;
+
+void timesync_init();
+
+void getTime(struct timeval * tv);
+int runAtTime(void (*schedFunc)(void), struct timeval *tv);
+void runAtTrigger(void (*trigFunc)(struct timeval *tv));
+
+#define TMR LPC_TIM2
+#define MAX_TICK (SystemCoreClock)
+
+#define PCLK_DEVIDER 1  // PCLK = CCLK
+#define CLK_TIMER 12     // TIMER2 as the timer
+#define CLK_UART 16     // UART2 as uart
+#define PCTIM0 1        // Power Control Timer0
+#define PCTIM1 2        // Power Control Timer1
+#define PCTIM2 22        // Power Control Timer1
+#define PCUART2 24      // Power Control UART2
+
+#define QUEUE_SIZE 16
+
+#endif //TIMESYNC_H

diff -r 000000000000 -r 362932d519c6 timesync_slave.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/timesync_slave.cpp	Fri Dec 03 20:53:52 2010 +0000
@@ -0,0 +1,456 @@
+#include "mbed.h"
+#include "timesync.h"
+#include "timesync_slave.h"
+#include "hdtimeval_math.h"
+
+int32_t clock_sec;
+DigitalOut led(LED1);
+Serial uart_sync(p28,p27);
+Timeout tmr_sync;
+Timeout tmr_callback;
+
+hdtimeval_t front_lt;
+hdtimeval_t timeq[QUEUE_SIZE];
+void (*funcq[QUEUE_SIZE])(void);
+uint8_t qfront;
+uint8_t qtail;
+
+void (*trigf)(struct timeval *);
+hdtimeval_t trigt;
+//bool trigactive;
+
+//state_t state;
+synctype_t syncbusy;
+synctype_t tmp_synctype;
+hdtimeval_t txstamp;
+
+coeff_t coeff;
+
+void clock_init();
+void tmr1_irq();
+void uart_recv();
+__INLINE void getHDTime(struct hdtimeval * hdtv);
+void sync_req();
+void update_data(synctype_t t, void * pkt_p, hdtimeval_t * rxstamp);
+void getLocalTime(hdtimeval * gt, hdtimeval * lt);
+void getGlobalTime(hdtimeval * lt, hdtimeval * gt);
+void runTriggerFunc();
+
+void timesync_init()
+{
+    wait_ms(100);
+    coeff.v1div2.tv_sec = 0;
+    coeff.v1div2.ticks = 0;
+    coeff.w1div2.tv_sec = 0;
+    coeff.w1div2.ticks = 0;
+    coeff.beta1.tv_sec = 0;
+    coeff.beta1.ticks = 1;
+    coeff.beta2.tv_sec = 0;
+    coeff.beta2.ticks = 1;
+    coeff.size = 0;
+    coeff.front = 0;
+    trigf = NULL;
+    
+    clock_init();
+    uart_sync.baud(115200);
+    uart_sync.attach(&uart_recv, Serial::RxIrq);
+
+    syncbusy = SYNCTYPE_NONE;
+    tmp_synctype = SYNCTYPE_64;
+    tmr_sync.attach(&sync_req, 0.1);
+}
+/*
+void timesync_poll()
+{
+    //static last_poll;
+    tmp_synctype = SYNCTYPE_24;
+    sync_req();
+}
+*/
+void clock_init()
+{
+    // select PCLK
+    LPC_SC->PCLKSEL1 = (LPC_SC->PCLKSEL1 & ~(3<<CLK_TIMER)) | (PCLK_DEVIDER<<CLK_TIMER);
+    // power on timer
+    LPC_SC->PCONP |= (1<<PCTIM2);
+    
+    // reset timer
+    TMR->TCR = 2;
+    TMR->CTCR = 0;
+    TMR->PR = 0;
+    TMR->TC = 1;
+    TMR->MR0 = SystemCoreClock - 1;
+    TMR->MCR = 3;
+    
+    clock_sec = 0;
+
+    // capture on CAP.0        
+    TMR->CCR = 0x07;
+    LPC_PINCON->PINSEL0 = LPC_PINCON->PINSEL0  | (3<<8);
+    
+    // setup interrupt
+    __enable_irq();
+    NVIC_SetVector(TIMER2_IRQn, (uint32_t)&tmr1_irq);
+    NVIC_EnableIRQ(TIMER2_IRQn);
+    
+    NVIC_SetPriority(TIMER2_IRQn, 0);
+    NVIC_SetPriority(UART2_IRQn, 1);
+    NVIC_SetPriority(TIMER3_IRQn, 2);
+    NVIC_SetPriority(UART0_IRQn, 2);
+    NVIC_SetPriority(UART3_IRQn, 1);
+
+    // begin freerun
+    TMR->TCR = 1;
+}
+
+void tmr1_irq()
+{
+    if (TMR->IR & (1<<4)) {
+        if (trigf) {
+            trigt.tv_sec = clock_sec;
+            if ((TMR->IR & 0x01) && TMR->CR0 < 256)
+                trigt.tv_sec = clock_sec+1;
+            trigt.ticks = TMR->CR0;
+            //runTriggerFunc();
+            tmr_callback.attach_us(&runTriggerFunc, 10);
+        }
+        TMR->IR = (1<<4);
+    }
+    if (TMR->IR & 0x1) {
+        // Overflow!
+        clock_sec ++;
+        TMR->IR = 0x1;
+    }
+    if (TMR->IR & 0x2) {
+        if (clock_sec == front_lt.tv_sec) {
+            hdtimeval_t * nowtv = timeq+qfront;
+            funcq[qfront]();
+            qfront = (qfront+1)%QUEUE_SIZE;
+            while(qfront != qtail 
+            && nowtv->tv_sec == timeq[qfront].tv_sec
+            && nowtv->ticks == timeq[qfront].ticks) {
+                funcq[qfront]();
+                qfront = (qfront+1)%QUEUE_SIZE;
+            }
+            if (qfront == qtail) {
+                TMR->MCR &= ~(1<<3);
+            } else {
+                getLocalTime(&timeq[qfront], &front_lt);
+                TMR->MR1 = front_lt.ticks;
+            }
+        } else if (clock_sec > front_lt.tv_sec) {
+            // we missed it!
+            qfront = (qfront+1)%QUEUE_SIZE;
+            if (qfront == qtail) {
+                TMR->MCR &= ~(1<<3);
+            } else {
+                getLocalTime(&timeq[qfront], &front_lt);
+                TMR->MR1 = front_lt.ticks;
+            }
+        }
+        TMR->IR = 0x2;
+    }
+}
+
+void runTriggerFunc()
+{
+    timeval_t t;
+    getGlobalTime(&trigt, &trigt);
+    hdtv_totv(&t, &trigt);
+    trigf(&t);
+}
+
+void uart_recv()
+{
+    hdtimeval_t tstamp;
+    getHDTime(&tstamp);
+    
+    //if (!(LPC_UART2->IIR & 0x01) && (LPC_UART2->LSR & 0x01)) {
+    if (uart_sync.readable()) {
+        sync_pkt64_t pkt64;
+        sync_pkt32_t pkt32;
+        sync_pkt24_t pkt24;
+        char * data = NULL;
+        void * pkt_p = NULL;
+        uint8_t len = 0;
+        
+        if (!syncbusy) {
+            len = uart_sync.getc();
+            return;
+        }
+        
+        switch(syncbusy) {
+            case SYNCTYPE_64:
+                data = pkt64.raw;
+                pkt_p = (void *)&pkt64;
+                len = 10;
+                break;
+            case SYNCTYPE_32:
+                data = pkt32.raw;
+                pkt_p = (void *)&pkt32;
+                len = 6;
+                break;
+            case SYNCTYPE_24:
+                pkt24.no_use = 0;
+                pkt_p = (void *)&pkt24;
+                data = pkt24.raw;
+                len = 5;
+                break;
+        }
+        while(len-->0) {
+            //while(!(LPC_UART2->LSR & 0x01));
+            *(data++)= uart_sync.getc(); //LPC_UART2->RBR;
+        }
+        
+        update_data(syncbusy, pkt_p, &tstamp);
+    }
+    syncbusy = SYNCTYPE_NONE;
+}
+
+void sync_req()
+{
+    //if (syncbusy) return;
+    __disable_irq();
+    txstamp.tv_sec = clock_sec;
+    txstamp.ticks = TMR->TC;
+    uart_sync.putc(tmp_synctype);
+    //LPC_UART2->THR = tmp_synctype;
+    __enable_irq();
+    syncbusy = tmp_synctype;
+}
+
+void update_data(synctype_t t, void * pkt_p, hdtimeval_t * rxstamp)
+{
+    hdtimeval_t v, w;
+    hdtimeval_t master_rxt, master_txt;
+    int32_t ticks_diff;
+    hdtv_add(&v, &txstamp, rxstamp);
+    switch (t) {
+        case SYNCTYPE_64: {
+            sync_pkt64_t * p = (sync_pkt64_t *)pkt_p;
+            master_rxt = p->rx_time;
+            master_txt.tv_sec = master_rxt.tv_sec;
+            master_txt.ticks = (master_rxt.ticks & 0xFFFF0000) + p->tx_time;
+        } break;
+        case SYNCTYPE_32: {
+            sync_pkt32_t * p = (sync_pkt32_t *)pkt_p;
+            hdtimeval_t tmp;
+            getGlobalTime(&txstamp, &tmp);
+            if (tmp.ticks > MAX_TICK/2) {
+                if (p->rx_time > tmp.ticks-MAX_TICK/2) {
+                    master_rxt.tv_sec = tmp.tv_sec;
+                } else {
+                    master_rxt.tv_sec = tmp.tv_sec +1;
+                }
+            } else {
+                if (p->rx_time > tmp.ticks+MAX_TICK/2) {
+                    master_rxt.tv_sec = tmp.tv_sec-1;
+                } else {
+                    master_rxt.tv_sec = tmp.tv_sec;
+                }
+            }
+            master_rxt.ticks = p->rx_time;
+            master_txt.tv_sec = master_rxt.tv_sec;
+            master_txt.ticks = (master_rxt.ticks & 0xFFFF0000) + p->tx_time;
+        } break;
+        case SYNCTYPE_24: {
+            sync_pkt24_t * p = (sync_pkt24_t *)pkt_p;
+            hdtimeval_t tmp;
+            getGlobalTime(&txstamp, &tmp);
+            p->rx_time >>= 8;
+            if (tmp.ticks > (1<<23)) {
+                tmp.ticks = tmp.ticks-(1<<23);
+                master_rxt.ticks = (tmp.ticks & 0xFF000000) + p->rx_time;
+                master_rxt.tv_sec = tmp.tv_sec;
+            } else {
+                tmp.ticks = tmp.ticks+MAX_TICK-(1<<23);
+                master_rxt.ticks = (tmp.ticks & 0xFF000000) + p->rx_time;
+                master_rxt.tv_sec = tmp.tv_sec-1;
+            }
+            if (master_rxt.ticks < tmp.ticks) {
+                master_rxt.ticks += 0x1000000;
+                if (master_rxt.ticks > MAX_TICK) {
+                    master_rxt.ticks &= 0xFFFFFF;
+                    master_rxt.tv_sec ++;
+                }
+            }
+            master_txt.tv_sec = master_rxt.tv_sec;
+            master_txt.ticks = (master_rxt.ticks & 0xFFFF0000) + p->tx_time;            
+        } break;
+    }
+    //printf("t1,t2: %d,%d, %d,%d \r\n",master_rxt.tv_sec, master_rxt.ticks>>24, master_rxt.ticks & 0xFFFFFF, master_txt.ticks);
+    if (master_rxt.ticks > master_txt.ticks) {
+        master_txt.ticks += 0x10000;
+        if (master_txt.ticks >= MAX_TICK) {
+            master_txt.ticks &= 0xFFFF;
+            master_txt.tv_sec ++;
+        }
+    }
+    hdtv_add(&w, &master_rxt, &master_txt);
+
+    hdtv_div2(&v, &v);
+    hdtv_div2(&w, &w);
+    // evaluate performance
+    {
+        hdtimeval_t tmp;
+        getGlobalTime(&v, &tmp);
+        hdtv_sub(&tmp, &tmp, &w);
+        if (tmp.tv_sec > 10) 
+            ticks_diff = 0x7FFFFFFF;
+        else {
+            ticks_diff = tmp.tv_sec * MAX_TICK + tmp.ticks;
+        }
+        //printf("perf: %d \r\n", ticks_diff);
+    }
+    if (coeff.size)
+        if (coeff.size<HISTORY_LEN) {
+            hdtv_sub(&coeff.beta1, &w, &(coeff.hisw[0]));
+            hdtv_sub(&coeff.beta2, &v, &(coeff.hisv[0]));
+        } else {
+            hdtv_sub(&coeff.beta1, &w, &(coeff.hisw[coeff.front]));
+            hdtv_sub(&coeff.beta2, &v, &(coeff.hisv[coeff.front]));
+        }
+    /*if(coeff.size>=8) {
+        hdtimeval_t tmp;
+        hdtv_div8(&tmp, &(coeff.hisw[coeff.front]));
+        hdtv_sub(&coeff.w1div2, &coeff.w1div2, &tmp);
+        hdtv_div8(&tmp, &(coeff.hisv[coeff.front]));
+        hdtv_sub(&coeff.v1div2, &coeff.v1div2, &tmp);
+        hdtv_div8(&tmp, &w);
+        hdtv_add(&coeff.w1div2, &coeff.w1div2, &tmp);
+        hdtv_div8(&tmp, &v);
+        hdtv_add(&coeff.v1div2, &coeff.v1div2, &tmp);
+    } else {*/
+        coeff.v1div2 = v;
+        coeff.w1div2 = w;
+    //}
+    coeff.hisw[coeff.front] = w;
+    coeff.hisv[coeff.front] = v;
+    coeff.front = (coeff.front+1) % HISTORY_LEN;
+    if (coeff.size<=HISTORY_LEN) coeff.size++;
+    /*if (coeff.size==8) {
+        hdtimeval_t tmp;
+        coeff.w1div2.tv_sec = 0;
+        coeff.w1div2.ticks = 0;
+        coeff.v1div2.tv_sec = 0;
+        coeff.v1div2.ticks = 0;
+        for (uint8_t i=0; i<8; i++) {
+            hdtv_div8(&tmp, &(coeff.hisw[i]));
+            hdtv_add(&coeff.w1div2, &coeff.w1div2, &tmp);
+            hdtv_div8(&tmp, &(coeff.hisv[i]));
+            hdtv_add(&coeff.v1div2, &coeff.v1div2, &tmp);
+        }
+        printf("updated!\r\n");
+    }*/
+    //printf("w/v: %d, %d \r\n", coeff.w1div2.ticks, coeff.v1div2.ticks);
+    if (qfront != qtail) {
+        getLocalTime(&timeq[qfront], &front_lt);
+        TMR->MR1 = front_lt.ticks;
+        TMR->MCR |= (1<<3);
+    }
+    // schedule next sync
+    ticks_diff = abs(ticks_diff);
+    if (ticks_diff < 200 && coeff.size>8) {
+        tmp_synctype = SYNCTYPE_24;
+        tmr_sync.attach(&sync_req, 60);
+    } else if (ticks_diff < 500 && coeff.size>4) {
+        tmp_synctype = SYNCTYPE_24;
+        tmr_sync.attach(&sync_req, 30);
+    } else if (ticks_diff < 800 && coeff.size>2) {
+        tmp_synctype = SYNCTYPE_24;
+        tmr_sync.attach(&sync_req, 20);
+    } else if (ticks_diff < 5000) {
+        tmp_synctype = SYNCTYPE_32;
+        tmr_sync.attach(&sync_req, 10);
+    } else {
+        tmp_synctype = SYNCTYPE_64;
+        tmr_sync.attach(&sync_req, 5);
+    }
+    led = !led;
+}
+
+__INLINE void getHDTime(struct hdtimeval * hdtv)
+{
+    __disable_irq();
+    hdtv->tv_sec = clock_sec;
+    hdtv->ticks = TMR->TC;
+    __enable_irq();
+    return;
+}
+
+void getTime(struct timeval * tv)
+{
+    hdtimeval_t tstamp, gstamp, tstamp1; 
+    static hdtimeval_t comp_delay = {0,0};
+
+    getHDTime(&tstamp);
+    
+    hdtv_add(&gstamp, &tstamp, &comp_delay);
+    getGlobalTime(&gstamp, &gstamp);
+    
+    getHDTime(&tstamp1);
+    hdtv_sub(&comp_delay, &tstamp1, &tstamp);
+    //printf("delay: %d\r\n", comp_delay.ticks);
+
+    tv->tv_sec = gstamp.tv_sec;
+    tv->tv_usec = ((gstamp.ticks+48) >> 5) / 3;
+    return; 
+}
+
+void getGlobalTime(hdtimeval * lt, hdtimeval * gt)
+{
+    hdtimeval_t tmp;
+    hdtv_sub(&tmp, lt, &coeff.v1div2);
+    hdtv_muldiv(&tmp, &tmp, &coeff.beta1, &coeff.beta2);
+    hdtv_add(gt, &tmp, &coeff.w1div2);
+}
+
+void getLocalTime(hdtimeval * gt, hdtimeval * lt)
+{
+    hdtimeval_t tmp;
+    hdtv_sub(&tmp, gt, &coeff.w1div2);
+    hdtv_muldiv(&tmp, &tmp, &coeff.beta2, &coeff.beta1);
+    hdtv_add(lt, &tmp, &coeff.v1div2);
+}
+
+int enqueue(void (*schedFunc)(void), hdtimeval_t *tv)
+{
+    uint8_t p = qtail;
+    if ((qtail+1)%QUEUE_SIZE == qfront) {
+        return 1;
+    }
+    while(p != qfront) {
+        uint8_t prev = (p+QUEUE_SIZE-1)%QUEUE_SIZE;
+        if (tv->tv_sec > timeq[prev].tv_sec 
+        || (tv->tv_sec == timeq[prev].tv_sec && tv->ticks >= timeq[prev].ticks)) break;
+        timeq[p] = timeq[prev];
+        funcq[p] = funcq[prev];
+        p = prev;
+    } 
+    timeq[p] = *tv;
+    funcq[p] = schedFunc;
+    qtail = (qtail+1)%QUEUE_SIZE;
+    return 0;
+}
+
+int runAtTime(void (*schedFunc)(void), struct timeval *tv)
+{
+    hdtimeval_t hdtv;
+    int ret;
+    hdtv.tv_sec = tv->tv_sec;
+    hdtv.ticks = tv->tv_usec * 96;
+    //getLocalTime(&hdtv, &hdtv);
+    ret = enqueue(schedFunc, &hdtv);
+    if (qfront != qtail) {
+        getLocalTime(&timeq[qfront], &front_lt);
+        TMR->MR1 = front_lt.ticks;
+        TMR->MCR |= (1<<3);
+    }
+    return ret;
+}
+
+
+void runAtTrigger(void (*trigFunc)(struct timeval *tv))
+{
+    trigf = trigFunc;
+}

diff -r 000000000000 -r 362932d519c6 timesync_slave.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/timesync_slave.h	Fri Dec 03 20:53:52 2010 +0000
@@ -0,0 +1,19 @@
+#ifndef TIMESYNC_SLAVE_H
+#define TIMESYNC_SLAVE_H
+
+#include "timesync.h"
+
+#define HISTORY_LEN 8
+
+typedef struct {
+    hdtimeval_t v1div2;
+    hdtimeval_t w1div2;
+    hdtimeval_t beta1;
+    hdtimeval_t beta2;
+    hdtimeval_t hisw[HISTORY_LEN];
+    hdtimeval_t hisv[HISTORY_LEN];
+    uint8_t front;
+    uint8_t size;
+} coeff_t;
+
+#endif //TIMESYNC_SLAVE_H