File content as of revision 0:988132ee7271:

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

//syncdata_t syncdata[2];
//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) {
        tmp_synctype = SYNCTYPE_24;
        tmr_sync.attach(&sync_req, 60);
    } else if (ticks_diff < 500) {
        tmp_synctype = SYNCTYPE_24;
        tmr_sync.attach(&sync_req, 30);
    } else if (ticks_diff < 800) {
        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;
}