Diff: timesync_master.cpp

Revision:

0:b9f06a7ae791

diff -r 000000000000 -r b9f06a7ae791 timesync_master.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/timesync_master.cpp	Fri Dec 03 20:53:21 2010 +0000
@@ -0,0 +1,239 @@
+#include "mbed.h"
+#include "timesync.h"
+#include "hdtimeval_math.h"
+
+int32_t clock_sec;
+DigitalOut led(LED1);
+Serial uart_sync(p28,p27);
+Timeout tmr_callback;
+
+hdtimeval_t timeq[QUEUE_SIZE];
+void (*funcq[QUEUE_SIZE])(void);
+uint8_t qfront;
+uint8_t qtail;
+
+uint32_t sync_count[3];
+
+void (*trigf)(struct timeval *);
+hdtimeval_t trigt;
+
+void clock_init();
+void tmr1_irq();
+void uart_recv();
+__INLINE void getHDTime(struct hdtimeval * hdtv);
+void runTriggerFunc();
+
+void timesync_init()
+{
+    sync_count[0]=0;
+    sync_count[1]=0;
+    sync_count[2]=0;
+
+    wait_ms(100);
+    clock_init();
+    uart_sync.baud(115200);
+    uart_sync.attach(&uart_recv, Serial::RxIrq);
+    
+    qfront = 0;
+    qtail = 0;
+    trigf = NULL;
+}
+
+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 = 0x05;
+    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 == timeq[qfront].tv_sec) {
+            uint32_t nowtick = timeq[qfront].ticks;
+            funcq[qfront]();
+            qfront = (qfront+1)%QUEUE_SIZE;
+            while(qfront != qtail 
+            && clock_sec == timeq[qfront].tv_sec
+            && nowtick == timeq[qfront].ticks) {
+                funcq[qfront]();
+                qfront = (qfront+1)%QUEUE_SIZE;
+            }
+            if (qfront == qtail) {
+                TMR->MCR &= ~(1<<3);
+            } else {
+                TMR->MR1 = timeq[qfront].ticks;
+            }
+        } else if (clock_sec > timeq[qfront].tv_sec) {
+            // we missed it!
+            printf("miss\r\n");
+            qfront = (qfront+1)%QUEUE_SIZE;
+            if (qfront == qtail) {
+                TMR->MCR &= ~(1<<3);
+            } else {
+                TMR->MR1 = timeq[qfront].ticks;
+            }
+        }
+        TMR->IR = 0x2;
+    }
+}
+
+void runTriggerFunc()
+{
+    timeval_t t;
+    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;
+        uint8_t len = 0;
+        char cmd;
+        
+        cmd = uart_sync.getc();
+        //cmd = LPC_UART2->RBR;
+        //printf("byte: %x \r\n", cmd);
+        switch (cmd) {
+            case SYNCTYPE_64: 
+                pkt64.rx_time = tstamp;
+                pkt64.tx_time = TMR->TC;
+                uart_sync.putc(pkt64.raw[0]);
+                len = 9;
+                data = pkt64.raw + 1;
+                sync_count[0]++;
+                break;
+            case SYNCTYPE_32: 
+                pkt32.rx_time = tstamp.ticks;
+                pkt32.tx_time = TMR->TC; 
+                uart_sync.putc(pkt32.raw[0]);
+                len = 5;
+                data = pkt32.raw + 1;
+                sync_count[1]++;
+                break;
+            case SYNCTYPE_24: 
+                pkt24.rx_time = tstamp.ticks << 8;
+                pkt24.tx_time = TMR->TC; 
+                uart_sync.putc(pkt24.raw[0]);
+                len = 4;
+                data = pkt24.raw + 1;
+                sync_count[2]++;
+                break;
+        }
+        while (len-->0) {
+            uart_sync.putc(*(data++));
+        }
+        //printf("rx_tick: %d,%d,%d \r\n", tstamp.tv_sec, tstamp.ticks>>24, tstamp.ticks & 0xFFFFFF);
+    }
+    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; 
+
+    getHDTime(&tstamp);
+    hdtv_totv(tv, &tstamp);
+    return; 
+}
+
+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;
+    ret = enqueue(schedFunc, &hdtv);
+    if (!ret && qtail != qfront) {
+        TMR->MR1 = timeq[qfront].ticks;
+        TMR->MCR |= (1<<3);
+    }
+    return ret;
+}
+
+void runAtTrigger(void (*trigFunc)(struct timeval *tv))
+{
+    trigf = trigFunc;
+}