- int index;
- int state;
- uint8_t buffer[256];
-} parser_t;
-
-static uint32_t serial_device_id = 0;
-
-const uint64_t serial_sync_delay = NSEC_PER_SEC / 100; // 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;
-
-/**
- * Convert world to local time
- */
-uint64_t serial_read_time(ntime_t time)
-{
- return ((uint64_t)time.seconds) * NSEC_PER_SEC
- + ((uint64_t)time.nanosec);
-}
-
-ntime_t serial_write_time(uint64_t time)
-{
- ntime_t buf = {};
- buf.seconds = time / NSEC_PER_SEC;
- buf.nanosec = time % NSEC_PER_SEC;
- return buf;
-}
-
-/**
- * Output initialization message init message
- */
-void serial_send_init(uint16_t device, uint64_t local)
-{
-}
-
-/**
- * Output time sync message
- */
-void serial_send_sync(sirq_t *port, uint64_t now)
-{
- if (serial_sync_due == 0 || now < serial_sync_due)
- return; // not ready
-
- //sirq_printf("sending sync\r\n");
-
- // Calculate world time
- uint64_t local = 0;
- 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;
-
- tdma_stop(serial_tdma_rcv);
-
- test_xmt_enab = 1;
- test_xmt_time0 = 0;
- test_xmt_time1 = 0;
-
- tdma_start(serial_tdma_xmt);
- sirq_write(port, &head, sizeof(head));
- sirq_write(port, &body, sizeof(body));
- tdma_stop(serial_tdma_xmt);
-
- // save transmit time
- //local = test_xmt_time1;
- int valid = tdma_stamp(serial_tdma_xmt, &local);
- if (!valid) {
- sirq_printf("sync transmit time -- missed\r\n");
- } else {
- //time_printf("sync transmit time ", local);
- //time_printf("sync transmit test0", test_xmt_time0);
- //time_printf("sync transmit test1", test_xmt_time1);
- }
-
- tdma_start(serial_tdma_rcv);
-
- serial_xmt_seq += 1;
- serial_sync_due = 0;
- serial_xmt_local = local;
-}
-
-/**
- * Output external event received message
- * event: id of the received event
- * time: compensated timestamp of the event
- */
-void serial_send_event(uint16_t event, uint64_t local)
-{
-// uint64_t world = time_to_world(local);
-
- // Message data
-#if 0
- header_t head;
- event_msg_t body;
-
- 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.%09u\r\n",
- (uint32_t)(local >> 32), (uint32_t)local,
- time.seconds, time.nanosec);
-
- // 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;
-
- tdma_stop(serial_tdma_rcv);
-
- test_xmt_enab = 1;
- test_xmt_time0 = 0;
- test_xmt_time1 = 0;
-
- tdma_start(serial_tdma_xmt);
- sirq_write(port, &head, sizeof(head));
- sirq_write(port, &body, sizeof(body));
- tdma_stop(serial_tdma_xmt);
-#endif
-}
-
-/**
- * Handle init message
- */
-void serial_handle_init(init_msg_t *msg)
-{
- if (msg->valid & MSG_VALID_DEVICE)
- serial_device_id = msg->device;
-
- if ((msg->valid & MSG_VALID_START) ||
- (msg->valid & MSG_VALID_PERIOD)) {
- uint64_t start = serial_read_time(msg->start);
- uint64_t period = serial_read_time(msg->period);
- emit_enable(start, period);
- }
-
- if (msg->valid & MSG_VALID_WORLD) {
- uint64_t world = serial_read_time(msg->world);
- uint64_t local = tdma_time();
- time_ext_init(local, world);
- }
-}
-
-/**
- * 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("sync receive time -- missing\r\n");
- //else
- // time_printf("sync receive time ", current);
- tdma_stop(serial_tdma_rcv);
-
- // Lookup times
- uint64_t world = ((uint64_t)msg->time.seconds) * NSEC_PER_SEC
- + ((uint64_t)msg->time.nanosec);
-
- // 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_INIT:
- //sirq_printf("received init msg\r\n");
- serial_handle_init((init_msg_t*)body);
- break;
- 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(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_MAX_ID) {
- 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;
- }
-}