syncMaster for problem 2

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers timesync_master.cpp Source File

timesync_master.cpp

00001 #include "mbed.h"
00002 #include "timesync.h"
00003 #include "hdtimeval_math.h"
00004 
00005 int32_t clock_sec;
00006 DigitalOut led(LED1);
00007 Serial uart_sync(p28,p27);
00008 Timeout tmr_callback;
00009 
00010 hdtimeval_t timeq[QUEUE_SIZE];
00011 void (*funcq[QUEUE_SIZE])(void);
00012 uint8_t qfront;
00013 uint8_t qtail;
00014 
00015 void (*trigf)(struct timeval *);
00016 hdtimeval_t trigt;
00017 
00018 void clock_init();
00019 void tmr1_irq();
00020 void uart_recv();
00021 __INLINE void getHDTime(struct hdtimeval * hdtv);
00022 void runTriggerFunc();
00023 
00024 void timesync_init()
00025 {
00026     wait_ms(100);
00027     clock_init();
00028     uart_sync.baud(115200);
00029     uart_sync.attach(&uart_recv, Serial::RxIrq);
00030     
00031     qfront = 0;
00032     qtail = 0;
00033     trigf = NULL;
00034 }
00035 
00036 void clock_init()
00037 {
00038     // select PCLK
00039     LPC_SC->PCLKSEL1 = (LPC_SC->PCLKSEL1 & ~(3<<CLK_TIMER)) | (PCLK_DEVIDER<<CLK_TIMER);
00040     // power on timer
00041     LPC_SC->PCONP |= (1<<PCTIM2);
00042     
00043     // reset timer
00044     TMR->TCR = 2;
00045     TMR->CTCR = 0;
00046     TMR->PR = 0;
00047     TMR->TC = 1;
00048     TMR->MR0 = SystemCoreClock - 1;
00049     TMR->MCR = 3;
00050     
00051     clock_sec = 0;
00052 
00053     // capture on CAP.0        
00054     TMR->CCR = 0x07;
00055     LPC_PINCON->PINSEL0 = LPC_PINCON->PINSEL0  | (3<<8);
00056     
00057     // setup interrupt
00058     __enable_irq();
00059     NVIC_SetVector(TIMER2_IRQn, (uint32_t)&tmr1_irq);
00060     NVIC_EnableIRQ(TIMER2_IRQn);
00061     
00062     NVIC_SetPriority(TIMER2_IRQn, 0);
00063     NVIC_SetPriority(UART2_IRQn, 1);
00064     NVIC_SetPriority(TIMER3_IRQn, 2);
00065     NVIC_SetPriority(UART0_IRQn, 2);
00066     NVIC_SetPriority(UART3_IRQn, 1);
00067 
00068     // begin freerun
00069     TMR->TCR = 1;
00070 }
00071 
00072 void tmr1_irq()
00073 {
00074     if (TMR->IR & (1<<4)) {
00075         if (trigf) {
00076             trigt.tv_sec = clock_sec;
00077             if ((TMR->IR & 0x01) && TMR->CR0 < 256)
00078                 trigt.tv_sec = clock_sec+1;
00079             trigt.ticks = TMR->CR0;
00080             //runTriggerFunc();
00081             tmr_callback.attach_us(&runTriggerFunc, 10);
00082         }
00083         TMR->IR = (1<<4);
00084     }
00085     if (TMR->IR & 0x1) {
00086         // Overflow!
00087         clock_sec ++;
00088         TMR->IR = 0x1;
00089     }
00090     if (TMR->IR & 0x2) {
00091         if (clock_sec == timeq[qfront].tv_sec) {
00092             uint32_t nowtick = timeq[qfront].ticks;
00093             funcq[qfront]();
00094             qfront = (qfront+1)%QUEUE_SIZE;
00095             while(qfront != qtail 
00096             && clock_sec == timeq[qfront].tv_sec
00097             && nowtick == timeq[qfront].ticks) {
00098                 funcq[qfront]();
00099                 qfront = (qfront+1)%QUEUE_SIZE;
00100             }
00101             if (qfront == qtail) {
00102                 TMR->MCR &= ~(1<<3);
00103             } else {
00104                 TMR->MR1 = timeq[qfront].ticks;
00105             }
00106         } else if (clock_sec > timeq[qfront].tv_sec) {
00107             // we missed it!
00108             qfront = (qfront+1)%QUEUE_SIZE;
00109             if (qfront == qtail) {
00110                 TMR->MCR &= ~(1<<3);
00111             } else {
00112                 TMR->MR1 = timeq[qfront].ticks;
00113             }
00114         }
00115         TMR->IR = 0x2;
00116     }
00117 }
00118 
00119 void runTriggerFunc()
00120 {
00121     timeval_t t;
00122     hdtv_totv(&t, &trigt);
00123     trigf(&t);
00124 }
00125 
00126 void uart_recv()
00127 {
00128     hdtimeval_t tstamp;
00129     getHDTime(&tstamp);
00130 
00131     //if (!(LPC_UART2->IIR & 0x01) && (LPC_UART2->LSR & 0x01)) {
00132     if (uart_sync.readable()) {
00133         sync_pkt64_t pkt64;
00134         sync_pkt32_t pkt32;
00135         sync_pkt24_t pkt24;
00136         char * data = NULL;
00137         uint8_t len = 0;
00138         char cmd;
00139         
00140         cmd = uart_sync.getc();
00141         //cmd = LPC_UART2->RBR;
00142         //printf("byte: %x \r\n", cmd);
00143         switch (cmd) {
00144             case SYNCTYPE_64: 
00145                 pkt64.rx_time = tstamp;
00146                 pkt64.tx_time = TMR->TC;
00147                 uart_sync.putc(pkt64.raw[0]);
00148                 len = 9;
00149                 data = pkt64.raw + 1;
00150                 break;
00151             case SYNCTYPE_32: 
00152                 pkt32.rx_time = tstamp.ticks;
00153                 pkt32.tx_time = TMR->TC; 
00154                 uart_sync.putc(pkt32.raw[0]);
00155                 len = 5;
00156                 data = pkt32.raw + 1;
00157                 break;
00158             case SYNCTYPE_24: 
00159                 pkt24.rx_time = tstamp.ticks << 8;
00160                 pkt24.tx_time = TMR->TC; 
00161                 uart_sync.putc(pkt24.raw[0]);
00162                 len = 4;
00163                 data = pkt24.raw + 1;
00164                 break;
00165         }
00166         while (len-->0) {
00167             uart_sync.putc(*(data++));
00168         }
00169         //printf("rx_tick: %d,%d,%d \r\n", tstamp.tv_sec, tstamp.ticks>>24, tstamp.ticks & 0xFFFFFF);
00170     }
00171     led = !led;
00172 }
00173 
00174 __INLINE void getHDTime(struct hdtimeval * hdtv)
00175 {
00176     __disable_irq();
00177     hdtv->tv_sec = clock_sec;
00178     hdtv->ticks = TMR->TC;
00179     __enable_irq();
00180     return;
00181 }
00182 
00183 void getTime(struct timeval * tv)
00184 {
00185     hdtimeval_t tstamp; 
00186 
00187     getHDTime(&tstamp);
00188     hdtv_totv(tv, &tstamp);
00189     return; 
00190 }
00191 
00192 int enqueue(void (*schedFunc)(void), hdtimeval_t *tv)
00193 {
00194     uint8_t p = qtail;
00195     if ((qtail+1)%QUEUE_SIZE == qfront) {
00196         return 1;
00197     }
00198     while(p != qfront) {
00199         uint8_t prev = (p+QUEUE_SIZE-1)%QUEUE_SIZE;
00200         if (tv->tv_sec > timeq[prev].tv_sec 
00201         || (tv->tv_sec == timeq[prev].tv_sec && tv->ticks >= timeq[prev].ticks)) break;
00202         timeq[p] = timeq[prev];
00203         funcq[p] = funcq[prev];
00204         p = prev;
00205     } 
00206     timeq[p] = *tv;
00207     funcq[p] = schedFunc;
00208     qtail = (qtail+1)%QUEUE_SIZE;
00209     return 0;
00210 }
00211 
00212 int runAtTime(void (*schedFunc)(void), struct timeval *tv)
00213 {
00214     hdtimeval_t hdtv;
00215     int ret;
00216     hdtv.tv_sec = tv->tv_sec;
00217     hdtv.ticks = tv->tv_usec * 96;
00218     ret = enqueue(schedFunc, &hdtv);
00219     if (qtail != qfront) {
00220         TMR->MR1 = timeq[qfront].ticks;
00221         TMR->MCR |= (1<<3);
00222     }
00223     return ret;
00224 }
00225 
00226 void runAtTrigger(void (*trigFunc)(struct timeval *tv))
00227 {
00228     trigf = trigFunc;
00229 }