X-Git-Url: http://pileus.org/git/?a=blobdiff_plain;ds=sidebyside;f=hw2%2Fmain.cpp;h=82f28f75a3d4e62bd722cd69459bab3120969cb1;hb=fcf3a5999ccfbb4a88f747ee3d9ed1c9c81891de;hp=4f42dd26110b517d368c74bc9754b04b519a07ae;hpb=29fe7a5ab17772fba51cb83c42e2b1961a767d59;p=~andy%2Fcsm213a-hw diff --git a/hw2/main.cpp b/hw2/main.cpp index 4f42dd2..82f28f7 100644 --- a/hw2/main.cpp +++ b/hw2/main.cpp @@ -1,4 +1,7 @@ +#include "messages.h" + #include "mbed.h" +#include "serial_irq.h" #include "serial_dma.h" #include "timer_dma.h" @@ -17,53 +20,36 @@ * Time synchronization is performed in both directions. */ -/*********************** - * Message Definitions * - ***********************/ - -#define MSG_HEADER 0x1234 - -typedef enum { - MSG_ID_SYNC, // Time synchronization - MSG_ID_EVENT, // Event occurred -} msgid_t; - -typedef struct { - uint32_t seconds; // Seconds since 1970 (without leap seconds) - uint32_t nanosec; // Nanoseconds since 'seconds' -} ntime_t; - -typedef struct { - uint16_t header; // Message Header - uint16_t mesgid; // Message ID - uint16_t length; // Body length - uint16_t cksum; // Body checksum -} header_t; - -typedef struct { - uint16_t seq; // Current sequence counter - uint16_t prev; // Sequence of previous message - ntime_t time; // Time of previous message -} sync_msg_t; - -typedef struct { - uint16_t device; // Device ID - uint16_t event; // Event ID - ntime_t time; // Timestamp -} event_msg_t; - /******************* * Timer functions * *******************/ +#define NSEC_PER_SEC 1000000000ULL + +uint64_t time_last_local; // timestamp at last time sync +uint64_t time_last_world; // offset at last time sync + +/** + * Generate time stamp for an async event: + * local: drift compensated wall-clock time + * world: nanoseconds in world time world + * valid: local timestamp at valid valid + */ +//uint64_t time_to_local(uint64_t world, uint64_t valid) +//{ +// uint64_t now = +// local = + (stamp); +//} + /** * Generate time stamp for an async event: * time: drift compensated wall-clock time - * stamp: event timestamp from Timer/PWM Module + * stamp: event timestamp from PIT Module */ -void time_stamp(ntime_t *time, uint32_t stamp) +uint64_t time_to_world(uint64_t local) { - // todo + uint64_t elapsed = local - time_last_local; + return time_last_world + elapsed; } /** @@ -79,23 +65,98 @@ void time_rtc_comp(void) /** * Synchronize the timer internal state with updates * from an external time sync message. - * ours: our internal timestamp for the event - * ref: reference timestamp from the other device + * local: our internal timestamp for the event + * world: reference timestamp from the other device */ -void time_ext_sync(ntime_t *ours, ntime_t *ref) +void time_ext_init(uint64_t local, uint64_t world) { - // todo + sirq_printf("initialize clocks: %d -> %d\r\n", + (int)(local/NSEC_PER_SEC), + (int)(world/NSEC_PER_SEC)); + + time_last_local = local; + time_last_world = world; +} + +/** + * Synchronize the timer internal state with updates + * from an external time sync message. + * local: our internal timestamp for the event + * world: reference timestamp from the other device + */ +void time_ext_sync(uint64_t local, uint64_t world) +{ + sirq_printf("syncing clocks: %d -> %d\r\n", + (int)(local/NSEC_PER_SEC), + (int)(world/NSEC_PER_SEC)); + + time_last_local = local; + time_last_world = world; } /************************ * Serial I/O functions * ************************/ +typedef struct { + int index; + int state; + uint8_t buffer[256]; +} parser_t; + +const uint64_t serial_sync_delay = NSEC_PER_SEC * 2; // 1hz +static uint64_t serial_sync_due = 0; + +static tdma_t *serial_tdma_rcv = NULL; +static tdma_t *serial_tdma_xmt = NULL; + +static uint64_t serial_prev_local = 0; +static uint64_t serial_prev_seq = 0; + +static uint64_t serial_xmt_local = 0; +static uint64_t serial_xmt_seq = 0; + /** * Output time sync message */ -void serial_send_sync(void) +void serial_send_sync(sirq_t *port, uint64_t now) { + if (serial_sync_due == 0 || now < serial_sync_due) + return; // not ready + + // Calculate world time + uint64_t world = time_to_world(serial_xmt_local); + + // Message data + header_t head; + sync_msg_t body; + + // Transmit sync message + head.header = MSG_HEADER; + head.msgid = MSG_ID_SYNC; + head.length = sizeof(body); + head.cksum = 0; // todo + + body.seq = serial_xmt_seq; + body.time.seconds = world / NSEC_PER_SEC; + body.time.nanosec = world % NSEC_PER_SEC; + + sirq_write(port, &head, sizeof(head)); + sirq_write(port, &body, sizeof(body)); + + serial_xmt_seq += 1; + serial_sync_due = 0; + + // Debug + sirq_printf("sync msg transmit\r\n"); + + // save transmit time + for (int i = 0; i < 1000; i++) + asm("nop"); + int valid = tdma_stamp(serial_tdma_xmt, &serial_xmt_local); + if (!valid) + sirq_printf("missing sync transmit time\r\n"); + tdma_reset(serial_tdma_xmt); } /** @@ -103,15 +164,124 @@ void serial_send_sync(void) * event: id of the received event * time: compensated timestamp of the event */ -void serial_send_event(uint16_t event, ntime_t *time) +void serial_send_event(uint16_t event, uint64_t local) { + uint64_t world = time_to_world(local); + + ntime_t time = {}; + time.seconds = (uint32_t)(world / NSEC_PER_SEC); + time.nanosec = (uint32_t)(world % NSEC_PER_SEC); + + sirq_printf("event received - %08x:%08x - %u.%u\r\n", + (uint32_t)(local >> 32), (uint32_t)local, + time.seconds, time.nanosec); + // todo +} + +/** + * Handle sync message + */ +void serial_handle_sync(sync_msg_t *msg) +{ + // Read receive timestamp for next time sync message + uint64_t current = 0; + int valid = tdma_stamp(serial_tdma_rcv, ¤t); + if (!valid) + sirq_printf("missing sync receive time\r\n"); + tdma_reset(serial_tdma_rcv); + + // Lookup times + uint64_t world = ((uint64_t)msg->time.seconds) * NSEC_PER_SEC + + ((uint64_t)msg->time.nanosec); + + // Initialize + if (msg->seq == 0) { + uint64_t local = tdma_time(); + time_ext_init(local, world); + } + + // Valid times timestamp + if (serial_prev_seq == (msg->seq-1)) { + uint64_t local = serial_prev_local; + time_ext_sync(local, world); + } + + // Queue transmit to other board + serial_sync_due = tdma_time() + serial_sync_delay; + + // Update states + serial_prev_local = current; + serial_prev_seq = msg->seq; +} + +/** + * Handle event message + */ +void serial_handle_event(event_msg_t *msg) +{ +} + +/** + * Deliver message + */ +void serial_deliver(int msgid, void *body) +{ + switch (msgid) { + case MSG_ID_SYNC: + sirq_printf("received sync msg\r\n"); + serial_handle_sync((sync_msg_t*)body); + break; + case MSG_ID_EVENT: + sirq_printf("received event msg\r\n"); + serial_handle_event((event_msg_t*)body); + break; + } } /** * Process serial receive messages */ -void serial_receive(void) +void serial_receive(parser_t *parser, int byte) { + //sirq_printf("serial_receive - %02x\r\n", byte); + + // Lookup pointers + header_t *head = (header_t*)parser->buffer; + void *body = (void*)(head+1); + const int max_length = sizeof(parser->buffer)-sizeof(header_t); + + // Process uart messages + parser->buffer[parser->index++] = byte; + switch (parser->state) { + case 0: // Search + if (parser->index == sizeof(uint16_t)) { + if (head->header == MSG_HEADER) { + parser->state = 1; + } else { + parser->buffer[0] = parser->buffer[1]; + parser->index = 1; + } + } + break; + case 1: // Header + if (parser->index == sizeof(header_t)) { + if (head->length <= max_length && + head->msgid <= MSG_MAXID) { + parser->state = 2; + } else { + parser->index = 0; + parser->state = 0; + } + } + break; + case 2: // Data + if (parser->index == (int)sizeof(header_t)+head->length) { + serial_deliver(head->msgid, body); + parser->index = 0; + parser->state = 0; + } + break; + } } /******************** @@ -122,35 +292,152 @@ void serial_receive(void) DigitalOut led1(LED1); DigitalOut led2(LED2); -// UARTs tx rx -Serial uart0(USBTX, USBRX); -Serial uart1(PTE0, PTE1); -Serial uart2(PTD3, PTD2); +// Message Parsers +parser_t parser_dbg; +parser_t parser_bbb; +parser_t parser_mbed; -// Serial DMA -sdma_t *sdma0; -sdma_t *sdma1; -sdma_t *sdma2; +// Serial IRQ +sirq_t *sirq_dbg; +sirq_t *sirq_bbb; +sirq_t *sirq_mbed; // Timer DMA -tdma_t *tdma0; -tdma_t *tdma1; -tdma_t *tdma2; -tdma_t *tdma3; +tdma_t *tdma_evt; +tdma_t *tdma_rcv; +tdma_t *tdma_xmt; + +/********* + * Tasks * + *********/ + +void task_serial(uint64_t local, uint64_t world) +{ + while (sirq_ready(sirq_dbg)) { + //sirq_printf("serial recv - dbg\r\n"); + serial_receive(&parser_dbg, sirq_getc(sirq_dbg)); + } + + while (sirq_ready(sirq_bbb)) { + //sirq_printf("serial recv - bbb\r\n"); + serial_receive(&parser_bbb, sirq_getc(sirq_bbb)); + } + + while (sirq_ready(sirq_mbed)) { + //sirq_printf("serial recv - mbed\r\n"); + serial_receive(&parser_mbed, sirq_getc(sirq_mbed)); + } +} + +void task_events(uint64_t local, uint64_t world) +{ + uint64_t event = 0; + +#ifdef VERBOSE + if (tdma_stamp(tdma_evt, &event)) { + sirq_printf("event received - evt\r\n"); + if (tdma_stamp(tdma_rcv, &event)) + sirq_printf("event received - rcv\r\n"); + if (tdma_stamp(tdma_xmt, &event)) + sirq_printf("event received - xmt\r\n"); +#endif + + if (tdma_stamp(tdma_evt, &event)) + serial_send_event(0, event); + tdma_reset(tdma_evt); +} + +void task_sync(uint64_t local, uint64_t world) +{ + serial_send_sync(sirq_bbb, local); +} + +void task_leds(uint64_t local, uint64_t world) +{ + static uint32_t which = 0; + led1 = (which == 0); + led2 = (which == 1); + which ^= 1; +} + +void task_debug(uint64_t local, uint64_t world) +{ + //tdma_debug(tdma_rcv); + sirq_printf("background - %6d.%02d -> %d.%02d\r\n", + (uint32_t)(local / NSEC_PER_SEC), + (uint32_t)(local % NSEC_PER_SEC / 10000000), + (uint32_t)(world / NSEC_PER_SEC), + (uint32_t)(world % NSEC_PER_SEC / 10000000)); +} /******** * Main * ********/ -void test_main(void); +#define N_ELEM(x) (sizeof(x) / sizeof((x)[0])) + +extern void test_main(void); +extern serial_t stdio_uart; + +static struct { + void (*task)(uint64_t, uint64_t); + uint64_t period; + uint64_t due; +} tasks[] = { + { task_serial, 0 }, // always + { task_events, 1000000000 }, // always + { task_sync, 0 }, // always + { task_leds, 100000000 }, // 10hz + { task_debug, 1000000000 }, // 1hz +}; + +void background(void) +{ + // Debugging + uint64_t local = tdma_time(); + uint64_t world = time_to_world(local); + + // Run the scheduler + for (unsigned i = 0; i < N_ELEM(tasks); i++) { + if (local >= tasks[i].due) { + tasks[i].task(local, world); + tasks[i].due += tasks[i].period; + } + } +} int main(int argc, char **argv) { - uart0.baud(115200); - uart1.baud(115200); - uart2.baud(115200); + tdma_init(); + + // Open serial ports + sirq_dbg = sirq_open(SIRQ_UART0, USBTX, USBRX, 115200); // to pc + sirq_bbb = sirq_open(SIRQ_UART1, PTD3, PTD2, 115200); // to bbb + sirq_mbed = sirq_open(SIRQ_UART2, PTE0, PTE1, 115200); // to mbed + + + // Setup timers + tdma_evt = tdma_open(TDMA_CHAN0, 3, PTC9, PullDown); // async event + + // mbed time sync + tdma_rcv = tdma_open(TDMA_CHAN2, 3, PTD2, PullUp); // time sync rcv + tdma_xmt = tdma_open(TDMA_CHAN3, 3, PTD3, PullUp); // time sync xmt + + // host time sync + //tdma_rcv = tdma_open(TDMA_CHAN2, 2, USBRX, PullUp); // time sync rcv + //tdma_xmt = tdma_open(TDMA_CHAN3, 2, USBTX, PullUp); // time sync xmt + + // Serial timestamping + serial_tdma_rcv = tdma_rcv; + serial_tdma_xmt = tdma_xmt; + + // Run background loop + printf("hello"); + while (true) + background(); - test_main(); + // Run tests + //test_main(); return 0; }